kalman滤波程序【matlab】

clear,clc
format long
N=1000;
detT=2*pi/N;
V=100;
R=100;
A=V^2/R;
RR=1e-0;
for i=1:N
    ax(i,1)=A*cos(pi/2+i*detT);
    ay(i,1)=A*sin(pi/2+i*detT*3);
end
a=[ax ax ay ay];
X=[0 V 0 0];
F=[1 detT 0 0;
   0 1 0 0;
   0 0 1 detT;
   0 0 0 1];
B=[(detT^2)/2 detT (detT^2)/2 detT];
XX(1,1:4)=X;
for i=2:N
    XX(i,1:4)=[F*XX(i-1,:)']'+B.*a(i,:);
end
Z=XX(:,[1 3])+randn(N,2)*RR;
Z1=XX(:,[1 3])+randn(N,2)*RR;

Ykf=zeros(N,4);  
Ykf(1,:)=X;
Q=1e-3;
P=eye(4);  %协方差矩阵初始化，4*4单位矩阵
R=RR*eye(2);
H=[ 1,0,0,0;
    0,0,1,0];
a=[ax ax ay ay]+randn(N,4)*RR;
for i=2:N  
    X_pre=Ykf(i-1,:)*F'+B.*a(i,:);  
    P_pre=F*P*F'+Q;  
    K=P_pre*H'/(H*P_pre*H'+R);  
    Ykf(i,:)=X_pre+((Z(i,:))-X_pre*H')*K';  
    P=(eye(4)-K*H)*P_pre;  
end 
plot(XX(:,1),XX(:,3),'-.')
hold on
plot(Z(:,1),Z(:,2),'ro-')
%plot(Z1(:,1),Z1(:,2),'g^-')
plot(Ykf(:,1),Ykf(:,3),'k.-')
legend('真实轨迹','观测1轨迹','滤波轨迹') 
axis equal
hold off