卡尔曼滤波实验及matlab实现

实验一 卡尔曼滤波
一、 实验目的
1、了解卡尔曼滤波的准则和信号模型，以及卡尔曼滤波的应用。

2、熟练掌握卡尔曼滤波的递推过程，提高对信号进行处理的能力。

3、分析讨论实际状态值和估计值的误差。

二、实验原理
1、卡尔曼滤波简介
卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。

它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。

卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。

因此设计卡尔曼滤波器要求已知状态方程和测量方程。

它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。

2、卡尔曼滤波的递推公式
)
(11∧
-∧
-∧
-+=k k k k k k k k x A C y H x A x (1)
1)(-+''=k k k k k k k R C P C C P H τ
τ.........(2) 11--+='k k k k k Q A P A P τ (3)
k k k k P C H I P '-=)(………(4) 3、递推过程的实现
如果初始状态0x 的统计特性][0x E 及]var[0x 已知，并 令 0
00][μ==∧
x E x
又
]
var[]))([(000000x x x x x E P =--=∧
∧
τ
将0P 代入式（3）可求得1P '，将1P '代入式（2）可求得1H ，将此1H 代入式
（1）可求得在最小均方误差条件下的∧
1x ，同时将1P '代入式（4）又可求得1P
；由1P
又可求2P '，由2P '又可求得2H ，由2H 又可求得∧
2x ，同时由2H 与2P '又可求得2P ……；以此类推，这种递推计算方法用计算机计算十分方便。

三、实验器材
1、计算机一台
2、MATLAB 软件一套
四、实验内容
一个系统模型为 )
()()1(,1,0),()()()1(22211k w k x k x k k w k x k x k x +=+=++=+
同时有下列条件：
（1） 初始条件已知且有T x ]0,0[)0(=。

（2） )(k w 是一个标量零均值白高斯序列，且自相关函数已知为
jk k w j w E δ=)]()([。

另外，我们有下列观测模型，即 )
1()1()1(,1,0),1()1()1(222111+++=+=+++=+k v k x k y k k v k x k y
且有下列条件：
（3） )1(1+k v 和)1(2+k v 是独立的零均值白高斯序列，且有 ,2,1,0,2)]()([,
)]()([2211===k k v j v E k v j v E jk jk δδ
（4） 对于所有的j 和k ，)(k w 与观测噪声过程)1(1+k v 和)1(2+k v 是不相关的，
即
,2,1,0,,2,1,0,0)]()([,
0)]()([21====k j k v j w E k v j w E
我们希望得到由观测矢量)1(+k y ，即T k y k y k y )]1(),1([)1(21++=+估计状态矢量T k x k x k x )]1(),1([)1(21++=+的卡尔曼滤波器的公式表示形式，并求解以下问题：
（a ） 求出卡尔曼增益矩阵，并得出最优估计)1(+k x 和观测矢量
)1(),...,2(),1(+k y y y 之间的递归关系。

（b ） 通过一个标量框图（不是矢量框图）表示出状态矢量)1(+k x 中元素
)1(1+k x 和)1(2+k x 估计值的计算过程。

（c ） 用模拟数据确定状态矢量)(k x 的估计值,10,...,
1,0),(=∧
k k k x 并画出当k ＝
0，1，…，10时)(1k k x ∧和)(2k k x ∧
的图。

（d ） 通常，状态矢量的真实值是得不到得。

但为了用作图来说明问题，表P8.1
和P8.2给出来状态矢量元素得值。

对于k ＝0，1，…，10，在同一幅图
中画出真实值和在（c ）中确定的)(1k x 的估计值。

对)(2k x 重复这样过程。

当k 从1变到10时，对每一个元素i ＝1，2，计算并画出各自的误差图，即)()(k k x k x i i ∧
-。

（e ） 当k 从1变到10时，通过用卡尔曼滤波器的状态误差协方差矩阵画出
)]
([2
1k k E ε和
)]
([2
2k k E ε，而
)
()()(111k k x k x k k ∧
-=ε，
)()()(222k k x k x k k ∧
-=ε。

（f ） 讨论一下（d ）中你计算的误差与（e ）中方差之间的关系。

表P8.1 题8.1到题8.3中的观测值
时间下标k 观测值)(1k y )(2k y 观测值1 2345678910
3.296919693.387365157.028306419.7121252111.4201831515.9787058322.0693428528.3021278130.4468383138.75875595
2.101342940.47540797
3.176888982.498111402.919924246.173076165.425192743.053657415.98051141
4.51016361
表P8.2 题8.1到题8.3中的由模拟得到的实际状态值时间下标k
实际状态值)
(1k x 实际状态值)
(1k x 0123456789100.00000000001.654287143.503007025.9978529249.1504074012.5087391016.9219259421.3448335225.8933514431.5413533036.936056700.0000000001.654287141.848719882.475522223.171878163.358331704.413186844.422907584.548517925.648001865.394470340
五、实验结果分析
（a ）卡尔曼增益矩阵：k k k T T 1H P C (C P C R)-=⨯⨯⨯⨯+’
’ 估计值与观测值之间的递归关系为：k k-1k-1k k k k k X A X H (Y C A X )∧
∧
∧
=⨯+⨯-⨯⨯
（b ）状态矢量估计值的计算框图：
（c ）)(1k k x ∧和)(2k k x ∧
的图：
1ˆk x
+
（d ）真实值与估计值的比较图：
各自的误差图：
（e ）通过用卡尔曼滤波器的状态误差协方差矩阵画出的)]([2
1k k E ε和
)]([2
2k k E ε：
（f）分析：（e）中的方差是（d）中的误差平方后取均值，是均方误差。

误差直接由真实值减去估计值，有正有负，而均方误差没有这个缺陷，更能综合的表示滤波的效果。

附程序：
%卡尔曼滤波实验程序
clc;
y1=[3.29691969,3.38736515,7.02830641,9.71212521,11.42018315,15.97870583 ,22.06934285,28.30212781,30.44683831,38.75875595]; %观测值y1(k)
y2=[2.10134294,0.47540797,3.17688898,2.49811140,2.91992424,6.17307616,5.
42519274,3.05365741,5.98051141,4.51016361]; %观测值y2(k)
p0=[1,0;0,1];p=p0; %均方误差阵赋初值
Ak=[1,1;0,1]; %转移矩阵
Qk=[1,0;0,1]; %系统噪声矩阵
Ck=[1,0;0,1]; %量测矩阵
Rk=[1,0;0,2]; %测量噪声矩阵
x0=[0,0]';xk=x0; %状态矩阵赋初值
for k=1:10
Pk=Ak*p*Ak'+Qk; %滤波方程3
Hk=Pk*Ck'*inv(Ck*Pk*Ck'+Rk); %滤波方程2
yk=[y1(k);y2(k)]; %观测值
xk=Ak*xk+Hk*(yk-Ck*Ak*xk); %滤波方程1
x1(k)=xk(1);
x2(k)=xk(2); %记录估计值
p=(eye(2)-Hk*Ck)*Pk; %滤波方程4
pk(:,k)=[p(1,1),p(2,2)]'; %记录状态误差协方差矩阵
end
figure %画图表示状态矢量的估计值
subplot(2,1,1)
i=1:10;
plot(i,x1(i),'k')
h=legend('x1(k)的估计值')
set(h,'interpreter','none')
subplot(2,1,2)
i=1:10;
plot(i,x2(i),'k')
h=legend('x2(k)的估计值')
set(h,'interpreter','none')
X1=[0,1.65428714,3.50300702,5.997852924,9.15040740,12.50873910,16.9219 2594,21.34483352,25.89335144,31.54135330,36.93605670]; %由模拟得到的实际状态值X1(k)
X2=[0,1.65428714,1.84871988,2.47552222,3.17187816,3.35833170,4.4131868 4,4.42290758,4.54851792,5.64800186,5.394470340]; %由模拟得到的实际状态值X2(k)
figure %在同一幅图中画出状态矢量的估计值与真实值
subplot(2,1,1)
i=1:10;
plot(i,x1(i),'k',i,X1(i+1),'b')
h=legend('x1(k)的估计值','x1(k)的真实值')
set(h,'interpreter','none')
subplot(2,1,2)
i=1:10;
plot(i,x2(i),'k',i,X2(i+1),'b')
h=legend('x2(k)的估计值','x2(k)的真实值')
set(h,'interpreter','none')
for i=1:10 %计算x(k)的误差
e1(i)=X1(i+1)-x1(i);
e2(i)=X2(i+1)-x2(i);
end
figure %画出误差图
subplot(2,1,1)
i=1:10;
plot(i,e1(i),'r')
h=legend('x1(k)的误差')
set(h,'interpreter','none')
subplot(2,1,2)
i=1:10;
plot(i,e2(i),'r')
h=legend('x2(k)的误差')
set(h,'interpreter','none')
figure %通过用卡尔曼滤波器的状态误差协方差矩阵画出E[ε1(k/k)^2]和E[ε2(k/k)^2]
i=1:10;
subplot(2,1,1)
plot(i,pk(1,i),'r')
h= legend('由状态误差协方差矩阵得到的E[ε1(k/k)^2]')
set(h,'Interpreter','none')
subplot(2,1,2)
plot(i,pk(2,i),'r')
h= legend('由状态误差协方差矩阵得到的E[ε2(k/k)^2]')
set(h,'Interpreter','none')。