extern int nhc(insstate_t *ins,const insopt_t *opt,const imud_t *imu)函数名
insstate_t* ins IO ins state
insopt_t* opt I ins options
imud_t* imu I imu measurement data
return : 1 (ok) or 0 (fail)
用NHC进行约束,其实用NHC做量测去kalman ,载体b系下 侧向和法向速度为0。
所以这里构建的kalman的量测ZK =hx -v v= hx -zk ,zk为两个方向的量测值0;v=hx;
kalman的量测更新公式中中需要v H R 以及P ,在函数
nv=bldnhc(opt,imu,ins->Cbe,ins->ve,nx,v,H,R)中构建出来。
b系下 的待估的误差项为x,
扰动分析:
vb量测 = 0 = Cbe'(包含失准角)*ve'(包含速度误差) ;
vb量测 = ( I-x)Cbe*(ve+) = Cbe*ve+Cbe* - xCbe*ve = Cbe*ve* + Cbe* + vb= HX-V
量测方程中有两个误差项,失准角和e下的速度误差,对应的系数找到
载体vb : matmul("TN",3,1,3,1.0,Cbe,ve,0.0,vb);
skewsym3(ve,C);
matmul("TN",3,3,3,1.0,Cbe,C,0.0,T); // T失准角的系数
matt(Cbe,3,3,C);// C e下速度误差的系数。