我是靠谱客的博主 体贴砖头,这篇文章主要介绍robot_localization中ekf滤波推导过程robot_localization中ekf滤波推导过程,现在分享给大家,希望可以做个参考。

robot_localization推导过程

  • robot_localization中ekf滤波推导过程
    • 坐标系确定
    • 姿态角更新方法
    • 线性化求解雅克比矩阵

robot_localization中ekf滤波推导过程

融合里程计和imu、GPS的robot_localization里面的ekf推导过程,我将其推导完成,并将其记录如下:
15个状态量
在这里插入图片描述
状态预测方程: 在这里插入图片描述,非线性,用泰勒展开线性化。
融合原理:将多种传感器的数据融合到里程计坐标系下,坐标系为初始时刻车的方向和原点,即前左上,但是这里的速度x y z,仍然是和沿着车辆方向的。

坐标系确定

欧拉角能用来描述任意旋转,但最有意义的是使用笛卡尔坐标系(右手坐标系)并按照一定顺序所组成的旋转序列。最常用的约定,即所谓“Yaw-pitch-roll”约定,当我们说到旋转的顺序是“Yaw-pitch-roll”时,是指从导航坐标系到载体坐标系,如果从载体坐标系到导航坐标系则相反。若坐标系选择为右手坐标系时,旋转方向与坐标轴指向之间符合右手法则时欧拉角取正,反之取负。
这里从载体坐标系到导航坐标系 roll-pitch-yaw,旋转顺序为X-Y-Z,这和北东地下一样的,那么
由 系到 系的转换过程可描述为:
在这里插入图片描述

各次基本旋转(由b系到n系)对应的坐标变换矩阵分别为:
在这里插入图片描述

按照上述旋转顺序,三维空间内b系到n系的旋转矩阵(矩阵左乘)为:
在这里插入图片描述

根据姿态矩阵的定义, 即为载体的姿态矩阵,其计算结果为:
在这里插入图片描述
这个公式和文章《A Generalized Extended Kalman Filter Implementation for the Robot Operating System》一致。可查阅。

姿态角更新方法

姿态角更新有很多方法,这里用了欧拉角法:
载体坐标系下角速率可以表示为:
在这里插入图片描述
姿态更新过程如下
在这里插入图片描述

线性化求解雅克比矩阵

预测方程是非线性方程,利用泰勒展开将其进行线性化处理。泰勒展开在ekf中的应用原理为
在这里插入图片描述在这里插入图片描述
在这里插入图片描述
针对robot_localization的方程式线性化过程为:
在这里插入图片描述
在这里插入图片描述
雅克比矩阵为:
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
整个过程要结合robot_localization代码一起看,才能更明白

最后

以上就是体贴砖头最近收集整理的关于robot_localization中ekf滤波推导过程robot_localization中ekf滤波推导过程的全部内容,更多相关robot_localization中ekf滤波推导过程robot_localization中ekf滤波推导过程内容请搜索靠谱客的其他文章。

本图文内容来源于网友提供,作为学习参考使用,或来自网络收集整理,版权属于原作者所有。
点赞(57)

评论列表共有 0 条评论

立即
投稿
返回
顶部