www.gusucode.com > EKF程序,用于计算目标跟踪的问题.运行情况很好,很适合非线性滤波问题 > code9/EKF1/track1.m

    function [B,F,G,X,Xt]=track1
Xt(:,1)=[220,1,1550,-0.5];Xo(:,1)=[-800,1.2,-3000,0.5];T=3;F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];
G=[T^2/2 0;0 T;T^2/2 0;0 T];
V=0.005*randn(2,1000);W=0.008*randn(1,1000);
X(:,1)=Xt(:,1);B(1)=atan(X(3,1)/X(1,1));
for it=2:1000
    Xt(:,it)=F*Xt(:,it-1)+G*V(:,it);
    X(:,it)=F*X(:,it-1);
    B(it)=atan(X(3,it)/X(1,it))+W(it);%用真实值得到测量值，在滤波时结果才会与真实值重合。
end