一起深入读懂Mahony互补滤波器

一起深入读懂Mahony互补滤波器

本文是Jesse Chen的原创文章，如有引用，请注明出处。本文

• 补全了论文Complementary Filter Design on the Special Orthogonal Group SO(3) 的绝大部分推导过程。
• 修正了论文的两处公式印刷错误。
• 通过求解微分方程，补充了滤波器收敛性数值仿真。
• 给出了direct complementary filter和passive complementary filter的对比数值仿真更多的仿真结果。

建议有兴趣的读者可以从网上下载论文 Complementary Filter Design on the Special Orthogonal Group SO(3) 对照本博文阅读。如果读者对本文的推导过程和仿真有需要讨论的地方，请邮件联系[email protected]

Filter Design and Error Criteria for Estimation on SO(3)

三种坐标系：

• $\mathcal{A}$表示惯性（固定）参考坐标系；
• $\mathcal{B}$表示固定在设备（body fixed frame）上的参考坐标系；
• $\mathcal{E}$表示估计参考坐标系；

定义固定在设备上的参考坐标系相对于惯性参考坐标系的姿态为$R={}_{\mathcal{B}}^{\mathcal{A}}R$，固定在设备上的参考坐标的角速度为$\Omega ={}^{\mathcal{B}}\Omega$。

姿态系统的动力学方程为：
$$\dot{R}=R{\Omega }_{\times }$$
其中${\Omega }_{\times }$表示skew-symmetric矩阵，${\Omega }_{\times }\mathbf{a}=\Omega \times \mathbf{a}$。从skew-symmetric matrix得到对应向量的函数为：
$$\Omega =\mathrm{vex}\left({\Omega }_{\times }\right)$$
做VSLAM的同学都知道姿态系统的动力学方程可以写成如下的四元数形式：
$$\dot{q}=\frac{1}{2}q\otimes \mathbf{p}\left(\Omega \right)$$
其中$q$表示一个单位四元数$q=\left\{\left(s,\mathbf{v}\right)|s^2+{\left|\mathbf{v}\right|}^2=1\right\}$，$\mathbf{p}\left(\Omega \right)$表示和向量$\Omega$对应的纯四元数$\mathbf{p}\left(\Omega \right)={\left(0,\Omega \right)}^{\prime }$。

陀螺仪测量到的角速度${\Omega }_y$是基于固定在设备上的参考坐标系的，
$${\Omega }_y=\Omega +b\left(t\right)+{\mu }_{\Omega }$$
其中$\Omega$是角速度的真实值，$b\left(t\right)$是缓慢变化的偏差(bias)，${\mu }_{\Omega }$表示噪声。陀螺仪的bias破坏了陀螺仪测量值(${\Omega }_y$)中的低频信息，但是${\Omega }_y$的高频信息还是很不错的。

加速度计和磁力计的测量值会一起提供姿态旋转$R_y$的代数估计。互补滤波器的目标是融合低频测量$R_y$和高频变换量测量${\Omega }_y$来获得姿态的一个好的估计$\hat{R}$。

$\hat{R}$可以看成对固定在设备上的参考坐标系的旋转矩阵$R$的一个估计。旋转$\hat{R}$自身也可以看成是估计参考坐标系的旋转：
$$\hat{R}={}_{\mathcal{E}}^{\mathcal{A}}\hat{R}:\mathcal{E}\to \mathcal{A}$$
估计误差也是一个旋转:
$$R={\hat{R}}^{T}R:\mathcal{B}\to \mathcal{A}$$
互补滤波器的设计目标是找到对于$\hat{R}\left(t\right)\in SO\left(3\right)$的一个动力学系统使得$R\to I$。

Jesse’s Comment:

• 对于较短的时间间隔$t$，我们可以得到：
  $$R\left(t\right)=R\exp \left({\Omega }_{\times }t\right)$$
  上式两边对$t=0$求导，可以得到：
  $$\dot{R}=R\frac{d}{dt}\exp \left({\Omega }_{\times }t\right){|}_{t=0}=R\exp \left({\Omega }_{\times }t\right){|}_{t=0}{\Omega }_{\times }=R{\Omega }_{\times }$$
• $R\in SO\left(3\right)$且$\hat{R}\in SO\left(3\right)$，因此$R\in SO\left(3\right)$;
• 如果$\hat{R}=R$，那么$R={\hat{R}}^{T}R=I$;

Known Mathematical Identities

