IGNAV_NHC分析

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下速度误差的系数。