IMU 姿态解算

分类于 ， ， 阅读次数： Waline： 本文字数： 7k 阅读时长 ≈ 25 分钟

IMU 通过加速度计解算姿态角

通过三角函数可以将加速度计三个轴的角速度解算为姿态角，其中 \(\alpha\) , \(\beta\) 和 \(\gamma\)（ \(\gamma\) 是 z 轴与重力加速度之间的夹角）与三个轴之间的关系如上图所示：

\[ \begin{aligned} &\beta = arcsin(\frac{a_x}{g}) \\ &\gamma = arcsin(\frac{a_y}{g}) \end{aligned} \]

其中重力加速度 $ g $ 的取值使用三轴角速度的矢量和即：

\[ g = \sqrt{a_x^{2} + a_y^{2} + a_z^{2}} \]

将 g 的值带入上式可以得到：

\[ \begin{aligned} &\beta = arctan(\frac{a_x}{\sqrt{a_y^{2} + a_z^{2}}}) \\ &\gamma = arctan(\frac{a_y}{\sqrt{a_x^{2} + a_z^{2}}}) \end{aligned} \]

其中 α 为俯仰角 pitch，β 为滚转角 roll，其中航向角 yaw 是没有办法通过加速度计来计算的。

陀螺仪积分得到角度

陀螺仪积分最直接的就是将陀螺仪输出的角速度与时间相乘做累加实现对角速度的积分得到角度，数据精度较高但是漂移很严重。在积分算法上常用龙格库塔法计算积分。

数据融合

一阶互补滤波

推导

角速度计和陀螺仪都可以解算得到姿态角，如下：

\[ \begin{aligned} &z_1 = x + u \\ &z_2 = x + v \end{aligned} \]

其中 \(z_1\) 和 \(z_2\) 分别是加速度通过三角函数计算得到的欧拉角和陀螺仪积分得到的欧拉角，\(x\) 是真实欧拉角，\(u\) 和 \(v\) 分别是测量噪声符合高斯噪声其中 \(u\) 为高频噪声 v 是低频噪声。\(x\) 的估计值为：

\[ \begin{aligned} &\hat{x} = k_1 \times z_1 + k_2 \times z_2 \\ &k_1 + k_2 = 1 \end{aligned} \]

将 k1、k2 用一阶滤波传递函数表示：

\[ \begin{aligned} &k_1(s) = \frac{1}{fs + 1} \\ &k_2(s) = \frac{fs}{fs + 1} \end{aligned} \]

其中 s 是拉普拉斯变换的 s 域即频域，\(\frac{1}{f}\) 是截止频率。于是得到：

\[ \hat{x}(s) = k_1(s)z_1(s) + k_2(s)z_2(s) = \frac{1}{fs+1}z_1(s) + \frac{fs}{fs+1}z_2(s) \]

根据拉普拉斯变换微分定理变换公式：

\[ L[\frac{d^{n}f(t)}{dt^{n}}] = s^{n} F(s) \]

用拉普拉斯反变换回去得到：

\[ f\dot{\hat{x}}(t) + \hat{x}(t) = z_1(t) + f\dot{\hat{z_2}}(t) \]

离散化后得到：

\[ f\frac{\hat{x}(k) - \hat{x}(k-1)}{dt} + \hat{x}_k = z_1(k) + f\frac{\hat{z_2}(k) - \hat{z_2}(k-1)}{dt} \]

化简整理后得：

\[ \hat{x}(k) = \frac{f}{f+dt}[\hat{x}(k-1) + z_2(k) - z_2(k-1)] + \frac{dt}{f+dt}z_1(k) \] 取 \[ K=\frac{dt}{f+dt} \]

得到：

\[ \hat{x}(k) = (1-K)[\hat{x}(k-1) + z_2(k) - z_2(k-1)] + K z_1(k) \]

这就是一阶互补滤波的最终公式。

将上面推导出的公式写成代码如下：

1
2
3
4
5
6
7
8
9
10
11
/**
 * @brief 一阶互补滤波器单轴
 * @param acc_angle 通过三角函数计算加速度计的值得到的角度值
 * @param gyro_rate 陀螺仪输出的角速度值
 * @param K1 滤波器系数，远小于 1
 * @return 返回融合后角度值
 */
static void firstOrderComplementaryFiltering(float acc_angle, float gyro_rate, float* angle)
{
  *angle = K * acc_angle + (1 - K) * (*angle + gyro_rate * dt);
}

调参

陀螺仪采样率是 1kHZ，dt 为 0.001 则算法涉及的参数只有一个就是 K 参数，假如取截止频率为 100 的话：

\[ K = \frac{dt}{f + dt} = \frac{0.001}{\frac{1}{100} + 0.001} \doteq 0.1 \]

kalman 滤波

原理

卡尔曼滤波包括预测和更新两个过程，下面是两个过程的详细推导：

预测过程

预测阶段主要是根据陀螺仪数据预测角度值，系统状态量是角度值和角速度偏差，而不是角度值和角速度值：

\[ \begin{aligned} &\hat{x}(k|k-1) = \hat{x}(k-1|k-1) + (\acute{\theta} - \acute{\theta }_b) \bigtriangleup t + w_\theta \\ &\acute{\theta }_b(k|k-1) = \acute{\theta }_b(k-1|k-1) + w_{\acute{\theta }_b} \\ &w(k) = \begin{bmatrix}w_{\theta } &w_{\acute{\theta}_b}\end{bmatrix} \end{aligned} \]

