linepart.m

一个kalman滤波的仿真程序

function endstate =linepart(state,Nline,Xn)
%state为初始工作状态矩阵,4*矩阵.1.X0;2.Y0;3.速度X;4.速度Y.
%Nline为模型运行的点数.
%endstate为模型运行完成后的状态.4*Nline矩阵.1.Xn;2.Yn;3.速度X;4.速度Y.
%Xn为实际观测的数据.

%初始工作状态
firststate=state;
Cs=[10000,0,0,0;0,10000,0,0;0,0,0,0;0,0,0,0];
%模型参数
Cn=[10000,0;0,10000];
Hn=[1,0,0,0;0,1,0,0];
T=2;
A=[1,0,T,0;0,1,0,T;0,0,1,0;0,0,0,1];
p=4;%观测矢量个数
Kn=zeros(4,2,Nline);
Mnn=zeros(4,4,Nline);
Snn=zeros(4,Nline);
Stemp=zeros(4,1);
Mtemp=zeros(4,4);
%数据迭代过程
Stemp=A*firststate;
Mtemp=A*Cs*A';
for i=1:Nline;
    Kn(:,:,i)=Mtemp*Hn'*inv(Cn+Hn*Mtemp*Hn');
    Snn(:,i)=Stemp+Kn(:,:,i)*(Xn(:,i)-Hn*Stemp);
    Mnn(:,:,i)=(eye(4)-Kn(:,:,i)*Hn)*Mtemp;
    Stemp=A*Snn(:,i);
    Mtemp=A*Mnn(:,:,i)*A';
end;
endstate=Snn;
%求滤波误差方差
% error=Snn(1,:)-XLreal;
plot(Snn(1,:),Snn(2,:),'b-');