singer3.m

雷达数据处理中的重要模型算法之一

trajectory1;

x(:,1)=X(:,1);  %初始状态向量

R=diag([200^2  10^2  200^2  10^2]);
P=50*eye(6);   %初始状态向量协方差

%：singer模型
a=1/20;
F=[1    T    -(1-a*T-exp(-a*T))/a^2   0     0            0
   0    1       (1-exp(-a*T))/a       0     0            0
   0    0         exp(-a*T)           0     0            0
   0    0             0               1     T    -(1-a*T-exp(-a*T))/a^2
   0    0             0               0     1       (1-exp(-a*T))/a
   0    0             0               0     0         exp(-a*T)  ];

    q11= 1/(2*a^5)*(1-exp(-2*a*T)+2*a*T+2/3*a^3*T^3-2*a^2*T^2-4*a*T*exp(-a*T));
    q12= 1/(2*a^4)*(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2);
    q13=1/(2*a^3)*(1-exp(-2*a*T)-2*a*T*exp(-a*T));
    q21=q12;
    q22=1/(2*a^3)*(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T);
    q23=1/(2*a^2)*(exp(-2*a*T)+1-2*exp(-a*T));
    q31=q13;
    q32=q23;
    q33=1/(2*a)*(1-exp(-2*a*T));

am=120;  %最大机动加速度
pm=0.5; %am的概率
p0=0.5; %非机动概率
delta=am^2*(1+4*pm-p0)/3;    
    
Q=2*a*[ q11  q12  q13   0    0    0
        q21  q22  q23   0    0    0 
        q31  q32  q33   0    0    0     
         0    0    0   q11  q12  q13
         0    0    0   q21  q22  q23
         0    0    0   q31  q32  q33]*delta*0.5;
 
for i=2:length(z)    
     x(:,i)=F*x(:,i-1);
     P=F*P*F'+Q;
     H=[ 1, 0, 0, 0, 0, 0
         0, 1, 0, 0, 0, 0
         0, 0, 0, 1, 0, 0
         0, 0, 0, 0, 1, 0];
     K=P*H'*inv(H*P*H'+R);% 增益K
     V=z(:,i)-H*x(:,i);
     x(:,i)=x(:,i)+K*V;
     P=(eye(6)-K*H)*P*(eye(6)+K*H)'-K*R*K';
end

%画图观察
% figure(1);
% plot(X(1,:),'b-'),hold on;
% plot(z(1,:),'k:')
% plot(x(1,:),'r:')
% title('横坐标位移');
% xlabel('秒');ylabel('米');
% legend('真实轨迹','测量值','滤波值');
% 
% figure(2);
% plot(X(2,:),'b-'),hold on;
% plot(z(2,:),'k:')
% plot(x(2,:),'r:')
% title('横坐标速度');
% xlabel('秒');ylabel('米/秒');
% legend('真实轨迹','测量值','滤波值');

% figure(3);
% plot(X(1,:),X(4,:),'b-'),hold on;
% plot(z(1,:),z(3,:),'k:')
% plot(x(1,:),x(4,:),'r:')
% title('纵坐标位移');
% xlabel('秒');ylabel('米');
% legend('真实轨迹','测量值','滤波值');
% 
% figure(4);
% plot(X(5,:),'b-'),hold on;
% plot(z(4,:),'k:')
% plot(x(5,:),'r:')
% title('纵坐标速度');
% xlabel('秒');ylabel('米/秒');
% legend('真实轨迹','测量值','滤波值');