其中 \(\acute{\theta}_b\) 是角度偏差， \(\acute{\theta }\) 是角速度， \(w(k)\) 是预测过程噪声符合高斯噪声写成矩阵的形式：

\[ \hat{x}(k|k-1) = F\hat{x}(k-1|k-1) + B\acute{\theta }(k) +W(K) \tag{1} \\ F = \begin{bmatrix}1 & - \bigtriangleup t\\ 0&1 \end{bmatrix} \\ B=\begin{bmatrix}\bigtriangleup T \\ 0\end{bmatrix} \]

将矩阵写成展开形式：

\[ \begin{bmatrix}\theta_k\\ \acute{\theta }_b\end{bmatrix} = \begin{bmatrix}1 & -\bigtriangleup t\\ 0&1 \end{bmatrix}\begin{bmatrix}\theta_{k-1}\\ \acute{\theta }_{k-1}\end{bmatrix} + \begin{bmatrix}\bigtriangleup t\\ 0\end{bmatrix}\acute{\theta }_k + w_k \]

1
2
3
4
5
6
7
/*
 * kalman 公式一、角度的先验状态估计 x(k|k-1) = F * x(k-1|k-1) + B* u
 * x 为角度和角速度的列向量 F=（[1,-dt],
 *                           [0, 1]）
 */
 gyro_rate-=q_bias;
 *angle+=gyro_rate * dt;

协方差矩阵的更新如下： 协方差矩阵为向量$ \begin{bmatrix}_k\ _b\end{bmatrix}$的协方差矩阵，该协方差矩阵会随着系统变换而变换，其原理是一个矩阵作用到向量后的矩阵协方差求法，如下：

\[ \begin{aligned} &Cov(x) = \Sigma \\ &cov(Ax) = A\Sigma A^{T} \\ cov(Ax)& = E[(Ax - E[Ax])(Ax - E[Ax])^{T}] \\ &= E[Ax -AE[x](Ax - AE[x])^{T}] \\ &= E(A(x-E[x])(x-E[x])^{T}A^{T}) \\ &= AE[(X-E[x])(X-E[x])^{T}]A^{T} \\ &= A\Sigma A^{T} \end{aligned} \]

故协方差方程为：

\[ P_{expect}(K|K-1) = FP(K-1|K-1)F^{T} + Q_k \tag{2} \]

这个方程表达的是新的协方差由上一次的协方差通过矩阵变换而来且再加上外部干扰，而协方差矩阵表征的是一个误差范围也就是不确定性，每次的不确定性由上一次的不确定性通过矩阵变换加上新的不确定性而来。将矩阵写为展开形式并化简得到：

\[ \begin{aligned} \begin{bmatrix}P_{00} &P_{01} \\ P_{10}& P_{11}\end{bmatrix}_{k|k-1} &= \begin{bmatrix}1 & -\bigtriangleup t\\ 0&1 \end{bmatrix}\begin{bmatrix}P_{00} &P_{01} \\ P_{10}& P_{11}\end{bmatrix}_{k-1|k-1}\begin{bmatrix}1 & 0\\ -\bigtriangleup t&1 \end{bmatrix} + \begin{bmatrix}Q_\theta & 0\\ 0&Q_{\acute{\theta }} \end{bmatrix}\bigtriangleup t \\ &=\begin{bmatrix}P_{00}-\bigtriangleup tP_{10} &P_{01}-\bigtriangleup tP_{11} \\ P_{10}&P_{11} \end{bmatrix}_{k-1|k-1}\begin{bmatrix}1 & \\ 0-\bigtriangleup t & 1\end{bmatrix} + \begin{bmatrix}Q_\theta & 0\\ 0&Q_{\acute{\theta }} \end{bmatrix}\bigtriangleup t \\ &=\begin{bmatrix}P_{00}-\bigtriangleup tP_{10}-\bigtriangleup t(P_{01}-\bigtriangleup tP_{11}) &P_{01}-\bigtriangleup tP_{11} \\ P_{10}-\bigtriangleup tP_{11}&P_{11} \end{bmatrix}_{k-1|k-1} + \begin{bmatrix}Q_\theta & 0\\ 0&Q_{\acute{\theta }} \end{bmatrix}\bigtriangleup t \\ &=\begin{bmatrix}P_{00}-\bigtriangleup tP_{10}-\bigtriangleup t(P_{01}-\bigtriangleup tP_{11}) +Q_\theta \bigtriangleup t&P_{01}-\bigtriangleup tP_{11} \\ P_{10}-\bigtriangleup tP_{11}&P_{11}+Q_{\acute{\theta }}\bigtriangleup t \end{bmatrix} \\ &=\begin{bmatrix}P_{00}-\bigtriangleup t(\bigtriangleup tP_{11}-P_{10}-P_{01} +Q_\theta )&P_{01}-\bigtriangleup tP_{11} \\ P_{10}-\bigtriangleup tP_{11}&P_{11}+Q_{\acute{\theta }}\bigtriangleup t \end{bmatrix} \end{aligned} \]

