ekf.m

利用扩展卡尔曼进行状态估计 状态向量四维 测量向量二维 误差为高斯白噪声

T=1;
okk_1=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];
 
a1=sqrt(1000*1000+1000*1000);
 a2=sqrt(1030*1030+1040*1040);
 b1=atan(1000/1000);
 b2=atan(1040/1030);
 B=[0 0 cos(b2) -a2*sin(b2);-cos(b1)/T a1*sin(b1)/T cos(b2)/T -a2*sin(b2)/T;0 0 sin(b2) a2*cos(b2);-sin(b1)/T -a1*cos(b1)/T sin(b2)/T a2*cos(b2)/T];
 R1=[0.04 0 0 0;0 0.0001 0 0;0 0 0.04 0;0 0 0 0.0001];
 Pk_1=B*R1*B';
 
 %Pk=[0.04 0.04/T 0 0;0.04/T 4+0.08/(T*T) 0 0;0 0 0.000001 0.000001/T;0 0 0.000001/T 4+0.000001/(T*T)]; 
%Pk=P2; 
 Qk=[1 2 0 0;2 4 0 0;0 0 1 2;0 0 2 4];
 %Qk=[0 0 0 0;0 4 0 0;0 0 0 0;0 0 0 4];
 %Qk=[0 0;1 0;0 0;0 1]*[1 0;0 4]*[0 0;1 0;0 0;0 1]';
 Rk=[0.04 0;0 0.0001];
 I=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
 %x=1060;
 %y=1080;
 X1=[1030;30;1040;40];
 X2=[1030;30;1040;40];
 Xk_1=[1030;30;1040;40];
for i=1:20
     %理想轨迹
Xlx=okk_1*X2;
X2=Xlx;
e(i)=Xlx(1,1);
f(i)=Xlx(3,1);
     %真实轨迹
Xs=okk_1*X1+[0;2*randn(1);0;2*randn(1)];
x=Xs(1,1);
y=Xs(3,1);
X1=Xs;
c(i)=Xs(1,1);
d(i)=Xs(3,1);

 Xkk_1=okk_1*Xk_1;%[0 0;1 0;0 0;0 1]*[randn(1);randn(1)] %一步预测
Hk=[Xkk_1(1,1)/sqrt(Xkk_1(1,1)*Xkk_1(1,1)+Xkk_1(3,1)*Xkk_1(3,1)) 0 Xkk_1(3,1)/sqrt(Xkk_1(1,1)*Xkk_1(1,1)+Xkk_1(3,1)*Xkk_1(3,1)) 0;
     -Xkk_1(3,1)/(Xkk_1(1,1)*Xkk_1(1,1)+Xkk_1(3,1)*Xkk_1(3,1)) 0 Xkk_1(1,1)/(Xkk_1(1,1)*Xkk_1(1,1)+Xkk_1(3,1)*Xkk_1(3,1)) 0];
 Pkk_1=okk_1*Pk_1*okk_1'+Qk;
Sk=Hk*Pkk_1*Hk'+Rk;
Kk=Pkk_1*Hk'*inv(Sk);
Pk=(I-Kk*Hk)*Pkk_1;
Pk_1=Pk;
%测量轨迹
Zk=[sqrt(x*x+y*y);atan(y/x)]+[0.2*randn(1);0.01*randn(1)];%
g(i)=Zk(1,1)*cos(Zk(2,1));
j(i)=Zk(1,1)*sin(Zk(2,1));
aa=g(i);
bb=j(i);
%估计轨迹
Zkk_1=Hk*Xkk_1;
Xk=Xkk_1+Kk*(Zk-Zkk_1);
Xk_1=Xk;
%Xk_1=[Xk(1,1);Xk(2,1);Xs(3,1);Xs(4,1)];
a(i)=Xk(1,1);
b(i)=Xk(3,1);

end



i=1:20;
subplot(2,2,1);plot(e(i),f(i));title('理想轨迹');%axis([0,20,19.9999,20.0001]);
subplot(2,2,2);plot(c(i),d(i));title('真实轨迹');%axis([-1,20,19,21]);
subplot(2,2,3);plot(a(i),b(i));title('估计轨迹');%axis([-1,20,19.9999,20.0001]);%
subplot(2,2,4);plot(g(i),j(i));title('测量轨迹');
%