为了阅读本博文方便，这里列举出一些用到的数学公式：
$${\pi }_a\left(R\right)=\frac{1}{2}\left(R-R^T\right),{\pi }_s\left(R\right)=\frac{1}{2}\left(R+R^T\right)$$
显然${\pi }_a$和${\pi }_s$会分别得到skew-symmetic矩阵和symmetric矩阵。
$$R={\pi }_a\left(R\right)+{\pi }_s\left(R\right),{\pi }_a{\left(R\right)}^T=-{\pi }_a\left(R\right),{\pi }_s{\left(R\right)}^T={\pi }_s\left(R\right)$$
假设$\left(\theta ,\mathbf{a}\right)$是$R$的角－轴表示，那么
$$\begin{array}{rl}R& =\exp \left(\theta {\mathbf{a}}_{\times }\right),\log \left(R\right)=\theta {\mathbf{a}}_{\times }\\ \cos \left(\theta \right)& =\frac{1}{2}\left(\mathrm{tr}\left(R\right)-1\right),{\mathbf{a}}_{\times }=\frac{1}{\sin \left(\theta \right)}{\pi }_a\left(R\right)\end{array}$$

Cost Function for the complementary filters

Mahony在论文中提出complementary filter的设计cost function为：
$$E_t:={\Vert I_3-R\Vert }^2=\frac{1}{2}\mathrm{tr}\left(I_3-R\right)=\left(1-\cos \left(\theta \right)\right)=2{\sin }^2\left(\frac{\theta }{2}\right)$$

Non-linear Complementary Filter Design on SO(3)

A. Theoretical development of complementary filter on SO(3)

对于$R\in SO\left(3\right)$和任意$\mathbf{a}\in {\mathbb{R}}^3$，都有：
$${\left(R\mathbf{a}\right)}_{\times }=R{\mathbf{a}}_{\times }{R}^T$$
所以，对于$\dot{R}=R{\Omega }_{\times }$有：
$$\begin{array}{rl}\dot{R}& =R{\Omega }_{\times }={\left(R\Omega \right)}_{\times }R\end{array}$$
Mahony提出如下的动力学系统：
$$\dot{\hat{R}}={\left(R\Omega +\hat{R}\omega \left(\hat{R},R\right)\right)}_{\times }\hat{R}$$
其中$\omega \left(\hat{R},R\right)\in \mathcal{E}$是修正项，当$\hat{R}=R$时，$\omega \left(\hat{R},R\right)=0$。
这就是direct complementary filter的动态方程。

对于$R$，其动力学方程为：
$$\begin{array}{rl}\dot{R}& ={\hat{R}}^T{\left(R\Omega +\hat{R}\omega \right)}_{\times }^{T}R+{\hat{R}}^T{\left(R\Omega \right)}_{\times }R={\hat{R}}^T{\left(\hat{R}\omega {\hat{R}}^T\right)}^{T}R\\ & ={\omega }_{\times }^{T}R=-{\omega }_{\times }R\end{array}$$

Jesse’s Comment: 推导上式的关键是：
$$R={\hat{R}}^{T}R\Rightarrow \dot{R}={\left(\dot{\hat{R}}\right)}^{T}R+{\hat{R}}^T\dot{R}$$
分别将
$$\begin{array}{rl}\dot{\hat{R}}& ={\left(R\Omega +\hat{R}\omega \left(\hat{R},R\right)\right)}_{\times }\hat{R}\\ \dot{R}& =R{\Omega }_{\times }={\left(R\Omega \right)}_{\times }R\end{array}$$
带入即可。

修正项$\omega$可以选为：
$$\omega =k_{est}\mathrm{vex}\left({\pi }_a\left(R\right)\right)\iff {\omega }_{\times }=k_{est}{\pi }_a\left(R\right),k_{est}>0$$

Lemma 3.1: 如果direct complementary filter的动态方程为：
$$\dot{\hat{R}}={\left(R\Omega +\hat{R}\omega \right)}_{\times }\hat{R}=\left({\left(R\Omega \right)}_{\times }+k_{est}\hat{R}{\pi }_a\left(R\right){\hat{R}}^T\right)\hat{R},\text{with}\hat{R}\left(0\right)={\hat{R}}_0$$
那么
$${\dot{E}}_t=-2k_{est}{\cos }^2\left(\frac{\theta }{2}\right)E_t$$
对于任何的初始条件${\hat{R}}_0$，只要
$${\theta }_0=\frac{1}{\sqrt{2}}\Vert \log \left(R_0\right)\Vert <\pi$$
那么$E_t\to 0$且$\hat{R}\left(t\right)\to R\left(t\right)$。

