单观测站纯方位EKF算法

建立水下纯方位目标单观测站系统，目标做匀速直线运动，观测站静止位于坐标原点，采用EKF非线性滤波算法跟踪水下运动目标，以下是matlab代码：

%%单观测站EKF方法
%%目标匀速直线运动，观测站静止
clear all; clc;close all;
x=0;          %%观测站x坐标
y=0;           %%观测站Y坐标

DS0=5000;            	    %目标初始距离      
V=15;                      	%速度范围30-70kn
DA=120/180*pi;  			%目标航向角
D=-30/180*pi;       	    %起始方位角
X0=DS0*sin(D);    	        %目标起点坐标
Y0=DS0*cos(D);
Vx=V*sin(DA);
Vy=V*cos(DA);

N=500;                  %%采样点数
T=1;
t=[1:N]*T;              %%观测时刻
Xt=X0+Vx*t;             %%目标瞬时坐标
Yt=Y0+Vy*t;

B0=atan(（Xt-x）./（Yt-y）);       %%观测站准确方位角

err_x=zeros(1,N);%分配空间
err_y=zeros(1,N);
err_vy=zeros(1,N);
err_vx=zeros(1,N);

for num=1:1                        %%蒙特卡罗实验数目
    errB=2.5/180*pi.*randn(1,N);    %%方位角测量误差
    B0=B0+errB;                     %%实际测量方位角
    Z=B0;
%%---运动目标为匀速模型，有噪声驱动的加速度
    F=[1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1];    %%状态转移方程
    G=[T^2/2 0; 0 T^2/2; T 0; 0 T];            %%噪声转移方程
    v=diag([0.01,0.02]); 
    x0=[X0 Y0 Vx Vy]';                         %%状态矢量初始值[X Y Vx Vy]
    Xk(:,1)=x0;
    q=v;
    P0=eye(4,4);                              %%初始误差矩阵
    R=[1/180*pi];                             %%%观测角度的测量方差
    H=zeros(1,4);
    figure(1)
    plot(x(1),y(1),'mo')
   box on
    grid on
    hold on
    plot(Xt,Yt,'k')
    %title('目标航迹')
   
    for i=2:N                                   %%EKF迭代过程
        Xn=F*x0;
        Pp=F*P0*F'+G*q*G';
        tx=Xn(1);
        ty=Xn(2);
        
        H(1,1)=(ty-y(1))/((ty-y(1))^2+(tx-x(1))^2);
        H(1,2)=-(tx-x(1))/((ty-y(1))^2+(tx-x(1))^2);
        K=Pp*H'*inv(H*Pp*H'+R);
   
        Zp=atan((tx-x(1))/(ty-y(1)));
        
        Xk(:,i)=Xn+K*(Z(:,i)-Zp);
        
        Pk=(eye(4)-K*H)*Pp;
        P0=Pk;
        x0=Xk(:,i);
        plot(Xk(1,i),Xk(2,i),'r');    %%画估计目标轨迹
        hold on
        
    end
    error_x=Xk(1,:)-Xt;
    error_y=Xk(2,:)-Yt;
    error_vx=Xk(3,:)-Vx;
    error_vy=Xk(4,:)-Vy;
  
    err_x=err_x+error_x;
    err_y=err_y+error_y;
    err_vx=err_vx+error_vx;
    err_vy=err_vy+error_vy;
end
figure(1)
        plot(Xk(1,:),Xk(2,:),'-r');                 %%画估计目标轨迹
        title('观测噪声方差R=1°时的目标航迹');
        legend('观测站位置','目标真实航迹','目标估计航迹');
        hold on
        box on 

figure(2)
subplot(2,2,1)
plot(abs(err_x/num))
title('X坐标估计误差')
xlabel('时间')
subplot(2,2,2)
plot(abs(err_y/num))
title('Y坐标估计误差')
xlabel('时间')

subplot(2,2,3)
plot(abs(err_vx/num))
title('Vx估计误差')
xlabel('时间')

subplot(2,2,4)
plot(abs(err_vy/num))
title('Vy估计误差')
xlabel('时间')