hmode_initial.m

kalman滤波的程序及ppt

function [o,g,h,q,perr,xk]=hmode_initial(T,r,e,zx,zy,xk1s,yk1s,vkxs,vkys,perr11,perr12,perr22)
o=6:6; g=6:2; h=2:6; q=2:2; xk=6:1; perr=6;6;
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 1];
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=vkxs(r-1/(1-e)-1)+2*jsx;
vy=vkys(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 1000];