Jesse’s Comment:
解微分方程：
$${\dot{E}}_t=-2k_{est}{\cos }^2\left(\frac{\theta }{2}\right)E_t$$
初始值：
$$E_0=2{\sin }^2\left(\frac{{\theta }_0}{2}\right)$$
可以得到：
$$E_t=2{\sin }^2\left(\frac{{\theta }_0}{2}\right)e^{-k_{est}\left(\cos \theta +1\right)t}$$
其中$\theta$是时变的，初始值$\theta \left(0\right)={\theta }_0$。显然$\cos \theta +1\ge 0$，只要${\theta }_0<\pi$，$E_t\to 0$。

我们可以进行数值仿真，得到${\theta }_0$取不同值时，$E_t$的曲线,

假设初始值$R=R_0$，那么
$$\log \left(R_0\right)={\theta }_0{\mathbf{a}}_{\times }$$
对上式两边取Frobenius范数，且${\left|\mathbf{a}\right|}^2=\frac{1}{2}{\Vert {\mathbf{a}}_{\times }\Vert }^2$，$\mathbf{a}$是单位向量($\left|\mathbf{a}\right|=1$)，那么
$$\begin{array}{rl}\Vert \log \left(R_0\right)\Vert & ={\theta }_0\Vert {\mathbf{a}}_{\times }\Vert \\ \Rightarrow {\theta }_0& =\frac{1}{\sqrt{2}}\Vert \log \left(R_0\right)\Vert \end{array}$$

B. Implementing a complementary filter on SO(3)

Direct complementary filter

如图所示，direct complementary filter采用测量的旋转$R_y$作为其中的一个输入。direct complementary filter存在的问题是：测量到的$R_y$通常可能会被高频噪声破坏掉。噪声会通过$\left(R_y{\Omega }_y\right)$传递到最终的输出。

Passive complementary filter

在passive complementary filter中，$\left(R_y{\Omega }_y\right)$中的$R_y$被估计的$\hat{R}$所替换。这样做的优点是，如果滤波器表现良好，那么$\hat{R}$是比$R_y$对真实旋转的一个更好的估计值。

C. Passive complementary filter design

如果采用估计值$\hat{R}$做为反馈量，可以得到在估计参考坐标系下的滤波器动力学方程：
$$\dot{\hat{R}}={\left(\hat{R}\Omega +\hat{R}\omega \right)}_{\times }\hat{R}=\left(\hat{R}{\Omega }_{\times }{\hat{R}}^T+k_{est}\hat{R}{\pi }_a\left(R\right){\hat{R}}^T\right)\hat{R}=\hat{R}\left({\Omega }_{\times }+k_{est}{\pi }_a\left(R\right)\right)=\hat{R}{\left(\Omega +\omega \right)}_{\times }$$
Jesse’s Comment：Mahony论文在$\left(\hat{R}{\Omega }_{\times }{\hat{R}}^T+k_{est}\hat{R}{\pi }_a\left(R\right){\hat{R}}^T\right)\hat{R}$前多印了一个$\hat{R}$，以本博文为准。

该公式的推导过程如下：
$$\begin{array}{rl}\dot{\hat{R}}& ={\left(\hat{R}\Omega +\hat{R}\omega \right)}_{\times }\hat{R}=\left({\left(\hat{R}\Omega \right)}_{\times }+{\left(\hat{R}\omega \right)}_{\times }\right)\hat{R}\\ & =\left(\hat{R}{\Omega }_{\times }{\hat{R}}^T+\hat{R}{\omega }_{\times }{\hat{R}}^T\right)\hat{R}=\left(\hat{R}{\Omega }_{\times }{\hat{R}}^T+\hat{R}{k}_{est}{\pi }_a\left(R\right){\hat{R}}^T\right)\hat{R}\\ & =\hat{R}\left({\Omega }_{\times }+k_{est}{\pi }_a\left(R\right)\right){\hat{R}}^T\hat{R}=\hat{R}\left(\Omega +{\omega }_{\times }\right)\end{array}$$
在Mahony论文中，还多次用到了一个结论：
$${\mathrm{Ad}}_R{\pi }_a\left(R\right)=R{\pi }_a\left(R\right)R^T={\pi }_a\left(R\right)$$
由$R\in SO\left(3\right)$，可得$R{R}^T=R^{T}R$，进而易得$R$和$\left(R-R^T\right)$是可交换的。
$$\begin{array}{rl}{\mathrm{Ad}}_R{\pi }_a\left(R\right)& =R{\pi }_a\left(R\right)R^T=\frac{1}{2}R\left(R-R^T\right)R^T\\ & =\frac{1}{2}\left(R-R^T\right)R{R}^T=\frac{1}{2}\left(R-R^T\right)\\ & ={\pi }_a\left(R\right)\end{array}$$