\(Q_k\) 是过程噪声协方差矩阵，由于加速度计的误差与陀螺仪之间的误差是相互独立的，不存在相关性（实际使用的时候角度值每次都是使用上次的最优值因此两者实际是有一定的相关性的），因此该矩阵是一个对称矩阵。其中陀螺仪的误差为累积误差，与时间\(\bigtriangleup t\)有关，其表达式如下：

\[ Q_k = \begin{bmatrix}Q_\theta & 0\\ 0& Q_{\acute{\theta }_b}\end{bmatrix}\bigtriangleup t \\ \]

其中 \(Q_{\acute{\theta }_b}\) 表征了陀螺仪的漂移误差， \(Q_\theta\) 表征了姿态角的误差。

1
2
3
4
5
6
7
/*
 * kalman 公式二、预测协方差矩阵 P(k|k-1) = F * P(k-1|k-1) * F' + Q(k)
 */
 P[0][0] += (Q_angle - P[0][1] - P[1][0] + P[1][1] * dt) * dt;
 P[0][1] += (-P[1][1]) * dt;
 P[1][0] += (-P[1][1]) * dt;
 P[1][1] += Q_gyro * dt;

更新过程

观测值 \(z_k\) 为：

\[ z_k = H * x_k + v_k \]

这里 \(z_k\) 为观测值，是通过加速度计解算的姿态角，\(H\) 为观测矩阵这里只能获取角度值，不能直接获取角度偏差值因此 \(H=[\begin{bmatrix}1 &0 \end{bmatrix}]\)（当然如果你将原始的加速度计值输入到 kalman 方程里，这里的 \(H\) 就是由加速度值到姿态角的变换矩阵，这里提前将加速度解算为姿态角后输入到 kalman 方程），\(v_k\) 为测量噪声符合高斯分布。

\[ z_k \sim N(0,R) \]

本系统中测量值只有一个角速度值，因此\(R\)可以用一个方差来表示。而加速度计解算姿态角是与时间无关的，该误差不会被累加到系统因此 \(v_k = v\) , 这个值设置的比较大代表测量噪声比较大，则系统相信预测值大一些，如果这个值设置的比较小代表测量噪声很小，相信测量值大一些在本系统中加速度计解算的姿态角代表测量值也就是相信加速度值大一些。 观测量与预测量一样有一个对不确定性的更新，就是预测过程的协方差，测量过程的协方差基于上一次的协方差来计算，如下：

\[ P_{measure}(k|k) = H*P(k|k-1)H^{T} + R \]

将其写成矩阵展开形式并化简得到如下：

\[ P_{measure}(k|k) =\begin{bmatrix}1 &0 \end{bmatrix} * \begin{bmatrix}P_{00} &P_{01} \\ P_{10} &P_{11} \end{bmatrix}_{k|k-1} * \begin{bmatrix}1\\ 0\end{bmatrix} + R = P_{00 k|k-1} +R \]

1
2
3
4
5
6
/*
 * 中间变量 E = H * P(k|k-1) H' + R
 */
 PCt_0 = C_0 * P[0][0];
 PCt_1 = C_0 * P[1][0];
 E = R_angle + C_0 * PCt_0;

测量过程的协方差结果就是一个数值（看到这应该很庆幸不用对矩阵进行求逆，这个参数其实就是卡尔曼增益里面求逆矩阵那一项），其实就是方差值。到这里有两个协方差，一个是预测过程的协方差另一个是测量过程的协方差，两个协方差代表了两个数据源的噪声，哪个值大代表哪个过程的噪声越大，对其信任度就会降低。 下面终于到了卡尔曼增益的计算，先看下卡尔曼增益张什么样子吧：

\[ K = H_k*P_k*H^{T}_k (H_k*P_k*H^{T}_k + R)^{-1} \]

这个公式看起来非常不友好，其实卡尔曼系数是将上面通过预测和测量得到的两个高斯方程相乘而得到的。具体可以参考以下链接，其中之一是中文翻译版： 英文原版链接 How a Kalman filter works, in pictures 中文翻译版：详解卡尔曼滤波原理 为了这里公式的完整性，还是决定把与之相关的内容搬过来吧，如下图：

这张图是原作者的图但是有些不准确，蓝色的融合后的曲线应该是更高的曲线才对。将红色和绿色两个高斯方程相乘可以得到蓝色曲线代表的高斯方程，这就是估计的最优值。先看一维数据下的高斯方程相乘的结果如下：

\[ \begin{aligned} &{\mu }' = \mu_0 + \frac{\sigma_0 ^{2} (\mu_1 - \mu_0)}{\sigma_0 ^{2} + \sigma_1 ^{2}} \\ &{\sigma}'^{2} = \sigma_0^{2} - \frac{\sigma_0^{4}}{\sigma_0 ^{2} + \sigma_1 ^{2}} \end{aligned} \]

取

\[ k = \frac{\sigma_0^{2}}{\sigma_0 ^{2} + \sigma_1 ^{2}} \]

得到：

\[ \begin{aligned} &{\mu }' = \mu_0 + k(\mu_1 - \mu_0) \\ &{\sigma}'^{2} = \sigma_0 ^{2} - k \sigma_0 ^{2} \end{aligned} \]

用 \(\Sigma\) 表示协方差，写成矩阵的形式如下：

