circlepart.m

一个kalman滤波的仿真程序

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

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