无人机开发

V1

2022/11/29阅读:25主题:默认主题

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与磁力计融合定位代码

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

分类:

人工智能

标签:

人工智能

作者介绍

无人机开发
V1