lmukf.m

ukf的程序

% 在二维平面对目标进行纯方位跟踪    UKF    应该是基于方位和距离的跟踪？      
clear all
close all
clc
N=50;  %观测次数
T=1;    %采样间隔 
F=[1 0 T 0;  
   0 1 0 T;
   0 0 1 0;
   0 0 0 1];    %状态转移矩阵
qv=0.01;
v1=normrnd(0,qv,[1,N]);   %目标在x速度方向上的噪声
v2=normrnd(0,qv,[1,N]);   %目标在y速度方向上的噪声
Q=[0 0 0     0    ;
   0 0 0     0    ;
   0 0 qv^2 0    ;
   0 0 0     qv^2];   %状态噪声协方差
X(:,1)=[250 1000 2 -2]';      %目标初始状态[x坐标 y坐标 x速度 y速度]'
for k=2:N                    
    X(:,k)=F*X(:,k-1)+[0 0 v1(k) v2(k)]';   %状态方程
end
w1=normrnd(0,1,[1,N]);     %观测噪声：距离
w2=normrnd(0,0.1,[1,N]);     %观测噪声：角度
R=[1 0; 
   0 0.01];   %噪声
%B=[0.5 0;0 0.5;1 0;0 1];
for k=1:N
    R(1,k)=sqrt(X(1,k)^2+X(2,k)^2)+w1(k);
    e(1,k)=atan(X(1,k)/X(2,k))+w2(k);
    Xx(1,k)=R(1,k)*cos(e(1,k));
    Xx(2,k)=R(1,k)*sin(e(1,k));
   Z(:,k)=[sqrt(X(1,k)^2+X(2,k)^2)+w1(k);atan(X(1,k)/X(2,k))+w2(k)];   %测量值
end
n=4;  %状态向量维数
i=1:n;
W0=1/n;
W(i)=1/(2*n);
W(i+n)=1/(2*n);
P=[1 0 0 0;
   0 1 0 0;
   0 0 1 0;
   0 0 0 1];
