lmode_initial.m

kalman滤波的程序及ppt

function [o,h,g,q,perr,xk]=lmode_initial(T,r,zx,zy,vkxs,vkys,perr2)
o=4:4; g=4:2; h=2:4; q=2:2; xk=4:1; perr=4:4;
o=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];
h=[1 0 0 0;0 0 1 0];
g=[(T^2)/2 0;T 0; 0 (T^2)/2;0 T];
q=[10000 0;0 10000];
xk=[zx(r-1);vkxs(r-1);zy(r-1);vkys(r-1)];
perr=perr2(1:4,1:4);