Pixhawk学习7——位置解算

Pixhawk的位置解算分为两部分，第一部分主要为传感器的数据获取，而该部分最主要的就是GPS数据的提取。第二部分为与惯性器件之间的组合导航。组合导航的好处我就不用多说了。

Pixhawk代码中目前主要有两处组合导航的代码：
1）Firmware/src/examples/ekf_att_pos_estimator
2）Firmware/src/modules/ekf2 + Firmware/src/lib/ecl/ekf

第一个状态变量的维数为22，第二个是24，整体上没有太大区别。以第一个为例：

该部分代码完全是按照惯性导航与组合导航的方法进行姿态和位置解算的，起到了类似于大型无人机捷联组合导航系统的作用。

该部分卡拉曼滤波组合导航采用22阶kalman，所需要估计的状态矩阵为：
Index // State vector: 注释
// 0-3: quaternions (q0, q1, q2, q3) 四元数
// 4-6: Velocity - m/sec (North, East, Down) 地理系速度
// 7-9: Position - m (North, East, Down) 地理系位置
// 10-12: Delta Angle bias - rad (X,Y,Z) 陀螺漂移
// 13: Delta Velocity Bias - m/s (Z) 加计零偏
// 14-15: Wind Vector - m/sec (North,East) 地理水平风速
// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) 地理系磁场
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) 机体系磁场

量测量包括： GPS速度位置、磁力计、空速、光流、

整体程序分为以下几个部分：

1、 捷联惯性导航

该部分内容完全是规规矩矩的捷联惯性导航的步骤，在这里不多做解释。代码内容可以看以光衢的《惯性导航原理》或者秦永元的《惯性导航》。
经过惯性导航解算，采用陀螺和加计的原始数据，得到了状态0-9的更新。

2、 数据提取

在task_main()中，首先进行了数据提取。

3、 Kalman滤波

在进行kalman滤波之前，首先判断了量测量是否可靠，需要根据量测量的可靠与否，在更新测量矩阵H，旨在决定是否要引入或者舍弃某些量测量信息。

4、 Publish组合导航的数据