hmodeinitial.m

该文件为Kalman滤波部分程序

function [o,g,h,q,perr,xk]=hmodeinitial(T,r,e,zx,zy,xk1s,yk1s,vxks,vyks,perr11,perr12,perr22)
o=[1,T,0,0,0.5*T^2,0;0,1,0,0,T,0;0,0,1,T,0,0.5*T^2;0,0,0,1,0,T;0,0,0,0,1,0;0,0,0,0,0,T];
h=[1,0,0,0,0,0;0,0,1,0,0,0];
g=[T^2/4,0;T/2,0;0,T^2/4;0,T/2;1,0;0,1];
q=[10000,0;0,10000];
jsx=0.5*(zx(r-1/(1-e))-xk1s(r-1/(1-e)));
jsy=0.5*(zy(r-1/(1-e))-yk1s(r-1/(1-e)));
vx=vxks(r-1/(1-e)-1)+2*jsx;
vy=vyks(r-1/(1-e)-1)+2*jsy;
xk=[zx(r-5);vx;zy(r-5);vy;jsx;jsy];
p111=10000;p112=10000;p115=5000;
p155=0.25*(10000+perr11(r-6)+4*perr12(r-6)+perr22(r-6));
p122=10000+perr11(r-6)+4*perr12(r-6)+2*perr12(r-6);
p125=5000+0.5*perr11(r-6)+perr22(r-6)+1.5*perr12(r-6);
perr=[p111,p112,0,0,p115,0;0,p122,0,0,p125,0;0,0,100,0,0,0;0,0,0,100,0,0;0,0,0,0,p155,0;0,0,0,0,0,100]