robot_pose_ekf源码解读
卡尔曼滤波理论
下面几个网址对卡尔曼滤波算法有详细的描述:
Kalman Filter For Dummies
How a Kalman filter works, in pictures
SLAM中的EKF,UKF,PF原理简介
卡尔曼滤波与扩展卡尔曼滤波的重要区别:
卡尔曼滤波只能针对线性模型,包括线性运动模型、线性观测模型并且随机误差满足高斯分布。当处理的模型是非线性的,比如运动方程是非线性的,那么从上一次状态(用高斯分布描述)经过转换矩阵到当前状态就不再是高斯分布能描述的了。而到底用什么分布函数来描述都很难说的清。对于线性模型来说,上一次状态用高斯分布来描述,线性转换后的状态依然可以用高斯分布来描述。扩展卡尔曼滤波是实现了一种将非线性模型线性化的方法。除此之外,其它运算过程与卡尔曼滤波是一致的。扩展卡尔曼滤波的思路是:
1)在工作点附近,用泰勒展开式去线性近似。泰勒展开式的高阶项被忽略了所以得到下面的公式。
ps:图片来自https://www.cnblogs.com/gaoxiang12/p/5560360.html
这一点在代码上的实现可以参考来自robot_pose_ekf包中的一段代码。
namespace BFL
{using namespace MatrixWrapper;NonLinearAnalyticConditionalGaussianOdo::NonLinearAnalyticConditionalGaussianOdo(const Gaussian& additiveNoise): AnalyticConditionalGaussianAdditiveNoise(additiveNoise,NUMCONDARGUMENTS_MOBILE),df(6,6){// initialize df matrixfor (unsigned int i=1; i<=6; i++){for (unsigned int j=1; j<=6; j++){if (i==j) df(i,j) = 1;else df(i,j) = 0;}}}NonLinearAnalyticConditionalGaussianOdo::~NonLinearAnalyticConditionalGaussianOdo(){}ColumnVector NonLinearAnalyticConditionalGaussianOdo::ExpectedValueGet() const //更新预测步骤的状态量,这里实现了x_t = f(x_t-1){ColumnVector state = ConditionalArgumentGet(0);ColumnVector vel = ConditionalArgumentGet(1);state(1) += cos(state(6)) * vel(1);//xstate(2) += sin(state(6)) * vel(1);//ystate(6) += vel(2);//yawreturn state + AdditiveNoiseMuGet();}Matrix NonLinearAnalyticConditionalGaussianOdo::dfGet(unsigned int i) const //得到Jacobian矩阵{if (i==0)//derivative to the first conditional argument (x){double vel_trans = ConditionalArgumentGet(1)(1);double yaw = ConditionalArgumentGet(0)(6);df(1,3)=-vel_trans*sin(yaw); //求x关于yaw的偏导df(2,3)= vel_trans*cos(yaw);//求y关于yaw的偏导return df;}else{if (i >= NumConditionalArgumentsGet()){cerr << "This pdf Only has " << NumConditionalArgumentsGet() << " conditional arguments\n";exit(-BFL_ERRMISUSE);}else{cerr << "The df is not implemented for the" <<i << "th conditional argument\n";exit(-BFL_ERRMISUSE);}}}}//namespace BFL
2)将预测步骤和测量更新步骤的误差项用均值为0的高斯分布近似替代。
关于协方差矩阵的几点理解:
1)协方差用于描述两个变量之间线性相关的强度。
2)在机器人的应用中,一般都是用多维度的高斯分布来描述机器人的状态。其均值是一个状态列向量。如(xyθ)\left(\begin{array}{l}{x} \\ {y} \\ {\theta}\end{array}\right)⎝⎛xyθ⎠⎞是用坐标x,y和偏航角来描述机器人的状态。而各维度的方差用协方差矩阵来表征。协方差矩阵统一描述不同维度间的协方差。它是一个对称矩阵,而且对角线是各个维度上的方差(表征对应维度样本数据的离散程度)。
3)当某维度的协方差为0时说明该维度上的 数据一点都不离散,是完全不波动的数据。在后面计算卡尔曼增益K=P′HTS−1K=P^{\prime} H^{T} S^{-1}K=P′HTS−1时,因为P′P^{\prime}P′某维度等于0,则计算的该维度的卡尔曼增益KKK也将为0。根据卡尔曼滤波算法公式x=x′+KyP=(I−KH)P′\begin{array}{l}{x=x^{\prime}+K y} \\ {P=(I-K H) P^{\prime}}\end{array}x=x′+KyP=(I−KH)P′
更新的状态量与上一次的状态量保持一致,协方差也保持和上一状态的一致。由此可以看出此时卡尔曼滤波是不起作用的。
4)当观测模型协方差矩阵对角线中某一维度的元素非常大,如9999.0,则滤波器将会忽略掉该维度的测量数据。下面是推理步骤:
R=[∞]S=HP′HT+RS=∞K=P′HTS−1K=P′HT(∞)−1K=0x=x′+Kyx=x′\begin{array}{l}{R=[\infty]} \\ {S=H P^{\prime} H^{T}+R} \\ {S=\infty} \\ {K=P^{\prime} H^{T} S^{-1}} \\ {K=P^{\prime} H^{T}(\infty)^{-1}} \\ {K=0} \\ {x=x^{\prime}+K y} \\ {x=x^{\prime}}\end{array} R=[∞]S=HP′HT+RS=∞K=P′HTS−1K=P′HT(∞)−1K=0x=x′+Kyx=x′
最后会发现后验状态与先验状态保持一致,即完全忽略了测量数据。
理解协方差的参考网站:
详解协方差与协方差矩阵
深入理解协方差矩阵
浅谈协方差矩阵
BFL库
bfl是内置在ROS系统中的一个贝叶斯滤波器包,而robot_pose_ekf功能包是对bfl的ros封装。所以要分析robot_pose_ekf包必须对bfl有所了解。
源码下载地址https://github.com/ros-gbp/bfl-release.git
需要注意该仓库的upstream分支才有bfl库的源代码,master分支为robot_pose_ekf包的源代码。
下面的文章对bfl库的使用作了介绍。
C++中贝叶斯滤波器包bfl的使用(1)
C++中贝叶斯滤波器包bfl的使用(2)
C++中贝叶斯滤波器包bfl的使用(3)
BFL库的文件结构
examples文件夹中包含了几个使用案例。robot_pose_ekf中会使用到的非线性模型的建立方法在里面也有体现。tests文件夹中包含了测试用例子。
src文件夹中包含了bfl库的主要源码。
1)filter文件夹中实现了众多基于贝叶斯思想的滤波算法,其中就包括扩展卡尔曼滤波和直方图滤波。
filter.cpp文件实现了滤波器的虚基类,统一了各滤波器操作的接口。kalmanfilter.cpp文件的KalmanFilter类继承了filter,cpp中的Filter类,实现了卡尔曼滤波算法的大部分运算。extendedkalmanfilter.cpp文件中的ExtendedKalmanFilter类继承了KalmanFilter类,并在SysUpdate和MeasUpdate方法中分别实现了运动模型和观测模型的相关运算。不管实例化一个卡尔曼滤波还是扩展卡尔曼滤波都是使用ExtendedKalmanFilter类来定义,只是实例化时传入的模型分了线性模型和非线性模型。
2)pdf文件夹实现了多种概率密度分布函数,其中就包括了卡尔曼滤波中用到的高斯分布函数。下图显示了相关类的继承关系。
3)model文件夹中主要是对pdf文件夹中的概率分布函数进行包装,抽象出线性/非线性预测模型和线性/非线性观测模型,并提供了操作的接口。
4)wrappers文件夹中实现了矩阵的多种运算操作,避免了对其他库的依赖。
robot_pose_ekf
1)robot_pose_ekf节点可以接收odom、IMU、vo(视觉里程计)和GPS的数据。每类数据的接收均在相应接收话题的回调函数里。
void OdomEstimationNode::odomCallback(const OdomConstPtr& odom)
void OdomEstimationNode::imuCallback(const ImuConstPtr& imu)
void OdomEstimationNode::voCallback(const VoConstPtr& vo)
void OdomEstimationNode::gpsCallback(const GpsConstPtr& gps)
2)节点中定义了一个定时器,用于周期性的执行卡尔曼滤波。下面的函数就是整个滤波循环。
// filter loopvoid OdomEstimationNode::spin(const ros::TimerEvent& e)
3)节点的执行流程图
注意,确定先验位姿所用的传感器是可选的。
4)在OdomEstimation类的构造函数里实例化了预测模型和4种传感器的测量模型。其中只有预测模型是非线性的。针对该非线性模型,在nonlinearanalyticconditionalgaussianodo.cpp文件中的ExpectedValueGet方法实现了扩展卡尔曼算法中的x′=f(x)x^{\prime}=f(x)x′=f(x),即状态预测更新。dfGet方法求得了线性化所需的Jacobian矩阵。在该类的update方法中执行了所有模型的更新操作。
PS:因水平能力有限,上述内容可能有理解不到位或不正确的地方。欢迎在下面的留言区给我留言。希望我写的东西也能帮到大家:)
关注公众号《首飞》回复“机器人”获取精心推荐的C/C++,Python,Docker,Qt,ROS1/2,机器人学等机器人行业常用技术资料。
robot_pose_ekf源码解读相关推荐
- Bert系列(二)——源码解读之模型主体
本篇文章主要是解读模型主体代码modeling.py.在阅读这篇文章之前希望读者们对bert的相关理论有一定的了解,尤其是transformer的结构原理,网上的资料很多,本文内容对原理部分就不做过多 ...
- Bert系列(三)——源码解读之Pre-train
https://www.jianshu.com/p/22e462f01d8c pre-train是迁移学习的基础,虽然Google已经发布了各种预训练好的模型,而且因为资源消耗巨大,自己再预训练也不现 ...
- linux下free源码,linux命令free源码解读:Procps free.c
linux命令free源码解读 linux命令free源码解读:Procps free.c 作者:isayme 发布时间:September 26, 2011 分类:Linux 我们讨论的是linux ...
- nodeJS之eventproxy源码解读
1.源码缩影 !(function (name, definition) { var hasDefine = typeof define === 'function', //检查上下文环境是否为AMD ...
- PyTorch 源码解读之即时编译篇
点击上方"AI遇见机器学习",选择"星标"公众号 重磅干货,第一时间送达 作者丨OpenMMLab 来源丨https://zhuanlan.zhihu.com/ ...
- Alamofire源码解读系列(九)之响应封装(Response)
本篇主要带来Alamofire中Response的解读 前言 在每篇文章的前言部分,我都会把我认为的本篇最重要的内容提前讲一下.我更想同大家分享这些顶级框架在设计和编码层次究竟有哪些过人的地方?当然, ...
- Feflow 源码解读
Feflow 源码解读 Feflow(Front-end flow)是腾讯IVWEB团队的前端工程化解决方案,致力于改善多类型项目的开发流程中的规范和非业务相关的问题,可以让开发者将绝大部分精力集中在 ...
- spring-session源码解读 sesion
2019独角兽企业重金招聘Python工程师标准>>> spring-session源码解读 sesion 博客分类: java spring 摘要: session通用策略 Ses ...
- 前端日报-20160527-underscore 源码解读
underscore 源码解读 API文档浏览器 JavaScript 中加号操作符细节 抛弃 jQuery,拥抱原生 JS 从 0 开始学习 GitHub 系列之「加入 GitHub」 js实现克隆 ...
最新文章
- Linux 命令行界面-GUI界面
- NVIDIA 显卡与 CUDA 在深度学习中的应用
- effective C++ 条款 3:尽可能使用const
- leetcode-卡车加气走环
- 【Flink】Flink 源码之Buffer Debloating
- OpenGL基础49:高度贴图(下)
- 我愿意为你在城里的月光下温暖冷冷的爱情
- python开发一个PC屏幕监控软件(2000块的道德底线)
- 浙江学生头戴“金箍”,上课走神自动通知家长
- 关于fixed元素的【子父div】宽度问题
- Automatic detection and segmentation of optic disc and fovea in retinal images
- 聊聊我在职场的至暗时刻!
- Java游戏开发框架LGame-0 2 8版发布(含JavaSE及Android版,已有文档)
- HtmlEncode是做什么的?
- 【摄影技术基础】图像处理之图像采集
- Inno Setup入门(十五)——Inno Setup类参考(1)
- win7 sp2 好神奇
- python计算cos_Python cos() 函数 - Python 教程 - 自强学堂
- 美团校招-2023.3.18.10点-第四题-商店-困难
- 不要想在任何事上都占上风