M=sqrt(n*P);
SX(:,1)=[250 1000 2 -2]';
%滤波算法
for k=2;N
  %  PX(:,k)=F*SX(:,k-1);
 %   PP=F*P*F'+B*R*B';
   % X0(:,k-1)=SX(:,k-1);
    X1(:,k-1)=SX(:,k-1)+M(:,1);
    X2(:,k-1)=SX(:,k-1)+M(:,2);
    X3(:,k-1)=SX(:,k-1)+M(:,3);
    X4(:,k-1)=SX(:,k-1)+M(:,4);
    X5(:,k-1)=SX(:,k-1)-M(:,1);
    X6(:,k-1)=SX(:,k-1)-M(:,2);
    X7(:,k-1)=SX(:,k-1)-M(:,3);
    X8(:,k-1)=SX(:,k-1)-M(:,4);
   % X0(:,k)=F*X0(:,k-1); 
    X1(:,k)=F*X1(:,k-1); X2(:,k)=F*X2(:,k-1); X3(:,k)=F*X3(:,k-1); X4(:,k)=F*X4(:,k-1); X5(:,k)=F*X5(:,k-1); X6(:,k)=F*X6(:,k-1);
    X7(:,k)=F*X7(:,k-1); X8(:,k)=F*X8(:,k-1);
    Xm(:,k)=1/(2*n)*(X1(:,k)+X2(:,k)+X3(:,k)+X4(:,k)+X5(:,k)+X6(:,k)+X7(:,k)+X8(:,k));
    PP=1/(2*n)*((X1(:,k)-Xm(:,k))*(X1(:,k)-Xm(:,k))'+(X2(:,k)-Xm(:,k))*(X2(:,k)-Xm(:,k))'+(X3(:,k)-Xm(:,k))*(X3(:,k)-Xm(:,k))'+(X4(:,k)-Xm(:,k))*(X4(:,k)-Xm(:,k))'+(X5(:,k)-Xm(:,k))*(X5(:,k)-Xm(:,k))'+(X6(:,k)-Xm(:,k))*(X6(:,k)-Xm(:,k))'+(X7(:,k)-Xm(:,k))*(X7(:,k)-Xm(:,k))'+(X8(:,k)-Xm(:,k))*(X8(:,k)-Xm(:,k))')+Q(k);
   % Z0(:,k)=[sqrt(X0(1,k)^2+X0(2,k)^2);atan(X0(1,k)/X0(2,k))];
    Z1(:,k)=[sqrt(X1(1,k)^2+X1(2,k)^2);atan(X1(1,k)/X1(2,k))];
    Z2(:,k)=[sqrt(X2(1,k)^2+X2(2,k)^2);atan(X2(1,k)/X2(2,k))];
    Z3(:,k)=[sqrt(X3(1,k)^2+X3(2,k)^2);atan(X3(1,k)/X3(2,k))];
    Z4(:,k)=[sqrt(X4(1,k)^2+X4(2,k)^2);atan(X4(1,k)/X4(2,k))];
    Z5(:,k)=[sqrt(X5(1,k)^2+X5(2,k)^2);atan(X5(1,k)/X5(2,k))];
    Z6(:,k)=[sqrt(X6(1,k)^2+X6(2,k)^2);atan(X6(1,k)/X6(2,k))];
    Z7(:,k)=[sqrt(X7(1,k)^2+X7(2,k)^2);atan(X7(1,k)/X7(2,k))];
    Z8(:,k)=[sqrt(X8(1,k)^2+X8(2,k)^2);atan(X8(1,k)/X8(2,k))];
    Zm(:,k)=1/(2*n)*(Z1(:,k)+Z2(:,k)+Z3(:,k)+Z4(:,k)+Z5(:,k)+Z6(:,k)+Z7(:,k)+Z8(:,k));
    Pzz=1/(2*n)*((Z1(:,k)-Zm(:,k))*(Z1(:,k)-Zm(:,k))'+(Z2(:,k)-Zm(:,k))*(Z2(:,k)-Zm(:,k))'+(Z3(:,k)-Zm(:,k))*(Z3(:,k)-Zm(:,k))'+(Z4(:,k)-Zm(:,k))*(Z4(:,k)-Zm(:,k))'+(Z5(:,k)-Zm(:,k))*(Z5(:,k)-Zm(:,k))'+(Z6(:,k)-Zm(:,k))*(Z6(:,k)-Zm(:,k))'+(Z7(:,k)-Zm(:,k))*(Z7(:,k)-Zm(:,k))'+(Z8(:,k)-Zm(:,k))*(Z8(:,k)-Zm(:,k))')+R(k);
    Pxz=1/(2*n)*((X1(:,k)-Xm(:,k))*(Z1(:,k)-Zm(:,k))'+(X2(:,k)-Xm(:,k))*(Z2(:,k)-Zm(:,k))'+(X3(:,k)-Xm(:,k))*(Z3(:,k)-Zm(:,k))'+(X4(:,k)-Xm(:,k))*(Z4(:,k)-Zm(:,k))'+(X5(:,k)-Xm(:,k))*(Z5(:,k)-Zm(:,k))'+(X6(:,k)-Xm(:,k))*(Z6(:,k)-Zm(:,k))'+(X7(:,k)-Xm(:,k))*(Z7(:,k)-Zm(:,k))'+(X8(:,k)-Xm(:,k))*(Z8(:,k)-Zm(:,k))');
    K=Pxz*inv(Pzz);
    SX(:,k)=Xm(:,k)+K*(Z(:,k)-Zm(:,k));
    P=PP-K*Pzz*K';
end
%绘制轨迹
k=1:N;
plot(X(1,:),X(2,:),'r');
%plot(Xx(1,:),Xx(2,:),'r');
hold on
plot(SX(1,:),SX(2,:),'g');
hold on;
grid on