【组合导航代码ECL】状态融合函数FuseVelocity.m

主函数‘

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_{vel}=1,H_{else}=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