semrush/江西seo推广软件

上一篇文章介绍了互补滤波器与ROS的接口定义，这篇文章将结合论文分析代码。

complementary_filter.cpp

首先从成员变量开始看，毕竟这些变量在后面用到的时候需要了解他所代表的意思，同时也要与论文中的变量一一匹配。

下面5个量都是硬编码在类里面的常量，基本不需要更改。
const double ComplementaryFilter::kGravity = 9.81; 定义的重力常量
const double ComplementaryFilter::gamma_ = 0.01; 这个暂时不清楚
const double ComplementaryFilter::kAngularVelocityThreshold = 0.2; 达到稳态时的角速度阈值
const double ComplementaryFilter::kAccelerationThreshold = 0.1; 达到稳态时的线加速度阈值
const double ComplementaryFilter::kDeltaAngularVelocityThreshold = 0.01; 达到稳态时的角加速度阈值

下面的量主要是可以通过launch文件调整的parameters。
double gain_acc_; 加速度计的增益，默认0.01
double gain_mag_; 地磁计的增益，默认0.01
double bias_alpha_; 偏差估计的增益，默认0.01
bool do_bias_estimation_; 是否进行偏差估计，默认是true
bool do_adaptive_gain_; 是否使用自适应增益，默认是true

下面的量是类内部的主要变量，记录滤波器内的工作状态。
bool initialized_; 是否初始化成功，标记是不是第一次得到数据
bool steady_state_; 是否达到稳态
double q0_, q1_, q2_, q3_; 四元数，q0是实数项
double wx_prev_, wy_prev_, wz_prev_; 上一个循环的角速度值
double wx_bias_, wy_bias_, wz_bias_; 角速度偏差

在成员变量之后我们再来看成员函数，其中有很多set和get开头的都是对变量值进行访问和设定，内容比较简单这里不展开了。之后找到最主要的函数update()，这个函数有2个多态，分别是根据6轴或9轴数据来进行滤波，由于我们现在没有考虑地磁计的数据，因此后面的函数展开也都是依据6轴的update函数来进行。先看代码：

d

这个代码已经把pipeline写的非常清晰了：

1. 判断是否是第一个循环，如果是第一个循环则只getMeasurement，这个函数只用到了3轴的线加速度信息，根据当前加速度的分量状态，估算出当前的姿态，之后直接进入下一个循环，同时以后也不会再进入该函数读取加速度计的信息，而是靠后面的函数来更新状态。
2. 这里我们默认进行偏差估计，那么就进入updateBiases函数。
3. 再根据角速度的信息预测下一刻的姿态，使用getPrediction函数。
4. 再之后依据线加速度的数据去修正预测的姿态，使用getAccCorrection函数。
5. 这里我们默认使用自适应增益，则进入到getAdaptiveGain函数来调整加速度计的增益。
6. 最后是三步计算的步骤，scaleQuaternion对修正后的四元数进行scale。
7. 四元数乘积计算quaternionMultiplication来获得期望的四元数数据。
8. 最终对四元数进行归一化计算，使用normalizeQuaternion函数。

完成这些步骤之后滤波器便更新了一轮数据，之后再由complementray_filter_ros里面调用滤波器的getOrientation函数来输出更新好的四元数。之后便一个函数一个函数的看起来，顺便和论文中的内容结合一下。互补滤波简单来说就是由文章中的这句话来表述：

For attitude estimation from IMU readings, a complementary filter performs high-pass filtering on the orientation estimated from gyroscope data affected by low-frequency noise, and low-pass filter on accelerometer data affected by high-frequency noise. The fusion between the two filtered estimations will ideally obtain an all-pass and noise-free attitude estimation. The term “complementary” derives from the cut-off frequency value, which is the same for both filters. Thus, its correct value is found as a trade-off between the preserved bandwidth of each single signal.

