惯导与里程计组合（2）

(5) 卡尔曼滤波初始化参数简单介绍

   Q 系统噪声协方差矩阵 ，该矩阵定义了在卡尔曼滤波模型噪声源的影响下，状态估计的不确定度随时间增长的规律。（通常对角元素为功率谱密度或随机游走误差的平方）

   R 观测噪声协方差矩阵 ，该矩阵一般情况下为对角矩阵，对角线是各观测量的方差

   P 系统误差协方差矩阵，定义为状态估计与真值间偏差平方的期望值。

(6) 组合解算

  6.1 惯导更新算法

    6.1.1 根据惯导陀螺获取的角增量及加速度计获取的速度增量，利用双子样（多子样）算法进行圆锥及划桨误差补偿。然后扣除陀螺及加计零偏误差影响

    6.1.2 更新获取t/2处地球参数（t/2处的有害加速度），求解载体坐标系下t时刻的比力fb与角速度（平均值），进一步转换至导航坐标系下比力fn，扣除有害加速度及导航坐标系旋转误差影响，更新速度

    6.1.3更新位置

    6.1.4更新姿态

   6.2 里程计姿态及位置更新

   6.3 卡尔曼滤波时间更新

   6.4 卡尔曼滤波测量更新

   6.5  闭环反馈校正