delay_ekf_non_line.m

带时延的纯方位角单站跟踪算法,它有效地解决了用EKF算法进行纯方位角跟踪时可能出现的不稳定和滤波发散现象以及声音信号的时延问题。

function [] =non_line()
%
steps=60;
T0=1;
T=zeros(1,steps);
x=zeros(7,steps);
vt=120;
vc=200;
a=50/180*pi;% target tracking angel
mm=2;
for mon=1:mm^2
x(1:4,1)=[20,vt*cos(a),90,vt*sin(a)]';
x(5,1)=sqrt(x(1,1)^2+x(3,1)^2);% distance
x(6,1)=x(5,1)/vc;% time delay
x(7,1)=0;% flying time
for k=1:20
    for t=0:0.001:100
        temp_x=x(1,k)+vt*t;
        temp_y=x(3,k);
        temp_vx=vt;
        temp_vy=0;
        temp_r=sqrt(temp_x^2+temp_y^2);
        temp_t=temp_r/vc;
        if abs((x(6,k)+T0)-(t+temp_t))<=0.01
            x(1,k+1)=temp_x;
            x(2,k+1)=temp_vx;
            x(3,k+1)=temp_y;
            x(4,k+1)=temp_vy;
            x(5,k+1)=temp_r;
            x(6,k+1)=temp_t;
            x(7,k+1)=t;
            break;
        end
    end
end
for k=21:40
    for t=0:0.001:100
        temp_x=x(1,k)+vt*cos(a)*t;
        temp_vx=vt*cos(a);
        temp_y=x(3,k)+vt*sin(a)*t;
        temp_vy=vt*sin(a);
        temp_r=sqrt(temp_x^2+temp_y^2);
        temp_t=temp_r/vc;
        if abs((x(6,k)+T0)-(t+temp_t))<=0.01
            x(1,k+1)=temp_x;
            x(2,k+1)=temp_vx;
            x(3,k+1)=temp_y;
            x(4,k+1)=temp_vy;
            x(5,k+1)=temp_r;
            x(6,k+1)=temp_t;
            x(7,k+1)=t;
            break;
        end
    end
end
for k=41:steps
    for t=0:0.001:100
        temp_x=x(1,k);
        temp_vx=0;
        temp_y=x(3,k)+vt*t;
        temp_vy=vt;
        temp_r=sqrt(temp_x^2+temp_y^2);
        temp_t=temp_r/vc;
        if abs((x(6,k)+T0)-(t+temp_t))<=0.01
            x(1,k+1)=temp_x;
            x(2,k+1)=temp_vx;
            x(3,k+1)=temp_y;
            x(4,k+1)=temp_vy;
            x(5,k+1)=temp_r;
            x(6,k+1)=temp_t;
            x(7,k+1)=t;
            break;
        end
    end
end
% x(7,:)*cos(a)*vt;
% x(7,:);
% plot(x(1,:),x(3,:),'b.');

% ideal data
z_v=zeros(2,steps);
v0=0.01;
v=v0*randn(2,steps);
for k=1:steps
    z_v(1,k)=atan(x(3,k)/x(1,k))+v(1,k);
    z_v(2,k)=vt+v(2,k);
end

w0=20;
w=w0*randn(2,steps);
F=zeros(4,4,steps);
G=zeros(4,2,steps);
% F=[1,T,0,0
%     0,1,0,0
%     0,0,1,T
%     0,0,0,1];
% G=[0.5*T^2,0
%     T,0
%     0,0.5*T^2
%     0,T];
H=zeros(2,4,steps);
X=zeros(4,steps);
% X(:,1)=[300,vt*cos(z_v(1,1)),400,vt*sin(z_v(1,1))];
X(:,1)=[50,vt,70,vt];
% X(:,1)=x(1:4,1);
% X(:,1)=[30,4,40,3];
Xp=zeros(4,steps);
P=zeros(4,4,steps);
Q=w0^2*eye(2);
R=v0^2*eye(2);
I=eye(4,4);
P(:,:,1)=[10,0,0,0
            0,1,0,0
            0,0,10,0
            0,0,0,1];
Pp=zeros(4,4,steps);
K=zeros(4,2,steps);
ze=zeros(2,steps);
X1rms=zeros(1,steps);
Y1rms=zeros(1,steps);
for k=1:(steps-1)
    for t=0:0.01:10
        temp_F=[1,t,0,0
                0,1,0,0
                0,0,1,t
                0,0,0,1];
        temp_G=[0.5*t^2,0
                t,0
                0,0.5*t^2
                0,t];
        temp_xp=temp_F*X(:,k);
        if abs((sqrt(temp_xp(1)^2+temp_xp(3)^2)/vc+t)-(sqrt(X(1,k)^2+X(3,k)^2)/vc+T0))<0.01
            F(:,:,k)=temp_F;
            G(:,:,k)=temp_G;
            T(k)=t;
            break;
        end
    end
    Xp(:,k+1)=F(:,:,k)*X(:,k);
    Pp(:,:,k+1)=F(:,:,k)*P(:,:,k)*F(:,:,k)'+G(:,:,k)*Q*G(:,:,k)';
    H(1,:,k+1)=[(-Xp(3,k+1))/(Xp(1,k+1)^2+Xp(3,k+1)^2),0,(Xp(1,k+1))/(Xp(1,k+1)^2+Xp(3,k+1)^2),0];
    H(2,:,k+1)=[(Xp(1,k+1)-X(1,k))/(sqrt((Xp(1,k+1)-X(1,k))^2+(Xp(3,k+1)-X(3,k))^2+eps)*T(k)),0,...
        (Xp(3,k+1)-X(3,k))/(sqrt((Xp(1,k+1)-X(1,k))^2+(Xp(3,k+1)-X(3,k))^2+eps)*T(k)),0];
    K(:,:,k+1)=Pp(:,:,k+1)*H(:,:,k+1)'*inv(H(:,:,k+1)*Pp(:,:,k+1)*H(:,:,k+1)'+R);
    ze(1,k+1)=atan(Xp(3,k+1)/Xp(1,k+1));
    ze(2,k+1)=sqrt((Xp(1,k+1)-X(1,k))^2+(Xp(3,k+1)-X(3,k))^2)/T(k);
    X(:,k+1)=Xp(:,k+1)+K(:,:,k+1)*(z_v(:,k+1)-ze(:,k+1));
    P(:,:,k+1)=(I-K(:,:,k+1)*H(:,:,k+1))*Pp(:,:,k+1);

end
for ii=1:steps
 X1rms(1,ii)=X1rms(1,ii)+(X(1,ii)-x(1,ii)).^2;
 Y1rms(1,ii)=Y1rms(1,ii)+(X(3,ii)-x(3,ii)).^2;
end
end
 X1rms(1,:) = sqrt(X1rms(1,:)./(mm^2-1));
 Y1rms(1,:) = sqrt(Y1rms(1,:)./(mm^2-1));   
 X1rms(1,1) = 0;
 Y1rms(1,1) = 0;% m=50;

plot(x(1,:),x(3,:),'b.');
hold;
plot(X(1,:),X(3,:),'r*');
text(200,2500,'蓝点为目标真实轨迹，红点为跟踪结果')
h1=figure;
k=1:steps;
     subplot(2,1,1),stem(k,X1rms(1,:)),title ('X 方向的误差');
     subplot(2,1,2),stem(k,Y1rms(1,:)), title ('Y 方向的误差');