主函数‘
function [...H
states, ... % state vector after fusion of measurements
P, ... % state covariance matrix after fusion of corrections
innovation,... % NED velocity innovations (m/s)
varInnov] ... % NED velocity innovation variance ((m/s)^2)
= FuseVelocity( ...
states, ... % predicted states from the INS
P, ... % predicted covariance
measVel, ... % NED velocity measurements (m/s)
gateSize, ... % Size of the innovation consistency check gate (std-dev)
R_OBS) % velocity observation variance (m/s)^2
初始化
innovation(1,2)
varInnov(1,2);
H(3,24)
H v e l = 1 , H e l s e = 0 H_{vel}=1,H_{else}=0 Hvel=1,Helse=0
求预测残差与协方差
i n n o v a t i o n 1 × 3 = P o s − − P o s m e a innovation_{1×3}=Pos^--Pos_{mea} innovation