PIXHAWK 飞控中的EKF姿态估计的欧拉角求解

Posted bbzz2

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了PIXHAWK 飞控中的EKF姿态估计的欧拉角求解相关的知识,希望对你有一定的参考价值。


题主没有说明是原生固件还是APM版的固件,原生固件已经取消EKF姿态解算了,官方解释的原因是占RAM太大。估且就认为你问的是原生固件EKF姿态解算算法的步骤吧,简单的介绍下,看不明白,以及相关理论不清楚的查阅介绍惯性导航的书籍即可,应该没多少人需要真正研究算法的吧~
首先是建模,设状态变量,
技术分享

其中w为体系角速度,wa为体系角加速度,ze为重力向量,mu为磁场向量。如果使用惯性矩阵修正当前角加速度,那么:

技术分享则机体系角速度为:

技术分享

再根据机体坐标系相对地理坐标系的旋转角速度变换矩阵:

技术分享

如果采用指数函数的一阶泰勒近似有:

技术分享

技术分享则状态转移A矩阵为:

技术分享

过程噪声矩阵为:

技术分享

分别表示陀螺角速度过程噪声,角加速度过程噪声,加速度过程噪声,磁力计过程噪声。

协方差阵为:

技术分享

测量噪声矩阵:

技术分享观测矩阵H为:

技术分享

测量方程为:

技术分享

引入测量噪声:

技术分享EKF的卡尔曼增益矩阵:

技术分享状态估计:

技术分享

更新协方差矩阵:

技术分享

提取状态信息:

技术分享技术分享最后得到参考系和机体系的变换矩阵:

技术分享转换成欧拉角:

技术分享EKF方法计算量大,在计算状态转换阵和基础矩阵时采用了一阶近似,这种近似误差较大,如果计算量允许,可以用2-3阶。

原生固件有关EKF解算的,嵌入式代码是matlab自动生成的,千万不要直接看C代码。。。。




以上是关于PIXHAWK 飞控中的EKF姿态估计的欧拉角求解的主要内容,如果未能解决你的问题,请参考以下文章

飞控姿态解算中,欧拉角与四元数之间的转换

根据相机旋转矩阵求解三个轴的旋转角/欧拉角

四旋翼飞控里 为啥一定要用四元数?用欧拉角不一样吗? 就算用四元数也是将四元数转化为欧拉角进

滤波跟踪基于matlab拓展卡尔曼滤波EKF的四旋翼无人机姿态估计含Matlab源码 2402期

MPU6050陀螺仪与Processing和上位机飞控联动实录

运动学基于matlab EKF姿态估计含Matlab源码 1638期