本文的多传感器融合是建立在读懂《Quaternion kinematics for the error-state
Kalman ?lter》基础上的 ，是一种相机和IMU融合的理论，里面讲解了IMU的误差状态运动方程构建。误差状态四元数，是有开源的程序的，但是它是集成在rtslam里面的，不方便提取出来使用。
但还有另外一个开源的程序，ETH的MSF，可以比较方便地用在自己的工程里面，并且它的理论与误差状态四元数很接近，稍微有点不同，所以MSF开源程序就成了一个不错的选择。
所以本人研究了ETH的两篇文章：《Vision Based Navigation for Micro
Helicopters》和《A Robust and Modular Multi-Sensor Fusion
Approach Applied to MAV Navigation》
2. 算法理论
我们使用IMU含噪声的测量值和bias去计算估计状态量X。并且没有考虑测量值的噪声项和扰动，从而产生了误差。我们把所有不确定性引来的误差放在误差变量δx中考虑，并推导了误差状态运动模型，以此求解Fd状态转移矩阵和P过程协方差矩阵
利用其它来源的数据如GPS,视觉等来作为矫正（使误差可观）。通过估计状态量X计算Hj，利用观测量和估计量计算残差，最后计算K，来更新P和误差变量δx，最后更新状态量X。
3. 核心代码分析
4. 代码实战
roslaunch msf_updates
position_pose_sensor.launch
roslaunch msf_updates viconpos_sensor.launch
rosrun rqt_reconfigure rqt_reconfigure#初始化卡尔曼滤波器,很重要!!
如下界面,需要点击filter
|
