dckalman.m

喜欢的给点好评哦 发个卡尔曼滤波递推程序

%卡尔曼滤波递推程序 ;
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;