依据imu姿态角计算z轴倾角_树莓派小车-07-IMU姿态解算 imu_complementray_filter

上一篇文章介绍了互补滤波器与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轴的线加速度信息，根据当前加速度的分量状态，估算出当前的姿态，之后直接进入下一个循环&#x