Mahony互补滤波器代码详解

Mahony互补滤波器代码详解

在博客文章《一起深入读懂Mahony互补滤波器》（https://blog.csdn.net/jessecw79/article/details/84668189） 的基础上，本文将对Mahony互补滤波器的Matlab代码进行详细解读。本文是Jesse Chen的原创文章，如果转载请注明出处。

回顾《一起深入读懂Mahony互补滤波器》

在上一篇文章中，我们介绍了Mahony互补滤波器的推导过程和公式。如图1是一个基本SO(3)的Mahony互补滤波器的框图。

图1 Block diagram of the passive complementary filter on SO(3)

如果采用估计值$\hat{R}$作为反馈，可以得到在估计参考坐标系下的互补滤波器方程(将陀螺仪的bias考虑在内)：
$$\begin{array}{rl}\dot{\hat{R}}& =\hat{R}{\left({\Omega }^y-\hat{\mathbf{b}}+\omega \right)}_{\times }\\ \dot{\hat{\mathbf{b}}}& =-k_b\mathrm{vex}\left({\pi }_a\left(\tilde{R}\right)\right),\text{with }{k}_b>0\\ \omega & =k_{\text{est}}\mathrm{vex}\left({\pi }_a\left(\tilde{R}\right)\right),\text{with }{k}_{\text{est}}>0\\ \tilde{R}& ={\hat{R}}^{T}R\end{array}$$
假设$k_P=k_{\text{est}}$，$k_I=k_b$，那么很容易得到参考文献[1],[2],[3]中的passive complementary filter:
$$\begin{array}{rl}\dot{\hat{R}}& =\hat{R}{\left({\Omega }^y-\hat{\mathbf{b}}+k_P\omega \right)}_{\times },\hat{R}\left(0\right)={\hat{R}}_0\\ \dot{\hat{\mathbf{b}}}& =-k_I\omega ,\hat{\mathbf{b}}\left(0\right)={\hat{\mathbf{b}}}_0\\ \omega & =\mathrm{vex}\left({\mathbb{P}}_a\left(\tilde{R}\right)\right),\tilde{R}={\hat{R}}^T{R}_y\end{array}$$
其中${\mathbb{P}}_a\left(\cdot \right)={\pi }_a\left(\cdot \right)$。

IMU的测量值

一般情况下，IMU可以是9轴，包括：

• 3轴的陀螺仪；
• 3轴的加速度计；
• 3轴的磁力计；

捷联式IMU的参考坐标系是和IMU固定在一起的(body fixed frame)坐标系 { B } \{\mathcal{B}\} { B}，惯性参考坐标系为 { A } \{\mathcal{A}\} { A}，旋转$R={}_{\mathcal{B}}^{\mathcal{A}}R$表示 { B } \{\mathcal{B}\} { B}相对于 { A } \{\mathcal{A}\} { A}的旋转。

陀螺仪测量误差模型

陀螺仪的测量值可以表示为：
$${\Omega }^y=\Omega +\mathbf{b}+\mathbf{\mu }\in {\mathbb{R}}^3$$
其中$\Omega$是真实值，$\mathbf{\mu }$是加性高斯噪声，而