无人机开发
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、状态方程、协方差方程递推 卡尔曼滤波的状态递推方程如下:
其中
2、卡尔曼滤波初始化
2.1 俯仰、滚转角初始化
当imu静止时,其加速度计敏感到的力为重力的反作用力,故其
即可以得到初始俯仰角和滚转角
2.2 航向角初始化
*** 通过磁力计获取** 建立临时体系,其坐标系为ned系旋转初始航向角所形成的坐标系
*** 通过双天线获取** 双天线单位矢量在体系的坐标为
即
2.3 高度初始化
利用气压计或者外部视觉高度进行初始化,由_params.vdist_sensor_type进行控制。
2.4 Ned系地磁向量初始化
由磁力计测得的磁矢量为
2.5 其余状态初始化
其余状态全部初始化为0。
2.6 协方差初始化
已知姿态角初始方差
其中
即
水平速度项噪声为
垂向速度项噪声为
水平位置项噪声为
垂向位置项噪声
陀螺零偏的初始方差为
加速度计零偏的初始方差为
其imu最优传感器切换时,即复位于上述值。
3、下一讲继续讲px4 ekf模块里面的测量方程、时间同步与空间同步推导
关注公众号,获取imu与磁力计融合定位代码
❤️ 扫一扫,添加我的公众号或者搜索【无人机开发】
分类:
人工智能标签:
人工智能作者介绍