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

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

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

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

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

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

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

2、卡尔曼滤波初始化

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

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

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

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

即

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

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

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

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

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

即对应的协方差为

水平速度项噪声为

垂向速度项噪声为

水平位置项噪声为

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

陀螺零偏的初始方差为

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

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

、的初始方差为

的初始方差为

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