1.项目代码功能经验证ok,确保稳定可靠运行。欢迎下载使用!
2.主要针对各个计算机相关专业,包括计科、信息安全、数据科学与大数据技术、人工智能、通信、物联网等领域的在校学生、专业教师或企业员工使用。
3.项目具有丰富的拓展空间,不仅可作为入门进阶,也可直接作为毕设、课程设计、大作业、初期项目立项演示等用途。
4.当然也鼓励大家基于此进行二次开发。在使用过程中,如有问题或建议,请及时私信沟通。
5.期待你能在项目中找到乐趣和灵感,也欢迎你的分享和反馈!
【资源说明】
EKF扩展的卡尔曼滤波器源码(C++和python两个版本)+项目说明.zip
软件架构
项目分两个目录C++和python,分别对应于两个开发语言版本;
**一. python目录:**
1.tinyekf
是一个简单的ekf算法基类,其源码来自于上面参考文献4.
本人对其代码做了必要的中文注释和简单的结构调整;
其测试程序为: kalman_mousetracker.py
2.RobotEKF
这是一个从tinyekf继承来的子类.模拟一个小车,小车运动的加速度和方向由鼠标提供,
根据小车的运动学方程分析,对应于卡尔曼滤波方程扩展了F_k,增加了B_k(控制矩阵)和u_k(控制向量).
运行效果和tinyekf差不多,但鼠标移动过快时,加速度a过大,模拟小车就会飞出屏幕外.
总的来说模拟的效果并不好.但主要目的是扩展tinyekf,学习引入运动学方程;
其测试程序为: ekf_mouseRobot.py
3.AltitudeDataFusion
这是一个模拟飞机飞行时,基于气压计,陀螺仪(或IMU),GPS确定飞机海拔高度的例子;
也就是卡尔曼滤波器对三个传感器的数据融合;
主要目的有两个:
1>.学习多传感器数据融合;
2>.学习传感器非线性数据线性化处理了过程,即构建雅各比矩阵过程;
其测试程序为: ekf_AltitudeDataFusion.py
其测试数据效果图为: ekf_AltitudeDataFusion.xlsx
**二. C++目录:**
此目录是一个ROS功能包:xtark_study(此包名无意义,自己学习用的).
背景介绍:
先介绍ROS系统下的robot_pose_ekf 扩展卡尔曼滤波算法包这个包用于评估机器人的 3D 位姿,使用了来自不同源的位姿测量信息,它使用带有 6D(3D position and 3D orientation)模型信息的扩展卡尔曼滤波器来整合来自轮子里程计, IMU 传感器和视觉里程计的数据信息。 基本思路就是用松耦合方式融合不同传感器信息实现位姿估计。
该功能包依赖于一个BFL(贝叶斯老滤波库),robot_pose_ekf的主要滤波算法也是由BFL完成.
出于学习的目的,自己想写一个自己的滤波器,用于完成BFL一样的滤波功能;
于是工作开始了:
第一步: ekf/TinyEKF.cpp 把上面python版本tinyekf用C++语言重新以便,作为EKF核心基类;
第二步: 为了先测试,编译了一个和上面python版本类似的多传感器数据融合计算海拔高度的例子: AltitudeDataFusion4Test.
GPS海拔高度,大气压值,IMU惯性传感器海拔高度,三者数据融合,得到最优海拔高度
扩展子类: test/AltitudeDataFusion4Test.cpp
可执行main测试: test/test_AltitudeDataFusion.cpp
融合数据图形化效果: test/ekf_AltitudeDataFusion.xlsx
注: 如不在ros环境, 可以运行此测试样例. 但要注意对orocos-bfl库的依赖,因为用到了其中的SymmetricMatrix, Matrix, ColumnVector等
第三步: 也是重点
1. 开发针对位姿估计的ekf子类: OdomEstimationEKF.cpp 我的滤波器的主要代码这在此类及其基类中.
设计思路上和robot_pose_ekf中的BLF有所不同:
* BLF采用松耦合的方式为每种传感器(odom和imu)设计了独立的PDF(概率密度函数),并独立
设计了sysModel,及各传感器的测量模型mesurementModel. 获取到传感器数据后,也是各部分独立计算后,汇总到总的estimate状态中;
* 我的EKF思路则相对简单,设计了一个整体的状态方程模型和测量值模型.
2. 开发ROS系统节点(node)入口程序(main),包括ros初始化,odom,imu等消息的订阅,tf发布等.
需要说明的事,此入口程序,直接复制了ROS系统自带的