track.m

本程序是用matlab编写的UKF的滤波程序,并且运行通过.

function [B,F,G,X,Xt]=track
Xt(:,1)=[220,1,1550,-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