也就是说对于陀螺仪这种以低频噪声为主的传感器施加高通滤波，对于加速度计这种以高频噪声为主的传感器施加低通滤波，最后再加权求平均来保证在全频域方面的低噪声，这也是“互补”这一说法的来源。这个噪声按照我自己的理解是这样的：因为陀螺仪只有在你进行旋转的时候产生的数据才是有用的，而保持一个固定姿态不动时它读取的数据实际上是一个零点的误差，因为需要对角速度求积分那么这样的误差会累积的非常大，这种误差（噪声）也就是在你的系统不动的状态下一直存在的，那么就是一个频率很低的偏差，因此需要高通滤波器来消除。同样的道理，由于重力加速度的存在，由加速度计计算出来的姿态角会长时间保持与重力加速度一致的数据，但是在系统运动时会暂时性地产生加速度分量的变化，这一变化通常不会持续很长时间，因此可以说加速度计在很长一段时间内的数值是正确的而短时间内可能产生较大误差，那么就要通过施加低通滤波器来消除这种高频噪声。

论文当中前半部分对四元数的概念进行了比较基础的介绍，同时也介绍了如何通过加速度计和地磁计来估算当前姿态的，这个对于我这种惯导小白来说还是蛮不错的。因为我觉得代码应该是论文的实现方式，从代码来看能够更有逻辑性地理解论文中的内容，因此就按照上面介绍的pipeline看起，

1. getMeasurement

这个函数的意义就是将加速度计测到的重力分量进行归一化，然后根据z轴重力分量的正负情况计算当前姿态的四元数。函数里简单粗暴的根据a.z的符号来判断，这也要求车辆的imu安装方向要正确并且尽量不要出现45度仰望天空的装法吧。。。虽然一般来说是应该不会出现的。以az>0的情况为例，四元数的计算如下，其中ax，ay，az是一个单位向量的三个分量，这个向量是由加速度计的计算结果经过归一化之后得到的。话说我是没太看懂这个函数是怎么算的，不过结合论文可以看到这一步基本就是实现下面的公式（论文中公式25，有推导）。

q0_meas 

这样就可以根据当前加速度计的结果计算当前的姿态，这里插一句，从我一个C++新手的角度看这个源代码还是挺赏心悦目的，函数的输入输出都说的很明确可读性也很强。而我自己写的函数经出现doSomething（），然后在里面对类成员变量一顿操作，最后我自己都不知道这个函数输出了什么。。。

2. updateBiases

这个函数在现在的阶段还没有起到实质作用，它本身就是把稳态的角速度误差减掉了，这样保证输出的rostopic当中角速度基本是一个0左右的值。checkState便是检查是否达到了稳态，如果没有达到稳态则保留当前值并在下一循环继续计算，如果达到了问题则把稳态误差输出出来，并且在下一个循环时将稳态误差与测量值乘以系数后累积到稳态误差当中，类似于一个积分控制器用来最终消除稳态误差。

void 

3. getPrediction

这个函数是基于当前的角速度测量值进行预测。首先减去稳态误差，之后根据角速度与之前加速度计估算的姿态进行下一个时间步长下位姿估算。这里的q0_等是类的成员变量，在getMeasurement一步当中已经被赋予了的新的数值，而由于getMeasurement仅在第一个循环使用，因此在后续的循环当中这个q0_等数据则是表示滤波器当前认可的姿态状态。这里的这个公式我也一头雾水，不过可以查看论文的推导过程（公式37-42）。公式里的欧米伽便是代码中的w，tk-1是当前值，tk是预测值，圈积的算法在论文中有说明。

void 

4. getAccCorrection

这个函数在也比较短小，其中主要的是一些四元数的运算，这个函数基本和第一步的getMeasurement有些类似，是先通过归一化之后的线加速度向量估算对之前预测姿态四元数的修正，p0等四元数是上一步的预测结果，之后通过rotateVectorByQuaternion公式计算出gx，gy，gz分量，并根据这个向量的值计算其对应的四元数。原文中有个说明，说通过加速度计对预测的四元数进行修正主要是针对roll和pitch的，而如果要对yaw进行修正则需要引入地磁计的数据，这个应该就是dq3一项取0的原因。如果查看getMagCorrection函数可以看到dq3是非0的而dq1和dq2是0。