\[ \begin{aligned} &K = \Sigma_0 (\Sigma_0 + \Sigma_1)^{-1} \\ &\vec{\mu'} = \vec{\mu_0} + K (\vec{\mu_1} - \vec{\mu_0}) \\ &{\Sigma}' = \Sigma_0 - K \Sigma_0 \end{aligned} \]

将我们前面推导的预测和测量带入到上面的公式就得到下面卡尔曼滤波的后三个公式如下：

\[ \begin{aligned} &K = H_k * P_k * H^{T}_k (H_k * P_k * H^{T}_k + R)^{-1} \\ &H_k * \hat{x}_{k|k} = H_k * \hat{x}_{k|k-1} + K (z_k - H_k * \hat{x}_{k|k-1}) \\ &H_k * {P}'_{k|k} * H^{T}_k = H_k * P_{k|k} * H^{T}_k - K * (H_k * P_{k|k} * H^{T}_k) \end{aligned} \]

将这三个方程联系起来做运算得到如下更新方程：

\[ K_k = P_{k|k-1} * H^T * P^{-1}_{measure}(k|k) \tag{3} \]

将其写为矩阵展开形式并化简得到：

\[ \begin{aligned} \begin{bmatrix}K_0\\ K_1\end{bmatrix}_k &= \begin{bmatrix}P_{00} &P_{01} \\ P_{10}&P_{11} \end{bmatrix}_{k|k-1} * \begin{bmatrix}1\\ 0\end{bmatrix} * P^{-1}_{measure}(k|k) \\ &=\begin{bmatrix}P_{00}\\ P_{10}\end{bmatrix}_{k|k-1} * P^{-1}_{measure}(k|k) \end{aligned} \]

1
2
3
4
5
/*
 * kalman 公式三、K = P(k|k-1) * H' / (H * P(k|k-1) H' + R)
 */
 K_0 = PCt_0 / E;
 K_1 = PCt_1 / E;

协方差的更新：

\[ P_{k|k} = (I - K_k* H_k)P_{k|k-1} \tag{4} \]

将矩阵写为展开形式并化简得到如下：

\[ \begin{aligned} \begin{bmatrix}P_{00} & P_{01} \\ P_{10} & P_{11}\end{bmatrix}_k &= (\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix} -\begin{bmatrix}K_0\\ K_1\end{bmatrix}_k * \begin{bmatrix}1 & 0 \end{bmatrix}) * \begin{bmatrix}P_{00} & P_{01} \\ P_{10} & P_{11}\end{bmatrix}_{k-1} \\ &=(\begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix} -\begin{bmatrix}K_0 &0 \\ K_1 &0 \end{bmatrix}_k) * \begin{bmatrix}P_{00} & P_{01} \\ P_{10} & P_{11}\end{bmatrix}_{k-1} \\ &=\begin{bmatrix}P_{00} & P_{01} \\ P_{10} & P_{11}\end{bmatrix}_{k-1} - \begin{bmatrix}K_0P_{00} &K_0P_{01} \\ K_1P_{00} &K_1P_{01} \end{bmatrix} \end{aligned} \]

1
2
3
4
5
6
7
8
9
/*
 * kalman 公式四、P(k|k) = (I - K * H) * P(k|k-1)
 */
 t_0 = PCt_0;
 t_1 = C_0 * P[0][1];
 P[0][0] -= K_0 * t_0;
 P[0][1] -= K_0 * t_1;
 P[1][0] -= K_1 * t_0;
 P[1][1] -= K_1 * t_1;

下面是最优估计值

\[ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K * (z_k - H_k * \hat{x}_{k|k-1}) \tag{5} \]

写成矩阵的展开形式并化简如下：

\[ \begin{bmatrix}\theta \\ \acute{\theta_b}\end{bmatrix}_{k|k} = \begin{bmatrix}\theta \\ \acute{\theta_b}\end{bmatrix}_{k|k-1} + \begin{bmatrix}K_0\\ K_1\end{bmatrix}_k * (z_k - \theta_{k|k-1}) \]

1
2
3
4
5
6
7
8
9
10
11
/*
 * 中间变量 角度残差 z(k) - H * x(k)
 * H = ([1,0])
 */
 angle_err = acc_angle - *angle;

/*
 * kalman 公式五、x(k|k) = x(k|k-1) + K * (z(k) - H * x(k))
 */
 *angle += K_0 * angle_err;
 q_bias += K_1 * angle_err;

以上就是卡尔曼滤波的预测和更新过程。但是我们前面在说卡尔曼系数的时候是将两个卡尔曼方程相乘得到了最优解，这里有个问题就是这种方式得到的值是不是合理，或者说卡尔曼滤波是不是无偏估计的，这里贴出网上的一个推导，自行查看。https://www.zhihu.com/question/331568328/answer/735287556

调参

卡尔曼系数\(K\)表征了对预测量和测量量的信任程度，如下式：

\[ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K * (z_k - H_k * \hat{x}_{k|k-1}) \]

