卡尔曼滤波五个公式_基于ROS的卡尔曼滤波姿态解算
前段时间由于项目关系需要实现基于卡尔曼滤波的姿态解算,也就是说融合加速度计、陀螺仪及磁罗盘进行AHRS姿态解算,得到机器人的姿态角。
本文的学习需要有一定的卡尔曼滤波器基础,可以参考白巧克力亦唯心的卡尔曼滤波博客,或者我的另一篇文章说说我所理解的卡尔曼滤波器。
移动机器人主要工作于二维平面,其位姿用X,Y和Yaw三个状态量表示。目前获得航向角Yaw的方式主要是GPS、磁罗盘和陀螺仪等。GPS容易被墙壁阻隔,穿透性差,不适合在室内使用。陀螺仪主要用于测量车体的角速度,通过角速度的积分可以得到车身转过的角度,其优势在于更新频率高,动态响应快,但由于其存在零点漂移等原因,会存在着持续误差,误差会由于积分而随着时间的推移越来越大。磁罗盘利用地球磁场水平分量指向北极来获取绝对的航向角,其虽然不存在累计误差,但是其动态响应差,所以一般与陀螺仪融合,优势互补,以获得良好的动态响应。
多种滤波器可以用来进行AHRS姿态解算,查了很多资料,目前普遍使用的还是mahony互补滤波器。互补滤波器从加速度计和磁罗盘获得低频数据融合从陀螺仪获得的高频数据进行姿态解算,其显著优势是计算量小,适合在嵌入式平台使用比如无人机姿态解算,是现在商用最广泛的滤波器,但是动态性能低于卡尔曼滤波器,而且对于磁场干扰等情况可能无法有效处理。貌似PX4等飞控的姿态解算也在慢慢转向卡尔曼滤波器。
决定用卡尔曼滤波器姿态解算后,网上的资料尤其是高质量的代码还是少之又少的,直到我在github上找到了大疆libing实现的基于ROS的卡尔曼滤波器姿态解算,github链接如下:
libing64/att_ekfgithub.com
本文首先对他的代码核心进行分析,再指出其代码存在的不足之处,也不一定是不足,只是我参考了经典论文觉得按照我的方法那样写滤波器更加稳定。
Libing的这个源码应该是参考了px4的飞控,代码的具体实现与这个博客四旋翼姿态解算--互补滤波和拓展卡尔曼基本吻合,结合该博客应该能看懂作者是如何实现卡尔曼滤波器的。
作者的核心代码在att_ekf.cpp中,在void Att_ekf::predict(double t) 函数中实现了卡尔曼滤波中的预测,即根据运动方程预测下一时刻状态量,在void Att_ekf::update_magnetic函数中根据测量的磁场实现了状态量的更新。卡尔曼滤波器状态矩阵和测量矩阵如下图。
代码解析如下,开头为行号。
//卡尔曼滤波第一个公式,根据上一时刻状态量及角速度进行状态预测,得到先验状态量。
58、x.segment<3>(0) += wa*dt;
59、x.segment<3>(6) += -skew_symmetric(w)*ra*dt;
60、x.segment<3>(9) += -skew_symmetric(w)*rm*dt; //卡尔曼滤波第二个公式,根据上一时刻协方差和运动方程的协方差得到先验协方差矩阵。
//Q表明的是运动方程状态转移的不确定性。
71、P = A*P*A.transpose() + Q;//Q?//该代码对应卡尔曼滤波第三个公式,即计算K,K表明是更相信预测值还是测量值。
//假设K = 0,则表明完全相信预测值。R表明的是测量过程的不确定性。
80、MatrixXd K = P.inverse()*H.transpose()*(H*P.inverse()*H.transpose() + R_mag).inverse();//卡尔曼滤波第四个公式,融合先验状态量和测量数据得到后验状态量。
83、x = x + K*(z - H*x);
//卡尔曼滤波第五个公式,更新后验协方差矩阵。
84、P = (I- K*H)*P.inverse();
而对于IMU计算出的raw和pitch更新,也和上面一样。对此,对于源代码的分析也算是结束了,但是个人在实际运行的时候发现滤波器很容易发散,后来发现是83行代码的问题,根据我所查看的论文 A Double-Stage Kalman Filter 分析,卡尔曼滤波器的架构应该如下图所示。
根据论文中的算法,如我下图红线部分所示。在使用磁罗盘数据进行测量更新的时候,此时需要将预测值与测量值差值中除磁场部分的其余分量置为0。
具体的代码实现如下。经过此代码后,滤波器运行稳定,不再发散。
Matrix<double, 12, 1> x_mag= K*(z - H*x);
x_mag.segment<9>(0) = MatrixXd::Zero(9,1);
x = x + x_mag;
同样,使用IMU更新的时候也要将更新部分以外的变量置为0。
Matrix<double, 12, 1> x_gyro_acc = K*(z - H*x); //只更新gyro和acc x_gyro_acc.segment<3>(3) = MatrixXd::Zero(3,1);x_gyro_acc.segment<3>(9) = MatrixXd::Zero(3,1);x = x + x_gyro_acc;
有同学应该对卡尔曼滤波器协方差矩阵Q和R的取值表示困惑,Q的取值应该是偏小,作者可能是认为短时间内由于角速度变化不大,预测更新可信度高。在我自己的代码实现过程中,我是参考了PX4的取法。
r1 表示的是对陀螺仪的置信度,由于陀螺仪在短时间内非常准确,所以将其设的很小,我将其固定不变,主要调节的是加速度计协方差 r2和磁罗盘协方差r3 。对于r3 来说,这个值越大,则表明越不相信磁罗盘,抗磁干扰能力越强,静止的时候数据越稳定,数据较为平滑,但是延迟大,将其设为较小值如10的时候,灵敏度很高,静止的时候输出的航向角变化幅度相对较大。
到这里,对于卡尔曼滤波器的姿态解算应该讲完了,结合我推荐的资料和我自己的分析,相信你应该能够理解卡尔曼滤波器的实现源码。
————————————————
版权声明:本文为CSDN博主「当你途经我」的原创文章,遵循CC 4.0 BY-SA版权协议,转载请附上原文出处链接及本声明。
原文链接:https://blog.csdn.net/sinat_33970617/article/details/100145445
卡尔曼滤波五个公式_基于ROS的卡尔曼滤波姿态解算相关推荐
- bvp解算器是什么_几种飞控的姿态解算算法
姿态解算是飞控的一个基础.重要部分,估计出来的姿态会发布给姿态控制器,控制飞行平稳,是飞行稳定的最重要保障.有关姿态解算的基础知识,这里笔者不会细细描述,有关这方面的资料,网上已经有很多.主要是先掌握 ...
- 一看就懂的卡尔曼滤波五个公式
一看就懂的卡尔曼滤波五个公式 任何物体的运动都有一个运动方程: xk^=Axk−1+u+w,w∈(0,Q)(1)\hat{x_k} =Ax_{k-1}+u+w ,w\in (0,Q) \tag{1} ...
- 无人机姿态解算_扩展卡尔曼滤波(2)
一.扩展卡尔曼滤波 KF和EKF的公式对比(基本没差别) 二.扩展卡尔曼五个公式 利用扩展卡尔曼滤波估计四元数. 下图是论文中的截图.可以和前面的卡尔曼滤波估计高度文章的那五个公式对应一下. 观测矩阵 ...
- 基于STM32F407四旋翼无人机 --- 姿态解算讲解(四元数)(叉积法融合陀螺仪数据和加速度数据)(五)
基于STM32F407四旋翼无人机 --- 姿态解算讲解(五) 姿态解算 姿态解算定义 欧拉角 四元数 四元数性质 方向余弦矩阵 四元数方向余弦矩阵 叉积法融合陀螺仪数据和加速度数据 叉积运算 一阶龙 ...
- 基于STM32的四旋翼无人机项目(二):MPU6050姿态解算(含上位机3D姿态显示教学)
前言:本文为手把手教学飞控核心知识点之一的姿态解算--MPU6050 姿态解算(飞控专栏第2篇).项目中飞行器使用 MPU6050 传感器对飞行器的姿态进行解算(四元数方法),搭配设计的卡尔曼滤波器与 ...
- Pixhawk之姿态解算篇(4)_补充篇
一.开篇 大家期待已久的第四篇来了,但是本篇可能比较水啊~~~见谅~~~ 首先,上一周没有什么收获,虽然看了不少的论文,但是却没有什么质的飞越~~~~ 看的论文都是关于姿态解算的,用的算法大部分也都是 ...
- Pixhawk之姿态解算篇(3)_源码姿态解算算法分析
一.开篇 终于到ardupilot源代码的姿态解算了,有了前期关于mahony姿态解算算法的基础以后,理解源代码的姿态解算算法就快多了,所有的东西都在脑海中初步有了一个框架:首先要做什么,然后再做什么 ...
- 匿名飞控TI版_姿态解算
匿名飞控TI版_姿态解算 准备电赛 准备大创 先看看匿名姿态解算的代码 文章目录 匿名飞控TI版_姿态解算 一,姿态解算原理 1.介绍 2.方向余弦矩阵 (1)方向余弦 (2)DCM矩阵 3.欧拉角 ...
- android 陀螺仪滤波_Arduino MPU6050陀螺仪运用卡尔曼滤波姿态解算实验
Arduino MPU6050陀螺仪运用卡尔曼滤波姿态解算实验 版权声明:本文为博主原创文章,未经博主允许不得转载. 2019年3月20日 发布 实例效果 输出效果: 首先看看本例程XYZ轴的输出效果 ...
最新文章
- 你也可以设计简单的内容管理系统
- sklearn pipeline_我的sklearn学习经验
- 分模块开发创建父工程——(六)
- 设计一个有getMin功能的栈 (python)
- 额,看房没戴头盔,损失二十万 。。。
- php上传文件损坏,PHP 上传文件故障排除
- R7-4 检查密码 (15 分)
- python和tableau优缺点_matplotlib和Tableau之间哪一个最好?
- Gartner数据:RPA以75.6%增长率成2019年Q1增速最快的企业级软件(附全球十大RPA市场数据)
- Codeforces - 102222H - Fight Against Monsters - 贪心
- MATLAB算法实战应用案例精讲-【智能优化算法】蝙蝠算法-BA(附MATLAB和Python代码)
- PPT做的很好却不会讲?掌握这些职场PPT演讲技巧,不再惧怕做汇报
- JsonMappingException: Problem deserializing 'setterless' property '': get method returned null
- Xcode 8.0 Beta发布,详解Swift语言的重大变化
- bom成本分析模型_拆解苹果HomePod发现BoM成本高达248.4美元
- 作为3-5年的iOS开发者:你为什么迟迟进不去大厂呢?
- 亲密关系科学(04)让人快速爱上你的36问
- 电脑连不上ishanghai_ishanghai电脑版下载-ishanghai电脑客户端v5.2.3 最新版 - 极光下载站...
- C语言--指针作为数组参数获取数组平均数
- 转载: 一个正在渡劫的小老弟
热门文章
- kddcup99预处理matlab,KDD CUP99数据集预处理(Python实现)-Go语言中文社区
- CUIT J2EE 第二次课内容
- 【RT-Thread】STM32的UART设备读取GPS数据
- python递归版算阶乘算法
- 《数据结构(C++语言版)》邓俊辉第3版 教材全部代码(取最优方案)及部分重要课后习题代码 第1章 绪论
- 3星|《财经》2018年第5期:西伯利亚冻土层的猛犸象牙是合法的,一根能卖到数万美元...
- Java SE 7 新增特性
- 样机模板素材哪家强?Top4都在这!
- 用通达信破解接口指标公式判断股市牛熊的三个方法
- CSS3简单特效--animation实现流光按钮