kalmandynamic.m

卡尔曼在目标跟踪中的应用本文对作匀速运动的二维空中机动目标进行研究

%%函数2 动态模型
function [Xm_est,Pm_estimate,ua1,qq,m]=kalmandynamic(Xm_pre,Xm_est,Pm_estimate,z1,k,P,qq,m);
T=1;
I=diag([1,1,1,1,1,1]);
Phi=[1,T,0,0,(T^2)/2,0;0,1,0,0,T,0;0,0,1,T,0,(T^2)/2;0,0,0,1,0,T;0,0,0,0,1,0;0,0,0,0,0,1];
H=[1,0,0,0,0,0;0,0,1,0,0,0];
G=[(T^2)/2,0;T,0;0,(T^2)/2;0,T;1,0;0,1];
R=[10000,0;0,10000];     %  观测噪声方差阵
alpha=0.8;               %  加权衰减因子
window=1/(1-alpha);      %  检测机动的有效窗口长度
Xm_estimate(k-1,:)=Xm_est;
if qq==1   %由非机动进入机动模型，需进行修正， 初始化
    Xm_predict(k,:)=Xm_pre;
    Xm_estimate(k,5)=[z1(1)-Xm_predict(k,1)]*2/(T^2);
    Xm_estimate(k,6)=[z1(2)-Xm_predict(k,3)]*2/(T^2);
    Xm_estimate(k,1)=z1(1);
    Xm_estimate(k,3)=z1(2);
    Xm_estimate(k,2)=Xm_estimate(k-1,2)+Xm_estimate(k,5)*T;
    Xm_estimate(k,4)=Xm_estimate(k-1,4)+Xm_estimate(k,6)*T;
    % 修正协方差阵
    Pm_estimate(1,1)=R(1,1);
    Pm_estimate(3,3)=R(2,2);

    Pm_estimate(1,2)=R(1,1)*2/T;
    Pm_estimate(2,1)=Pm_estimate(1,2);
    Pm_estimate(3,4)=R(2,2)*2/T;
    Pm_estimate(4,3)=Pm_estimate(3,4);

    Pm_estimate(1,5)=R(1,1)*2/(T^2);
    Pm_estimate(5,1)=Pm_estimate(1,5);
    Pm_estimate(3,6)=R(2,2)*2/(T^2);
    Pm_estimate(6,3)=Pm_estimate(3,6);

    Pm_estimate(5,5)=[R(1,1)+P(1)+P(2)*2*T+P(3)*T*T]*4/(T^4);
    Pm_estimate(6,6)=[R(2,2)+P(4)+P(5)*2*T+P(6)*T*T]*4/(T^4);

    Pm_estimate(2,2)=R(1,1)*4/(T^2)+P(1)*4/(T^2)+P(3)+P(2)*4/T;
    Pm_estimate(4,4)=R(2,2)*4/(T^2)+P(4)*4/(T^2)+P(6)+P(5)*4/T;

    Pm_estimate(2,5)=R(1,1)*4/(T^3)+P(1)*4/(T^3)+P(3)*2/T+P(2)*6/(T^2);
    Pm_estimate(5,2)=Pm_estimate(2,5);
    Pm_estimate(4,6)=R(2,2)*4/(T^3)+P(4)*4/(T^3)+P(6)*2/T+P(5)*6/(T^2);
    Pm_estimate(6,4)=Pm_estimate(4,6);
    Xm_est=Xm_estimate(k,:);

    qq=0;%修正后，标志qq复位（不再初始化），ua1设为１０，使不进入非机动模型
    ua1=10;
    m=0;
else 
    %  滤波方程
    Xm_predict(k,:)=(Phi*Xm_estimate(k-1,:)')';
    Q=[(Xm_estimate(k-1,5)/20)^2,0;0,(Xm_estimate(k-1,6)/20)^2];
    Pm_predict=Phi*Pm_estimate*(Phi)'+G*Q*G';
    K=Pm_predict*(H)'*inv(H*Pm_predict*(H)'+R);
    Xm_estimate(k,:)=(Xm_predict(k,:)'+K*(z1-H*Xm_predict(k,:)'))';
    Pm_estimate=(I-K*H)*Pm_predict;
    Xm_est=Xm_estimate(k,:);
    m=m+1;
    delta(k)=[Xm_estimate(k,5),Xm_estimate(k,6)]*[Pm_estimate(5,5),0;0,Pm_estimate(6,6)]*[Xm_estimate(k,5);Xm_estimate(k,6)];
    if m>=window
        ua(k)=delta(k)+delta(k-1)+delta(k-2)+delta(k-3)+delta(k-4);
        ua1=ua(k);
    else
        ua1=10;
    end
end