ekf.asv

EKF程序,用于计算目标跟踪的问题.运行情况很好,很适合非线性滤波问题.

function fdekf
[B,F,G,X,Xt]=track1;
 P=[0.005 0 0 0;0 0.005 0 0;0 0 0.005 0;0 0 0 0.005];
 X1(:,1)=[200 1.3 1500 -0.3];
for i=1:999    
        X1(:,i+1)=F*X1(:,i);
        H=[-X1(3,i+1)/(X1(3,i+1)^2+X1(1,i+1)^2) 0 X1(1,i+1)/(X1(3,i+1)^2+X1(1,i+1)^2) 0];
        P=F*P*F'+G*0.005*G';
        S=0.008+H*P*H';
        BB(i+1)=B(i+1)-atan(X1(3,i+1)/X1(1,i+1));
        K=P*H'/S;
        X1(:,i+1)=X1(:,i+1)+K*BB(i+1);
        P=P-K*S*K';
end
plot(1:1000,X1(1,1:1000)-X(1,1:1000))