function [a_rt,p_rt]=f_poly_fit(k,t,mes_now,a0,p0,r);
%利用k+1時刻的量測數(shù)據(jù)，遞推估計多項式擬合系數(shù)
% 階乘函數(shù)
%k:仿真時間
%t:采樣間隔
%a_rt:返回的多項式系數(shù)估計
%p_rt:返回的多項式系數(shù)估計協(xié)方差
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';