f_get_xp.m

来自「多目标跟踪 (三维)-算法matlab编制」· M 代码 · 共 24 行

function [x_rt,p_rt,d_rt]=f_get_xp(M,t,matrix,a,p,mes_total,measurement,r)

x_rt=[];
p_rt=[];
d_rt=0;%该假设整个统计距离
temp1=zeros(3,3);
for k=1:M
    
    index=sum(mes_total(1:k-1))+matrix(k);
    mes_now=measurement(:,index);
    kt=(k-1)*t;
    h=[1 kt kt^2/2 0 0 0 0 0 0;0 0 0 1 kt kt^2/2 0 0 0;0 0 0 0 0 0 1 kt kt^2/2];
    z_predict=h*a; 
    d_rt=d_rt+(mes_now-z_predict)'*inv(r)*(mes_now-z_predict);
    temp2=[1 kt kt^2/2;0 1 kt;0 0 1];
    fait=[temp2 temp1 temp1];
    fait=[fait;temp1 temp2 temp1];
    fait=[fait;temp1 temp1 temp2];
   
    x=fait*a;
    p_temp=fait*p*fait';
    x_rt=[x_rt x];%各个时刻的状态估计值
    p_rt=[p_rt p_temp];
end