PX4 EKF模块解读含matlab代码

这里主要介绍px4里面的定位模块，即EKF库。

1、状态向量与协方差的预测

1、Px4的状态向量为24维，其如下所示：

其中 表示NED系至体系的旋转四元数， 表示在ned系的速度， 表示机体系原点(imu位置）在ned系的位置,， 表示陀螺仪角度增量的偏差， 为加速度计获取的速度增量偏差， 为地磁向量在ned系的投影， 为磁力计的零偏， 为水平方向的风速。

2、IMU传播

• 四元数传播

上式中 为最优传感器输出的结果（通过标定、圆锥补偿、温补）， 为地球自转运动所产生的角度增量， 为均值为0，方差为daVar的高斯白噪声。将 转换为四元数 得到，即；

• 速度状态传播

上式中 为最优加速度计传感器输出的结果（通过标定、圆锥补偿、温补）， 为均值为0，方差为dvVar的高斯白噪声。故而得到

• 位置状态传播

• 其它状态传播 其它状态全部建立为CV模型，故而

3、状态方程、协方差方程递推 卡尔曼滤波的状态递推方程如下：

其中 为imu的测量信息，即 。

2、卡尔曼滤波初始化

2.1 俯仰、滚转角初始化

当imu静止时，其加速度计敏感到的力为重力的反作用力，故其

即可以得到初始俯仰角和滚转角

2.2 航向角初始化

*** 通过磁力计获取** 建立临时体系，其坐标系为ned系旋转初始航向角所形成的坐标系 系，通过 即获取初始磁向量在 系的投影，即磁向量与 系x轴之间的夹角为 即真实航向角为 ，其中 为磁偏角，可通过查表获取。 *** 通过视觉获取** 由于通过视觉传感器信息可以获取导航系至体系的转换矩阵，其形式为 ，故得到

*** 通过双天线获取** 双天线单位矢量在体系的坐标为 ，其中 为双天线矢量与无人机纵轴之间的夹角，其方向为纵轴绕z轴进行顺时针旋转。

即

2.3 高度初始化

利用气压计或者外部视觉高度进行初始化，由_params.vdist_sensor_type进行控制。

2.4 Ned系地磁向量初始化

由磁力计测得的磁矢量为 （已扣除先前估计的磁零偏值）， ，其中 由初始化所得到的姿态角获取。

2.5 其余状态初始化

其余状态全部初始化为0。

2.6 协方差初始化

已知姿态角初始方差 为sq(_params.initial_tilt_err)

其中 为姿态角, 为对准后的航向误差，其服从均值为0，方差为 （由对应航向传感器参数决定）。即 ，其中 , 为初始化姿态角， 。

即 对应的协方差为

水平速度项噪声为

垂向速度项噪声为

水平位置项噪声为

垂向位置项噪声 则由不同高度源对应的标准差参数确定。

陀螺零偏的初始方差为

加速度计零偏的初始方差为

其imu最优传感器切换时，即复位于上述值。

、 的初始方差为

的初始方差为

3、下一讲继续讲px4 ekf模块里面的测量方程、时间同步与空间同步推导

关注公众号，获取imu与磁力计融合定位代码

❤️ 扫一扫，添加我的公众号或者搜索【无人机开发】