cs1.m

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

trajectory1;

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

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

%CS
F1=[1  T  T^2/2  0   0    0
   0  1    T    0   0    0
   0  0    1    0   0    0
   0  0    0    1   T  T^2/2
   0  0    0    0   1    T
   0  0    0    0   0    1];

a=1/40;
a_max=100;
a_max_minus=-100;

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));

q=[ 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];
 
for i=2:length(z)   
     x(:,i)=F1*x(:,i-1);
     if x(3,i) >= 0
        delta(1)=(4-pi)/pi*(a_max-x(3,i))^2;
     else
        delta(1)=(4-pi)/pi*(a_max_minus+x(3,i))^2;
     end
     if x(6,i) >= 0
        delta(2)=(4-pi)/pi*(a_max-x(6,i))^2;
     else
        delta(2)=(4-pi)/pi*(a_max_minus+x(6,i))^2;
     end
     deltaA=diag([delta(1) delta(1) delta(1) delta(2) delta(2) delta(2)]); 
     Q=2*a*deltaA*q;
     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(4,:),'b-'),hold on;
% plot(z(3,:),'k:')
% plot(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('真实轨迹','测量值','滤波值');