MSF详细解读与使用

转载自：https://www.cnblogs.com/ilekoaiq/p/9311357.html

相机IMU四部曲(三)融合:MSF详细的解释和使用

极品巧克力

前言

通过前两篇文章，《D-LG-EKF相机和IMU整合的所有理论都被推导出来了。此外，《误差状态四元数》还指导了实际操作中可能遇到的一些情况。

这些理论都比较完整，那么，如何在实践中操作呢？如何应用于实际产品？误差状态为4元，有开源程序，但集成在一起rtslam（ https://www.openrobots.org/wiki/rtslam/ ）内部，不方便提取使用。

但是还有另一个开源程序，ETH的MSF（https://github.com/ethz-asl/ethzasl_msf），它是一个独立的开源程序，可以更方便地用于自己的项目，它的理论和误差状态非常接近，有点不同，所以MSF开源程序成了不错的选择。

所以，我把MSF所有所有程序后，结合程序推导MSF成本文总结理论，包括MSF与大家分享实验。

1.基本模型

如下图所示。

MSF新的传感器可以在程序中连接，例如GPS，激光雷达、码盘等。

主要理论是误差状态四元数中的理论。

与《卡尔曼四元数带外参融合法》相比，其方法更好。

首先，它的核心状态在世界坐标系下没有表示重力。因为它使用的是《误差状态四元数》中的5元.3.1的方法是直接以水平坐标系为世界坐标系。只是根据当前加速度计的测量值和理论重力计算初始位置，初始位置只有协方差。这种方法更方便，因为它最终将转换为水平坐标系。

而且，IMU世界坐标系和相机世界坐标系不需要同时绑在一起建立。可以在IMU打开一段时间后，相机将获得其世界坐标系。但是，间隔应该尽可能短，因为位移取决于IMU时间长了，加速度计的积分会很不准确，如果有轮子码盘，那就更好了。调整的目的可能是因为世界坐标系和相机的世界坐标系在时间戳上不对应，所以需要微调。

或者，为了避免这样的麻烦，直接把它拿走IMU世界坐标系与相机世界坐标系绑在一起，同时建立。

也应该用ETH手眼标定法，借棋盘格先标定，再固定。直接用尺子测量，固定。如果使用双目相机，则不需要考虑。