这一部分的核心思路是这样的，上一步已经得到了预测的姿态四元数，之后依据这个四元数对当前加速度计测出来的重力分量的向量进行逆运算，也就是假设我已经知道了下一步的姿态以及当前时刻的重力分量向量，那么通过让重力分量按照下一步的姿态反转之后就会得到一个预测好的下一步的重力分量向量的理论值，之后再通过这个理论值结合与公式25类似的计算方式得到一个delta四元数，这个数值会比较下，用来描述一个把预测的重力分量向量旋转到真实策略的重力分量向量的四元数。

void 

5. getAdaptiveGain

这个函数会求出一个自适应的增益值，这个值会在后面的函数当中使用，如果不启用自适应增益则这个增益直接使用gain_acc_这个参数的值。输入的变量alpha最初就是来自gain_acc_，之后根据当前加速度计计算的重力合力的大小评估一个误差，再依据误差计算增益值，如果误差越大增益则越小，证明加速度计的参数不可信。

论文中给出的解释是当车辆进行加速减速运动时其加速计的数据幅值与真实重力加速度差别较大，但是如果这个时候仍旧使用固定增益的话会导致姿态估计产生偏差；同时陀螺仪在计算姿态变化时不会受到运动状态的影响，因此在发生明显加减速运动时应该更多依赖陀螺仪的数据，这也就是为什么提出来自适应增益这样的方法。

double 

6. scaleQuaternion

这个函数是依据第四步计算出的delta四元数展开计算，这一步是一个scale down的过程，论文原文描述这段内容是这样的：

As the delta quaternion is affected by the accelerometer’s high frequency noise, before applying it to the predicted quaternion, we scale it down by using an interpolation with the identity quaternion qI.

从中能够看出该互补滤波的方法并不是直接使用一个低通滤波器来过滤加速度计产生的高频噪声，而是对预测得到的四元数进行插值并降低其幅值，主要就是通过上一步计算出来的gain值来控制，使其能够长时间累积而不会受短时间加速突变的影响。这个Slerp和Lerp都是对四元数进行插值的方法，具体算法可以参考论文公式图4。见下图。

这个dq0<0.0的判断在代码和论文当中不同，论文当中是当dq0<0.9时便使用Slerp方法因为该方法能给出一个更加准确的在球面上的插值结果，而代码当中将其设为0.0，那也就是只使用Lerp方法进行插值，我想可能是因为想要提高计算效率吧。论文中提到这个gain值实际上就是低通滤波器的cutoff frequency，这个我觉得有些抽象，不是很能理解，不过论文针对这句话也给出了reference [35]来解释，有兴趣的同学可以看看。最后对dq0等四元数进行归一化计算使其变成单位四元数。

void 

7. quaternionMultiplication

这个函数实际上只是一个数学计算过程，依照论文的公式53可以找到这一步的依据。由于两个四元数的圈积就是两次旋转，第一次按照第一个数，第二次按照第二个数，因此这个函数也就是将第3步得到的预测的姿态和第6步得到的delta四元数以此进行旋转，之后得到最终的预测出来的姿态。

回想一下第2步的bias estimation实际上就是高通滤波，将由陀螺仪产生的稳态误差进行消除，之后第3步由滤波后的陀螺仪数据计算预测的姿态，也就是圈积的第一项。而第4，5，6这三步则是对于加速度计进行一个低通滤波，之后把每次预测后的重力方向与真实测量得到的重力方向进行比对并计算一个修正旋转四元数，这个四元数会长时间内保证预测的重力方向与真实的重力方向一致，同时由于scaleQuaternion函数的原因使得其高频噪声得以抑制。

8. normalizeQuaternion

最后一步便是将两次旋转之后的四元数再进行归一化操作，供外面封装的ros节点读取并放到IMU消息当中发布出去。

以上便是对ros-imu-tools的complementary_filter的源码和论文解析，大部分的内容也都是比较粗浅，尤其是涉及到理论推导的部分恕我能力有限无法一步步分析，而写这篇文章的主要目的也还是记录一下自己学习的过程，如果能对大家有所帮助那更是锦上添花了，同时如果我的描述之中有任何疏漏之处也请指正，以免误导他人。谢谢！