kal.m

Kalman滤波器

function [xx,gg]=kal(sigma1,F,Q1,C,Q2,y,N,M)
xx=zeros(N,1);
x0=zeros(M,1);
k0=zeros(M,M);
k0(M,M)=sigma1;
for n=1:1:N
    R=C*k0*C'+Q2;
    G=F*k0*C'*inv(R);
    alpha=y(n)-C*x0;
    x1=F*x0+G*alpha;
    k1=k0-F*G*C*k0;
    k2=F*k1*F'+Q1;
    k0=k2;
    x0=x1;
    x2=inv(F)*x1;
    xx(n)=x2(M);
    gg(:,n)=G;
   
end