f_poly_fit.m

多目标跟踪 (三维)-算法matlab编制

function [a_rt,p_rt]=f_poly_fit(k,t,mes_now,a0,p0,r);
%利用k+1时刻的量测数据，递推估计多项式拟合系数
% 阶乘函数
%k:仿真时间
%t:采样间隔
%a_rt:返回的多项式系数估计
%p_rt:返回的多项式系数估计协方差
kt=k*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];

s=h*p0*h'+r;
w=p0*h'*inv(s);

a_rt=a0+w*(mes_now-h*a0);
p_rt=p0-w*s*w';