Lemma 3.2:　如果passive complementary filter的动力学方程为
$$\dot{\hat{R}}={\left(\hat{R}\Omega +\hat{R}\omega \right)}_{\times }\hat{R}=\left(\hat{R}{\Omega }_{\times }{\hat{R}}^T+k_{est}\hat{R}{\pi }_a\left(R\right){\hat{R}}^T\right)\hat{R}=\hat{R}\left({\Omega }_{\times }+k_{est}{\pi }_a\left(R\right)\right)=\hat{R}{\left(\Omega +\omega \right)}_{\times }$$
那么
$${\dot{E}}_t=-2k_{est}{\cos }^2\left(\frac{\theta }{2}\right)E_t$$
对于任意的初始值${\hat{R}}_0$，只要
$${\theta }_0=\frac{1}{\sqrt{2}}\Vert \log \left(R_0\right)\Vert <\pi$$
那么$E_t\to 0$且$\hat{R}\left(t\right)\to R\left(t\right)$。

Jesse’s Comment:对于Lema 3.2，这里将$\dot{R}$的推导过程补上。
$$\begin{array}{rl}\dot{R}& ={\left(\dot{\hat{R}}\right)}^{T}R+{\hat{R}}^T\dot{R}={\left(\hat{R}{\left(\Omega +\omega \right)}_{\times }\right)}^{T}R+{\hat{R}}^T{\left(R\Omega \right)}_{\times }R\\ & ={\left(\Omega +\omega \right)}_{\times }^T{\hat{R}}^{T}R+{\hat{R}}^{T}R{\Omega }_{\times }={\left(\Omega +\omega \right)}_{\times }^{T}R+R{\Omega }_{\times }\\ & =-{\left(\Omega +\omega \right)}_{\times }R+R{\Omega }_{\times }\end{array}$$

D. Simulation Results

按照论文中给出的仿真条件：

• 旋转矩阵的初始值$R\left(0\right)=I_3$;
• 偏差(deviation)$R$的初始值对应的欧拉角为$\left[\begin{array}{ccc}0& \frac{\pi }{2}& 0\end{array}\right]$;
• 角速度为$\Omega =0.3{\mathbf{e}}_3$;

经过博文作者编程仿真，可以得到和Mahony论文一致的结果：

经过检查发现，论文中的仿真结果是在$k_{est}=1.0$下获得的。让$k_{est}$取不同的值，可以得到不同的仿真结果：



检查三个$k_{est}$取值下，对应时刻direct filter和passive filter的仿真结果是否都在同心圆上，博文作者计算了不同的仿真时刻仿真结果相对圆心$\left(0,0\right)$的半径差，取$k_{est}=0.2$时候的结果来说明：

可以发现论文的结论是成立的，同一时刻direct complementary filter的仿真结果和passive complementary filter的仿真结果是在同心圆上的。（注意：半径差绝对值的量级是$10^{-14}$）

Attitude Extraction and Gyros Bias Estimation from Passive Complementary Filter

在定理4.1的证明中需要用到一个结论：
$${\mathbf{b}}^T\dot{\hat{\mathbf{b}}}=\frac{1}{2}\mathrm{tr}\left({\mathbf{b}}_{\times }^T{\dot{\hat{\mathbf{b}}}}_{\times }\right)$$
注意：论文原文中的结论不正确，以本博文给出的这个结论为准。

A Quaternion-Based derivation of the passive complementary filter

基于很多估计和控制算法是基于四元数，因此Mahony给出了基于四元数实现passive complementary filter的公式。

定义单位四元数误差$q$：
$$q={\hat{q}}^{-1}\otimes q=\left[\begin{array}{c}s\\ \mathbf{v}\end{array}\right]$$
其中$q$和$R$，$\hat{q}$和$\hat{R}$，$q$和$R$是对应的。