这个式子中 \(H_k\) 只是选取了角度信息去掉了加速度值，暂时可以忽略掉不看，当 \(K\) 为 \(0\) 时，最优值完全取预测值即完全相信预测值是真实值不相信测量值，当\(K\)取 1 时最优值取\(z_k\)测量值，则完全相信测量值而不相信预测值。\(K\)越大则越相信测量值，\(K\)越小越相信预测值。\(K\)的值动态调整预测和测量之间的比例因子。对于我们的系统中一开始相信陀螺仪数据多一些，随着时间不短累积则相信加速度计数据逐渐增多，最后会收敛到一个固定的值。 接下来是系统里的几个关键参数 \(Q\) 和 \(R\)，这里详细看下具体每个参数该怎么选取： 参数 \(Q\) 的角度误差的方差 \(Q_{\theta }\) ，该参数是预测角度的噪声，预测主要是基于上一次的最优值对陀螺仪数据进行积分得到预测值，该值代表了陀螺仪的整体误差，调试参数的时候该值一般设的比较小，对陀螺仪的信任度比较大。 角速度误差 \(Q_{\acute{\theta }}\) 就是陀螺仪的漂移方差，这个值反应的是陀螺仪漂移的方差，即在每次的漂移量都是符合高斯噪声的，但是这个值跟环境温度等相关，在不同温度下漂移值是不同的可以对温度漂移进行多次测量修正，还有跟陀螺仪的运动状态有关，例如在一个系统中陀螺仪固定像一个方向旋转，则可以在陀螺仪以固定角速度旋转下测量该漂移。整体 Q 代表的是预测噪声。 测量噪声 \(R\) 是代表了实际数据与测量值之间的误差，传感器不能 100%获取到实际物理量数据测量一定是有误差的，该值可以通过 IMU 的 datasheet 获取相关参数。例如 icm20602 的加速度误差为 \(100ug/\sqrt{hz}\) 则对于 1kz 的采样率则为 \(100ug * \sqrt{1000}\) , 但是这个值不能直接作为加速度计的观测噪声，还要看具体场景，加速度计的平移运动会产生加速度值，而这个值对计算角度来讲就是噪声，所以对于本系统 \(R\) 不仅仅是传感器本身误差，同时还包括 IMU 的平移运动产生的误差。在一个非常平稳的系统中加速度计的各轴平移加速度非常小，甚至可以忽略，此时加速度计的噪声是非常小的可以近似等于传感器测量噪声，相反在一个震动频率很高的场景下，加速度计的平移加速度就非常大，这个时候\(R\)的取值就应该比传感器的本身误差大很多。手册上的误差值代表了最理想情况下的误差，实际系统中应该根据具体情况去调整这个值。 整体调试过程中，\(R\)值的选取跟收敛速度相关，该值取的很小则收敛速度越快，因为长期来看滤波的值是像加速度计的值进行收敛的，对加速度的信任度越高则收敛越快反之则收敛越慢，但是对加速度计的信任度越高就会导致滤波效果不好有毛刺，\(Q\)和\(R\)的值共同决定了滤波效果。在实际的调参过程中将加速度计的曲线和滤波的曲线同时输出，对比两条曲线的关系就可以看到滤波的平滑程度以及收敛速度了。具体效果可以以缓慢旋转 IMU，快速旋转 IMU，不做旋转而左右晃动 IMU 来看滤波后的值是否是我们期望的。

mahony 滤波

陀螺仪模型

\[ \omega = \hat{\omega} + b_g + n_g \]

这里， \(\omega\) 是陀螺仪测得的角速度， \(\hat{\omega}\) 是我们希望恢复的潜在理想角速度， \(b_g\) 陀螺仪偏差会随时间和温度等其他因素而变化， \(n_g\)是陀螺仪高斯白噪声。

加速度计模型

\[ a = R^{T}(\hat{a} - g) + b_a + n_a \]

这里\(a\)是加速度测量值，\(\hat{a}\) 是加速度真实值，\(R\)是传感器在世界坐标系下的转换矩阵，\(g\) 是重力加速度， \(b_a\) 是随时间和温度等其他因素而变化的 acc 偏差， \(n_a\) 是白色高斯 acc 噪声。

Mahony 算法流程

在开始讨论 mahony 滤波器公式之前，让我们正式定义将要使用的坐标轴。让 \(I,W,B\) 分别表示惯性，世界和机体坐标系。通常 \(B\) 和 \(I\) 是相同的。下标表示源坐标系，上标表示目的坐标系。下图是 mahony 滤波器的主要流程：

步骤一：获取传感器值

从传感器获取陀螺仪和 acc 测量值。让 \(I_{w_t}\) 和 \(I_{a_t}\) 分别表示陀螺仪和 acc 测量值。\(I_{\hat{a}_t}\) 表示标准化的 acc 测量。 代码如下：

1
2
3
4
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

步骤二：使用 acc 测量计算误差

根据先验估计值利用 acc 计算误差：

\[ \begin{aligned} &v(\hat{_{I}^{W}\textrm{}q_{est}},t) = \begin{bmatrix}2(q_2q_4-q_1q_3)\\ 2(q_1q_2+q_3q_4)\\ (q_1^{2} - q_2^{2} - q_3^{2} + q_4^{2})\end{bmatrix} \\ &e_{t+1} = I_{\hat{a_{t+1}}} \times v(\hat{_{I}^{W}\textrm{}q_{est}},t) \\ &e_{i,t+1} = e_{i,t} + e_{t+1}\Delta t \end{aligned} \]