(如果加上轮速计，如何更新？它的功能和IMU是一样的，都是相对值，而不是绝对值。因此，它应该与IMU主状态与相机实时融合后，IMU预测一个位置，轮速计预测一个位置，然后将两者融入主状态。只有主状态才能与相机位置融为一体。如果集成，可以用误差状态的思想来集成。

所以，参考《误差状态四元数详细解读》，一个简单鲁棒的IMU与相机融合的模型应该是这样的。

核心状态是，。

核心状态的干扰是。

运动模型和《误差状态四元数》一样，

则相机位姿的观测方程为，

使用《误差状态四元数》中的公式，

其中，

首先，计算雅克比的第一项，

雅克比计算如下：

然后，第二项，要转成三个元素的形式，即角轴李代数的形式。

用matlab解决上面的公式和雅克比。

syms qicw qicx qicy qicz thetax thetay thetaz ...

qwiw qwix qwiy qwiz qzocw qzocx qzocy qzocz real

q1=[qicw,-qicx,-qicy,-qicz]';

q2=[1,-1/2*thetax,-1/2*thetay,-1/2*thetaz]';

q3=[qwiw,-qwix,-qwiy,-qwiz]';

q4=[qicw,qicx,qicy,qicz]';

q5=[qzocw,qzocx,qzocy,qzocz]';

temp = quaternion_mul(q1,q2);

temp = quaternion_mul(temp,q3);

temp = quaternion_mul(temp,q4);

temp = quaternion_mul(temp,q5);

temp

J=jacobian(2*temp(2:4，[thetax,thetay,thetaz])

simplify(J)

雅克比可以，把temp转换成角轴，然后关于求导。或者，角轴直接近似等于temp向量部分的两倍，然后是求导，就像上面一样matlab程序这样。

虽然这个计算是准确的，但它太麻烦了。在论文中也使用了类似的方法。为了方便地寻求雅克比，测量值的近似被直接转换为预测值，即。

此时，上式转换为角轴，即向量部分的两倍，即，。因此，雅克比是，

(这种用类似的方法计算雅克比，虽然不如从原来的公式推导准确，但可以大大简化计算，也许可以给出D-LG-EKF在计算H矩阵时进行参考。

因此，得到了雅克比矩阵。

然后，。可以计算。其余流程与《误差状态四元数》相同。

2.延迟处理

前提是每个传感器的时间戳是同一时间源的，或者时间戳是稳定的，可以通过一些方法找到时间戳之间的对应关系。不同传感器的时间戳可以准确对应。

然后，由于测量值有时需要一段时间才能处理，因此记录滤波器中的状态，然后在测量值过来时更新相应时间的状态。然后继续向后预测。如果有多个不同传感器的测量值，也是如此。

3.计算相对测量

单目相机要集成。考虑到单目尺度，我担心有时尺度会突然改变。为了应对这种情况，计算相对测量，即两个时刻之间的相对位置，这样这一段的规模就可以更准确地过滤。然后，将优化值添加到原始状态。该方法与误差状态四元数中的传播相似，即将新过滤的位置的平均值和扰动添加到原始状态的平均值和扰动中，整合新的平均值和扰动。

如果要集成轮子码盘，就不用用这种方法了。因为轮子码盘虽然有尺度问题，但尺度比较稳定。

假如是双目相机，也不必考虑这种尺度的突然变化。

4.准确计算每帧的协方差

为了更好地与GPS融合需要准确的协方差。

之前计算的相机位置协方差不准确，不考虑累积误差的影响。IMU融合后的状态协方差也不准确。每帧图像的协方差必须准确计算。这种协方差是通过全局BA计算出的方法。

但实际上是为了简化。假如真的要和。GPS融合的话。

在即时构图的情况下，使用上一段的方法。但是视觉误差的积累还是相当可观的，所以如果是远距离，应该是GPS对于外部绝对测量，视觉只用于计算相对测量。短距离视觉只能相对测量。在主状态之后，视觉的相对测量值和IMU集成相对测量值，集成相对测量状态，然后将相对状态的平均值和扰动合并到主状态和主状态，然后与主状态合并GPS整合，整合新的主状态。如果融合，可以用误差状态的思想来融合。

如果图纸已经提前建成，环检测已经完成，地图点已经固定准确，那么视觉协方差就是当前帧的协方差，不需要通过全局BA算出。

5. MSF方法总结

MSF该方法综合考虑，该理论框架可用于多种传感器的集成。

以上思考的方法，还考虑了轮速计码盘，GPS整合的具体操作，以后需要时再使用。

6.实验与改进

MSF本文可本文，

http://www.liuxiao.org/2016/07/ros-卡尔曼多传感器集成框架-ethzasl-msf-framework-编译与使用/。

然后，我自己用ORBSLAM2跑Euroc的V201数据集生成轨迹数据IMU一起发送数据MSF运行。为什么要运行标准数据集？因为标准数据集提供了IMU噪声的真实参数可以直接使用，并且有真实的轨迹，groundtruth，可用于评价融合结果的质量。

然而，操作结果总是发散的，融合后的轨迹锯齿非常严重。但从理论上讲，它应该能取得更好的效果吗？因此，猜测应该是程序与理论不对应。

我们必须从程序中检查问题的原因。在逐一对应程序中的所有计算过程和算法公式后，最终发现它是由程序中的两个地方引起的。

1.MSF程序有一个隐含的假设，即图像的世界坐标系是水平的。我以第一帧为世界坐标系，V201的第一帧不是水平的。

2. 仔细推导程序的计算过程后，发现MSF程序中的qwv，本质上是qvw，这导致参数的初始值错误。

改变以上两个问题后，MSF我自己提供的数据可以正常运行。

y轴上的结果如下。

以上，黑线是真实值，绿线是真实值ORBSLAM通过图像计算的位置，红线是图像位置和IMU在整合后，在线的每个点都代表一个输出数据。msf融合后的结，不仅可以把位姿的输出频率提高到和IMU一样的频率，还可以让轨迹更加接近真实值。

但是，msf有一个缺点，那就是IMU的bias收敛得很慢，猜测是由于近似造成的。如下图所示。

上图是加速度计的bias的收敛情况，和陀螺仪的bias的收敛情况。也许可以通过修改这部分的公式，让bias收敛得更快。

7.参考文献

1. Lynen S, Achtelik M W, Weiss S, et al. A robust and modular multi-sensor fusion approach applied to MAV navigation[C]// Ieee/rsj International Conference on Intelligent Robots and Systems. IEEE, 2013:3923-3929.