Thienel和Sanner在参考文献[2]中提出了
$$\dot{\hat{q}}=\frac{1}{2}\hat{q}\otimes \mathbf{p}\left(R\left({\Omega }_y-\hat{\mathbf{b}}+k_{est}\mathrm{sgn}\left(s\right)\mathbf{v}\right)\right)$$
bias的估计$\hat{\mathbf{b}}$的动态方程为：
$$\dot{\hat{\mathbf{b}}}=-k_b\mathrm{sgn}\left(s\right)\mathbf{v}$$
如果我们将$\mathrm{sgn}\left(s\right)$替换成$2s$，那么参考文献[2]中的滤波器等效于论文中提出的基于$SO\left(3\right)$的direct complementary filter。
$$2s\mathbf{v}=2\cos \left(\frac{\theta }{2}\right)\sin \left(\frac{\theta }{2}\right)\mathbf{a}=\sin \left(\theta \right)\mathbf{a}=\mathrm{vex}\left({\pi }_a\left(R\right)\right)$$
Jesse’s Comment: 对于单元四元数$q$，
$$q=\cos \left(\frac{\theta }{2}\right)+\left(u_x\mathbf{i}+u_y\mathbf{j}+u_z\mathbf{k}\right)\sin \left(\frac{\theta }{2}\right)=\cos \left(\frac{\theta }{2}\right)+\mathbf{a}\sin \left(\frac{\theta }{2}\right)$$
所以,
$$s=\cos \left(\frac{\theta }{2}\right)\mathbf{v}=\mathbf{a}\sin \left(\frac{\theta }{2}\right)$$
又由${\pi }_a\left(R\right)=\sin \left(\theta \right){\mathbf{a}}_{\times }$，可知：
$$\sin \left(\theta \right)\mathbf{a}=\mathrm{vex}\left({\pi }_a\left(R\right)\right)$$

因此
$$\dot{\hat{q}}=\frac{1}{2}\hat{q}\otimes \mathbf{p}\left(R\left({\Omega }_y-\hat{\mathbf{b}}+k_{est}\mathrm{vex}\left({\pi }_a\left(R\right)\right)\right)\right)=\frac{1}{2}\hat{q}\otimes \mathbf{p}\left(R\left({\Omega }_y-\hat{\mathbf{b}}\right)+k_{est}R\mathrm{vex}\left({\pi }_a\left(R\right)\right)\right)$$
同样，因为$R\mathrm{vex}\left({\pi }_a\left(R\right)\right)=\mathrm{vex}\left({\pi }_a\left(R\right)\right)$，上式可以最终写成：
$$\dot{\hat{q}}=\frac{1}{2}\hat{q}\otimes \mathbf{p}\left(R\left({\Omega }_y-\hat{\mathbf{b}}\right)+k_{est}\mathrm{vex}\left({\pi }_a\left(R\right)\right)\right)$$
$\hat{\mathbf{b}}$的估计：
$$\dot{\mathbf{b}}=-k_b\mathrm{vex}\left({\pi }_a\left(R\right)\right)$$
它们和论文中基于$SO\left(3\right)$的direct complementary filter的公式是一致的。

Jesse’ Comment: 由论文的公式(15),
$$\begin{array}{rl}\dot{\hat{R}}& =\hat{R}{\left(R\left({\Omega }_y-\hat{\mathbf{b}}\right)+\omega \right)}_{\times }=\hat{R}{\left(R\left({\Omega }_y-\hat{\mathbf{b}}\right)+k_{est}\mathrm{vex}\left({\pi }_a\left(R\right)\right)\right)}_{\times }\\ \dot{\hat{\mathbf{b}}}& =-k_b\mathrm{vex}\left({\pi }_a\left(R\right)\right)\end{array}$$
不难发现，确实是一致的。那么和$SO\left(3\right)$中的passive complementary filter
$$\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(R\right)\right)\end{array}$$
对应的四元数passive complementary filter就是
$$\begin{array}{rl}\dot{\hat{q}}& =\frac{1}{2}\hat{q}\otimes \mathbf{p}\left({\Omega }_y-\hat{\mathbf{b}}+\omega \right),\text{with}\omega =k_{est}s\mathbf{v}\\ \dot{\hat{\mathbf{b}}}& =-k_{b}s\mathbf{v}\end{array}$$

回顾

Mahony在这篇论文中

• 给出了如何从direct complementary filter演化得到pass complementary filter;
• 证明了在$SO\left(3\right)$上的direct complementary filter和passive complementary filter的收敛性(本博文在解微分方程的基础上增加了收敛性的数值仿真)；
• 给出了direct complementary filter和passive complementary filter的数值仿真（本博文展示了更多了仿真结果:-)）
• 如何用四元数实现$SO\left(3\right)$上的passive complementary filter;

对Mahony互补滤波器的实现感兴趣的同学，可以参考我的另一篇博文《Mahony互补滤波器代码详解》，https://blog.csdn.net/jessecw79/article/details/85763512

参考文献

[1] Robert Mahony, Tarek Hamel, Jean-Michel Pflimlin, Complementary filter design on the special orthogonal group SO(3), http://folk.ntnu.no/skoge/prost/proceedings/cdc-ecc05/pdffiles/papers/1889.pdf
[2] J. Thienel and R.M. Sanner. A coupled nonlinear spacecraft attitude controller and observer with an unknown constant gyro bias and gyro noise. IEEE Transactions on Automatic Control, 48, no 11:2011-2014, 2003