\(\Delta t\) 是采样间隔，即 t 到 t+1 的时间间隔。看到这里是不是一头雾水，当时我看到这里是完全没搞懂这是什么啊，没关系，还有望不到尽头的路子要走呢，继续 先放下前面那一堆乱七八糟的东西，看点简单的东西，about 复数和四元数

如果有一个复数 \(z = a + bi\)，那么 \(z\) 与任意一个复数 \(c\) 相乘都会将 \(c\) 逆时针旋转 \(θ = atan2(b, a)\) 度，并将其缩放 \(|z| = \sqrt{a^{2} + b^{2}}\) 倍．如果 \(|z| =1\) 那么久完全变成旋转了，\(z\)也就变成了旋转矩阵如下：

\[ z = \begin{bmatrix}cos(\theta ) &-sin(\theta) \\ sin(\theta)& cos(\theta)\end{bmatrix} \]

则对应的旋转公式（矩阵型）如下：

\[ {v}' = \begin{bmatrix}cos(\theta ) &-sin(\theta) \\ sin(\theta)& cos(\theta)\end{bmatrix} v \]

其中 z 可以用复数形式来表示即\(z = x + yi\)构造一个\(z = cos(\theta) + sin(\theta)i\)复数，则旋转可以表示为

\[ {v}' = zv = (cos(\theta) + i sin(\theta))v \]

这是二维的情况，推广到 3D 空间中任意一个 \(v\) 沿着单位向量 \(u\) 旋转 \(θ\) 角度之后的 v′ 为：

\[ v′ = cos(θ)v + (1 − cos(θ))(u · v)u + sin(θ)(u × v) \]

接着给出四元数旋转公式任意向量 \(v\) 沿着以单位向量定义的旋转轴 \(u\) 旋转 \(θ\) 度之后的 \(v′\) 可以使用四元数 乘法来获得．令 \(v = [0, v]，q = \begin{bmatrix}cos(\frac{1}{2}\theta ), sin(\frac{1}{2}\theta )u \end{bmatrix}\)，那么：

\[ {v}' = qvq^{*} = qvq^{-1} \]

换句话说，如果我们有 \(q = [cos(θ), sin(θ)u]\)，那么 \(v′ = qvq^{*}\) 可以将 \(v\) 沿着 \(u\) 旋 转 \(2θ\) 度。写为矩阵形式如下： 任意向量 \(v\) 沿着以单位向量定义的旋转轴 \(u\) 旋转 \(θ\) 角度之后的 \(v′\) 可以使用矩阵 乘法来获得．令 \(a =cos(\frac{1}{2}\theta ), b = sin(\frac{1}{2}\theta )u_x, c = sin(\frac{1}{2}\theta )u_y, d = sin(\frac{1}{2}\theta )u_z\) 那么：

\[ {v}' = \begin{bmatrix}1-2c^2-2d^2 & 2bc - 2ad & 2ac+2bd \\ 2bc+2ad & 1-2b^2-2d^2 &2cd - 2ab \\ 2bd-2ac & 2ad+2cd & 1-2b^2-2c^2 \end{bmatrix}v \]

其中这里的四元数是归一化四元数即\(1 = q_1^2 + q_2^2 + q_3^2 + q_4^2\)并用更通用的符号来表示则上面式子可以写为如下：

\[ v = \begin{bmatrix}q_1^2 + q_2^2 -q_3^2 -q_4^2 &2(q_2q_3 - q_1q_4) &2(q_2q_4 + q_1q_3) \\ 2(q_2q_3 + q_1q_4) & q_1^2 - q_2^2 + q_3^2 -q_4^2 & 2(q_3q_4 - q_1q_2) \\ 2(q_2q_4 - q_1q_3)& 2(q_3q_4 + q_1q_2) & q_1^2 - q_2^2 -q_3^2 +q_4^2 \end{bmatrix}v \]

这里其实是省略了相关推导，其中的详细推导过程我会在下面给出链接，这里我们直接使用这些已知的结论，不然东西太多，看着太累（其实是写起来太累）。到这里其实只是列出了四元数的旋转公式，好回到前面第二步中我们那个公式是怎么来的呢，下面看机体坐标系下的重力表示，将机体坐标系的三轴分量通过旋转矩阵的旋转后就是地理坐标系下的重力分量表示。而我们这里的旋转矩阵是正交矩阵即 \(M*M^T = I\)则有\(g = M*\hat{g}\) 则 \(\hat{g} = M^T * g\) 展开了写就是如下形式

\[ \begin{aligned} \hat{g} &= \begin{bmatrix}q_1^2 + q_2^2 -q_3^2 -q_4^2 &2(q_2q_3 - q_1q_4) &2(q_2q_4 + q_1q_3) \\ 2(q_2q_3 + q_1q_4) & q_1^2 - q_2^2 + q_3^2 -q_4^2 & 2(q_3q_4 - q_1q_2) \\ 2(q_2q_4 - q_1q_3)&2(q_3q_4 + q_1q_2) & q_1^2 - q_2^2 -q_3^2 +q_4^2 \end{bmatrix}' * \begin{bmatrix}0\\ 0\\ 1\end{bmatrix} \\ &=\begin{bmatrix}2(q_2q_4 - q_1q_3)\\ 2(q_3q_4 + q_1q_2)\\ q_1^2 - q_2^2 -q_3^2 +q_4^2\end{bmatrix} \end{aligned} \]

这就是上面我们第二步中的公式，在自然坐标系下的机体三轴表示。写成代码就是如下： 

1
2
3
4
5
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = imu->q1 * imu->q3 - imu->q0 * imu->q2;
halfvy = imu->q0 * imu->q1 + imu->q2 * imu->q3;
//halfvz = imu->q0 * imu->q0 - 0.5f + imu->q3 * imu->q3;
halfvz = imu->q0 * imu->q0 - imu->q1 * imu->q1 - imu->q2 * imu->q2 + imu->q3 * imu->q3;
这第二步还没完，后面还两个公式呢，先看第一个公式

\[ e_{t+1} = I_{\hat{a_{t+1}}} \times v(\hat{_{I}^{W}\textrm{}q_{est}},t) \]

这一步将加速度计获取的数据与陀螺仪准确来讲是预测值进行叉乘，来表示陀螺仪与加速度计的误差，这里为什么可以通过叉乘得到误差呢，其实是存疑的，向量的叉乘结果跟两个向量的夹角和模长相关，这里 acc 与 gyro 的数据如果是相同的那么他们之间的夹角为 0 则 \(sin(\theta) = 0\) 两个向量的叉积为 0 向量，而 acc 和 gyro 不同则两个向量不同但是笔者认为两者不能视为线性关系，叉乘的大小可以用来表示 acc 与 gyro 的误差大小，而这个计算恰好用于 pi 调节器来调节 gyro 的数据。到现在误差也出来了两个向量的叉乘，取 \(z_1 = a + bi z_2 = c + di\) 则两个向量叉乘结果为：

\[ \begin{aligned} z_1\times z_2 &= \begin{bmatrix}a &-b \\ b & a\end{bmatrix} \times \begin{bmatrix}c &-d \\ d & c\end{bmatrix} \\ &= \begin{bmatrix}ac-bd &-(bc + ad) \\ bc + ad & ac-bd\end{bmatrix} \end{aligned} \]

写为代码就是下面： 

1
2
3
4
// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
第三个公式就比较简单了对误差进行累计，这里代码跟下面第三步放到一起。第二步完。..

步骤三：对误差进行 PI 调节器调节

\[ I{w_{t+1}} = I{w_{t+1}} + K_p * e_{t+1} K_i * e_{i,t+1} \]

这个公式对于比较熟悉 pid 算法的人来讲应该是轻而易举的事了吧，关于 pid 调节这里也不是一两句话可以说的清楚的，后面专门再写一篇关于 pid 调节的文章吧，后续一定会用到的，这里先偷下懒，通俗点将这里就是对误差进行一个比例扩大或缩小即误差的积分累计运算，这个量会用于调节陀螺仪的数据，以纠正陀螺仪的累计误差让陀螺仪的数据最终收敛于加速度计数据。照例贴下代码如下： 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
	imu->integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
        imu->integralFBy += twoKi * halfey * (1.0f / sampleFreq);
	imu->integralFBz += twoKi * halfez * (1.0f / sampleFreq);
	gx += imu->integralFBx;	// apply integral feedback
	gy += imu->integralFBy;
	gz += imu->integralFBz;
}
else {
	imu->integralFBx = 0.0f;	// prevent integral windup
	imu->integralFBy = 0.0f;
	imu->integralFBz = 0.0f;
}
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;

步骤四：对陀螺仪数据进行积分

前面我们对姿态角的变化量加速度进行 pi 调节器的调节后（使其收敛于加速度计计算的重力向量）就可以进行积分了，其积分就涉及到微分方程的求解和运算，下面会给出推导步骤如下：

四元数微分方程 定义\(n\)为自然坐标系，\(b\)为机体坐标系，则\(n\)系到\(b\)系的旋转四元数为：

\[ Q = cos( \frac{1}{2}\theta ) + sin(\frac{1}{2}\theta )u^n \]

其中\(u^n\)为旋转轴，$$ 为旋转角。 将四元数 \(Q = cos(\frac{1}{2}\theta ) + sin(\frac{1}{2}\theta )u\) 对时间进行微分则：

\[ Q = cos(\frac{1}{2}\theta ) + sin(\frac{1}{2}\theta )u^n \\ \frac{dQ}{dt} = -\frac{1}{2}sin(\frac{1}{2}\theta )\cdot \frac{d\theta }{dt} + \frac{du^n}{dt}\cdot sin(\frac{1}{2}\theta ) + u^n \cdot \frac{1}{2}cos(\frac{1}{2}\theta )\cdot \frac{d\theta }{dt} \]

其中这是一个旋转四元数，其旋转轴是固定不变的，变化的是旋转角度，因此 \(\frac{du^n}{dt} = 0\) 故有：

\[ \frac{dQ}{dt} = -\frac{1}{2}sin(\frac{1}{2}\theta ) \cdot \frac{d\theta }{dt} + u^n \cdot \frac{1}{2}cos(\frac{1}{2}\theta )\cdot \frac{d\theta }{dt} \]

两边都乘以 \(\frac{1}{2} u^n \frac{d\theta }{dt}\) 得：

\[ \begin{aligned} \frac{1}{2} u^n \frac{d\theta }{dt} \otimes Q &= \frac{1}{2} u^n \frac{d\theta }{dt} * (cos(\frac{\theta }{2}) + sin(\frac{\theta }{2}) u^n) \\ & = \frac{\dot{\theta }}{2}cos(\frac{\theta }{2})u^n + u^n \otimes u^n \frac{\dot{\theta }}{2}sin(\frac{\theta }{2})r \end{aligned} \]

其中 $u^n u^n = -1 $ 得：

\[ \begin{aligned} \frac{1}{2} u^n \frac{d\theta }{dt} \otimes Q = \frac{\dot{\theta }}{2}cos(\frac{\theta }{2})u^n - \frac{\dot{\theta }}{2}sin(\frac{\theta }{2})r \end{aligned} \]

这个结果与上面推导的 \(\frac{dQ}{dt}\) 是不是一样的，于是 \(\frac{dQ}{dt}\) 可以写为如下：

\[ \frac{dQ}{dt} = \frac{1}{2} u^n \frac{d\theta }{dt} \otimes Q = \frac{1}{2} w_{nb}^{n}\otimes Q \]

式中 \(w_{nb}^{n}\) 是自然坐标系下角速度，而 IMU 获得的角速度是机体坐标系下的角速度，因此需要转换一下如下：

\[ r^n = Q \otimes r^b \otimes Q^* \]

带入上式得：

\[ \frac{dQ}{dt} = \frac{1}{2} Q \otimes w_{nb}^{b} \otimes Q^* \otimes Q \]

对于单位四元数有：\(Q^* \otimes Q = 1\) 则：

\[ \frac{dQ}{dt} = \frac{1}{2} Q \otimes w_{nb}^b \]

其中 \(w_{nb}^{b}\) 为陀螺仪测量数据，如下：

\[ w_{nb}^{b} = \begin{bmatrix}0\\ w_x\\ w_y\\ w_z\end{bmatrix} \]

由四元数乘法公式可以得到：

\[ \begin{bmatrix} \dot{q_0}\\ \dot{q_1}\\ \dot{q_2}\\ \dot{q_3}\end{bmatrix} = \frac{1}{2}\begin{bmatrix} q_0 & -q_1 & -q_2 & -q_3 \\ q_1 & q_0 & -q_3 & q_2 \\ q_2 & q_3 & q_0 & -q_1 \\ q_3 & -q_2 & q_1 & q_0 \end{bmatrix} * \begin{bmatrix}{0}\\ w_x\\ {w_y}\\ {w_z}\end{bmatrix} \]

或者：

\[ \begin{bmatrix}\dot{q_0}\\ \dot{q_1}\\ \dot{q_2}\\ \dot{q_3}\end{bmatrix} = \frac{1}{2}\begin{bmatrix} 0 & -w_x & -w_y& -w_z \\ w_x& 0 & w_z& -w_y \\ w_y& -w_z& 0 & w_x \\ w_z& w_y& -w_x& 0 \end{bmatrix} * \begin{bmatrix}{q_0}\\ q_1\\ {q_2}\\ {q_3}\end{bmatrix} \]

剩下的就是求解四元数微分方程了，求解方法有欧拉方法、毕卡算法，龙格库塔法，其中常用的是龙格库塔法求解，采用一阶龙格库塔法解算过程如下： 一阶龙格库塔公式 \(y_{n+1} = y_n + h \cdot y^{'}\) 其中 h 为步长，$y^{'} $ 为斜率。则微分方程 $ = f(t,Q) $ 可以写为：

\[ Q(t+\triangle t) = Q(t) + \triangle t \frac{dQ}{dt} \]

故可以得到：

\[ \begin{bmatrix}{q_0}\\ q_1\\ {q_2}\\ {q_3}\end{bmatrix}_{t+\triangle t } = \begin{bmatrix}{q_0}\\ q_1\\ {q_2}\\ {q_3}\end{bmatrix}_{t } + \frac{\triangle t }{2} * \begin{bmatrix} 0 & -w_x & -w_y& -w_z \\ w_x& 0 & w_z& -w_y \\ w_y& -w_z& 0 & w_x \\ w_z& w_y& -w_x& 0 \end{bmatrix} * \begin{bmatrix}{q_0}\\ q_1\\ {q_2}\\ {q_3}\end{bmatrix}_{t} \]

写为代码如下：

1
2
3
4
5
6
7
8
9
10
11
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);

最后再对四元数进行归一化，如下：

1
2
3
4
5
6
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;

至此所有理论推导都完成了，剩下的就是不停的循环上面的步骤就得到了四元数，剩下的任务就是将四元数转为欧拉角了，这里转欧拉角里面也是有坑的，需要考虑欧拉角的奇异性，在转化的过程中要注意对欧拉角的奇异性进行考虑就可以了。

参考文献

http://file.elecfans.com/web1/M00/7F/89/o4YBAFwnoJ2AGdjpACAXh7JE-_I005.pdf
https://krasjet.github.io/quaternion/quaternion.pdf
https://zhuanlan.zhihu.com/p/103623879
https://zhuanlan.zhihu.com/p/55681807
https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
https://nitinjsanket.github.io/tutorials/attitudeest/mahony
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
https://github.com/CarlyleLiu/KalmanFilter