a4youma
2007-06-11, 11:24
一个关于卡尔曼递推的小程序，大家看看有用没有？
%卡尔曼滤波递推程序 ;
clear
Ak=exp(-0.02); %各系数由前面确定；
Ck=1;
Qk=1-exp(-0.04);
Rk=1;
p(1)=1; %各初值；
p1(1)=Ak*p(1)*Ak'+Qk; %由p1代表p'；
x(1)=0; %设信号初值为0；
H(1)=p1(1)*Ck'*inv(Ck*p1(1)*Ck'+Rk);
zk=[-3.2,-0.8,-14,-16,-17,-18,-3.3,-2.4,-18,-0.3,-0.4,-0.8,-19,-2.0,-1.2,-11,-14,-0.9,0.8,10,0.2,0.5,-0.5,2.4,-0.5,0.5,-13,0.5,10,-12,0.5,-0.6,-15,-0.7,15,0.5,-0.7,-2.0,-19,-17,-11,-14]
%zk为测量出来的离散值；
N=length(zk); %要测量的点数 ；
for k=2:N
p1(k)=Ak*p(k-1)*Ak'+Qk; %未考虑噪声时的均方误差阵；
H(k)=p1(k)*Ck'*inv(Ck*p1(k)*Ck'+Rk); %增益方程；
I=eye(size(H(k))); %产生和H（k）维数相同的单位矩阵；
p(k)=(I-H(k)*Ck)*p1(k); %滤波的均方误差阵；
x(k)=Ak*x(k-1)+H(k)*(zk(k)-Ck*Ak*x(k-1)); %递推公式；
end,x %显示信号x(k)的数据；
m=1:N;
n=m*0.02;
plot(n,zk,'r',n,x,'b'); %便于比较zk和x(k)在同一窗口输出；
legend('测量值zk','信号估计值x(k)',2);
grid;
%卡尔曼滤波递推程序 ;
clear
Ak=exp(-0.02); %各系数由前面确定；
Ck=1;
Qk=1-exp(-0.04);
Rk=1;
p(1)=1; %各初值；
p1(1)=Ak*p(1)*Ak'+Qk; %由p1代表p'；
x(1)=0; %设信号初值为0；
H(1)=p1(1)*Ck'*inv(Ck*p1(1)*Ck'+Rk);
zk=[-3.2,-0.8,-14,-16,-17,-18,-3.3,-2.4,-18,-0.3,-0.4,-0.8,-19,-2.0,-1.2,-11,-14,-0.9,0.8,10,0.2,0.5,-0.5,2.4,-0.5,0.5,-13,0.5,10,-12,0.5,-0.6,-15,-0.7,15,0.5,-0.7,-2.0,-19,-17,-11,-14]
%zk为测量出来的离散值；
N=length(zk); %要测量的点数 ；
for k=2:N
p1(k)=Ak*p(k-1)*Ak'+Qk; %未考虑噪声时的均方误差阵；
H(k)=p1(k)*Ck'*inv(Ck*p1(k)*Ck'+Rk); %增益方程；
I=eye(size(H(k))); %产生和H（k）维数相同的单位矩阵；
p(k)=(I-H(k)*Ck)*p1(k); %滤波的均方误差阵；
x(k)=Ak*x(k-1)+H(k)*(zk(k)-Ck*Ak*x(k-1)); %递推公式；
end,x %显示信号x(k)的数据；
m=1:N;
n=m*0.02;
plot(n,zk,'r',n,x,'b'); %便于比较zk和x(k)在同一窗口输出；
legend('测量值zk','信号估计值x(k)',2);
grid;