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(R\right)\right),\text{with }{k}_b>0\\ \omega & =k_{\text{est}}\mathrm{vex}\left({\pi }_a\left(R\right)\right),\text{with }{k}_{\text{est}}>0\\ 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(R\right)\right),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)坐标系$\{\mathcal{B}\}$，惯性参考坐标系为$\{\mathcal{A}\}$，旋转$R={}_{\mathcal{B}}^{\mathcal{A}}R$表示$\{\mathcal{B}\}$相对于$\{\mathcal{A}\}$的旋转。

陀螺仪测量误差模型

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

加速度计测量误差模型

加速度计的测量值可以表示为：
$$\mathbf{a}=R^T\left(\dot{\mathbf{v}}-{\mathbf{g}}_0\right)+{\mathbf{b}}_a+{\mathbf{\mu }}_a$$
其中，${\mathbf{b}}_a$是加速度计的bias，${\mathbf{\mu }}_a$是加性高斯噪声。如果我们规定重力加速度的方向为${\mathbf{e}}_3$，那么${\mathbf{g}}_0=\left|{\mathbf{g}}_0\right|{\mathbf{e}}_3$，通常$\left|{\mathbf{g}}_0\right|=9.8$。

通常使用
$${\mathbf{v}}_a=\frac{\mathbf{a}}{\left|\mathbf{a}\right|}\approx -R^T{\mathbf{e}}_3$$
作为惯性坐标系的$z$轴在和IMU固定的坐标系(body fixed frame)中的一个低频估计。

磁力计

磁力计的测量值可以表示为：
$$\mathbf{m}=R^T{}^A\mathbf{m}+{\mathbf{B}}_m+{\mathbf{\mu }}_b$$
其中，${}^A\mathbf{m}$表示在惯性坐标系下表示的地球磁场，${\mathbf{B}}_m$表示本地磁力干扰，${\mathbf{\mu }}_b$是噪声。只有磁力计的方向和方向估计有关，因此我们通常使用如下的向量形式的测量
$${\mathbf{v}}_m=\frac{\mathbf{m}}{|\mathbf{m}|}$$

向量${\mathbf{v}}_a$和${\mathbf{v}}_m$可以用来构造${}_{\mathcal{B}}^{\mathcal{A}}R:\{\mathcal{B}\}\to \{\mathcal{A}\}$的一个瞬时代数测量值
$$R_y=\arg \underset{R\in SO\left(3\right)}{\min }\left({\lambda }_1|{\mathbf{e}}_3-R{\mathbf{v}}_a{|}^2+{\lambda }_2|{\mathbf{v}}_m^{\ast }-R{\mathbf{v}}_m{|}^2\right)\approx {}_{\mathcal{B}}^{\mathcal{A}}R$$
其中${\mathbf{v}}_m^{\ast }$是磁场的惯性方向。

注意：${\mathbf{v}}_a$和${\mathbf{v}}_m$就是参考文献中所说的惯性方向。

我想没人愿意按照上式在线估计$R_y$，Mahony等学者提出了直接使用IMU测量值得互补滤波器–explicit complementary filter。

Explicit Error Formulation of the Passive Complementary Filter

设${\mathbf{v}}_{0i}\in \mathcal{A},i=1,\cdots ,n$表示$n$个已知的惯性方向。而与之相对，我们所测量到的惯性方向都是在和IMU固定在一起(body fixed frame)的坐标系中的观测：
$${\mathbf{v}}_i=R^T{\mathbf{v}}_{0i}+{\mathbf{\mu }}_i,{\mathbf{v}}_i\in \mathcal{B}$$
其中${\mathbf{\mu }}_i$是噪声。因为我们只关心惯性方向的方向，因此我们可以做出归一化：
$$\begin{array}{rl}|{\mathbf{v}}_{0i}|& =1\\ |{\mathbf{v}}_i|& =1\end{array}$$

设$\hat{R}$是$R$的一个估计，定义：
$${\hat{\mathbf{v}}}_i={\hat{R}}^T{\mathbf{v}}_{0i}$$
是于${\mathbf{v}}_{0i}$对应的估计值。对于单个方向${\mathbf{v}}_i$，方向误差可以定义为：
$$E_i=1-\cos \left(\angle {\mathbf{v}}_i,{\hat{\mathbf{v}}}_i\right)=1-⟨{\mathbf{v}}_i,{\hat{\mathbf{v}}}_i⟩$$
继而得到
$$E_i=1-\mathrm{tr}\left({\hat{R}}^T{\mathbf{v}}_{0i}{\mathbf{v}}_{0i}^{T}R\right)=1-\mathrm{tr}\left(R{R}^T{\mathbf{v}}_{0i}{\mathbf{v}}_{0i}^{T}R\right)$$
对于多个测量${\mathbf{v}}_i$，考虑下面的误差：
$$E_{\text{mes}}=\sum _i{k}_i{E}_i=\sum _{i=1}^n{k}_i-\mathrm{tr}\left(RM\right),k_i>0$$
其中
$$M=R^T{M}_{0}R\text{with }{M}_0=\sum _{i=1}^n{k}_i{\mathbf{v}}_{0i}{\mathbf{v}}_{0i}^T$$
假设惯性方向之间是线性不相关的，那么

• 当$n\ge 3$时，矩阵$M$是正定的；
• 当$n\le 2$时，矩阵$M$是半正定的，且其中一个特征值为0。

我们可以得到按照如下方程组描述的Explicit Complementary Filter:
$$\begin{array}{rl}\dot{\hat{R}}& =\hat{R}\left({\left({\Omega }^y-\hat{\mathbf{b}}\right)}_{\times }+k_P{\left({\omega }_{\text{mes}}\right)}_{\times }\right),\hat{R}\left(0\right)={\hat{R}}_0\\ \dot{\hat{\mathbf{b}}}& =-k_I{\omega }_{\text{mes}}\\ {\omega }_{\text{mes}}:& =\sum _{i=1}^n{k}_i{\mathbf{v}}_i\times {\hat{\mathbf{v}}}_i,k_i>0\end{array}$$

Passive Complementary Filter和Explicit Complementary Filter之间的关系

下面我们来推导Explicit Complementary Filter和Passive Complementary Filter之间的关系。

容易证明：
$${\omega }_{\text{mes}}=\mathrm{vex}\left(\sum _{i=1}^n{k}_i\left({\hat{\mathbf{v}}}_i{\mathbf{v}}_i^T-{\mathbf{v}}_i{\hat{\mathbf{v}}}_i^T\right)\right)=-\mathrm{vex}\left(\sum _{i=1}^n{k}_i\left({\mathbf{v}}_i{\hat{\mathbf{v}}}_i^T-{\hat{\mathbf{v}}}_i{\mathbf{v}}_i^T\right)\right)$$

本人反复计算过上式，也借助过计算机，仍然和参考文献中间的结论有2倍的差异，但是由于有一组可调系数$\{k_i\}$，因此不会影响参考文献结论的正确性。看到本文的同学，如果对这段计算感兴趣或者对我的结论有不同意见的，请联系[email protected]。

同时也容易证明：
$$RM={\hat{R}}^T{M}_{0}R=\sum _{i=1}^n{k}_i{\hat{\mathbf{v}}}_i{\mathbf{v}}_i^T$$
显然
$${\left({\omega }_{\text{mes}}\right)}_{\times }={\mathbb{P}}_a\left(RM\right)$$
在一种特殊情况下：当$n=3$，$k_i=1$，且测量的惯性方向是两两正交的(${\mathbf{v}}_i^T{\mathbf{v}}_j=0,\forall i̸=j$)，显然$M_0={\mathbf{I}}_3$。那么
$$M=R^T{M}_{0}R=R^{T}R={\mathbf{I}}_3$$
在这种情况下，
$$\begin{array}{rl}{\left({\omega }_{\text{mes}}\right)}_{\times }& ={\mathbb{P}}_a\left(R\right)\\ \Rightarrow {\omega }_{\text{mes}}& =\mathrm{vex}\left({\mathbb{P}}_a\left(R\right)\right)\end{array}$$
这就和开篇回顾的Mahony互补滤波器公式一致了。前不久，有一个工程师和我讲：他看参考文献[3]写技术报告，因为参考文献[3]是非线性的，而参考文献[4]中的互补滤波器是线性的。这种说法是不正确的，本文已经说明了explicit complementary filter就是Mahony Complementary Filter。

Explicit Complementary Filter的四元数实现形式

$$\begin{array}{rl}{\omega }_{\text{mes}}& =\sum _{i=1}^n{k}_i\mathbf{v}\times {\hat{\mathbf{v}}}_i^T,k_i>0\\ \dot{\hat{q}}& =\frac{1}{2}\hat{q}\otimes \mathbf{p}\left({\Omega }^y-\hat{\mathbf{b}}+k_P{\omega }_{\text{mes}}\right),k_P>0\\ \dot{\hat{\mathbf{b}}}& =-k_I{\omega }_{\text{mes}},k_I>0\end{array}$$
其中，$\mathbf{p}:{\mathbb{R}}^3\to \mathbb{Q}$。

Mahony互补滤波器的Matlab代码详解

下面，我们将解读Matlab版本的Mahony互补滤波器代码，这个版本是由Madgwick实现的。

function obj = UpdateIMU(obj, Gyroscope, Accelerometer)
    q = obj.Quaternion; % short name local variable for readability
 
    % Normalise accelerometer measurement
    if(norm(Accelerometer) == 0), return; end   % handle NaN
    Accelerometer = Accelerometer / norm(Accelerometer);	% normalise magnitude
 
    % Estimated direction of gravity and magnetic flux
    v = [2*(q(2)*q(4) - q(1)*q(3))
        2*(q(1)*q(2) + q(3)*q(4))
        q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
 
    % Error is sum of cross product between estimated direction and measured direction of field
    e = cross(Accelerometer, v); 
    if(obj.Ki > 0)
        obj.eInt = obj.eInt + e * obj.SamplePeriod;   
    else
        obj.eInt = [0 0 0];
    end
            
    % Apply feedback terms
    Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;            
            
    % Compute rate of change of quaternion
    qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
 
    % Integrate to yield quaternion
    q = q + qDot * obj.SamplePeriod;
    obj.Quaternion = q / norm(q); % normalise quaternion
end

Note:

• obj.eInt就是$\hat{\mathbf{b}}$，这里是积分运算。

    if(obj.Ki > 0)
        obj.eInt = obj.eInt + e * obj.SamplePeriod;   
    else
        obj.eInt = [0 0 0];
    end

• 关于代码中Estimated direction of gravity and magnetic flux的解释：首先来看unit quaternion转换为旋转矩阵的公式：The orthogonal matrix corresponding to a rotation by the unit quaternion $\mathbf{z}=a+b\mathbf{i}+c\mathbf{j}+d\mathbf{k}$ (with $|\mathbf{z}|=1$，说明是unit quaternion) when post-multiplying with a column vector is given by
  $$R=\left(\begin{array}{ccc}{a}^2+b^2-c^2-d^2& 2bc-2ad& 2bd+2ac\\ 2bc+2ad& a^2-b^2+c^2-d^2& 2cd-2ab\\ 2bd-2ac& 2cd+2ab& a^2-b^2-c^2+d^2\end{array}\right)$$
  那么代码中的这段

v = [2*(q(2)*q(4) - q(1)*q(3))
     2*(q(1)*q(2) + q(3)*q(4))
     q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];

的计算就是
$${\hat{\mathbf{v}}}_i={\hat{R}}^T\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)$$

• 这段代码的中的Accelerometer就是${\mathbf{v}}_i$:
  $${\mathbf{v}}_i=R^T{\mathbf{v}}_{0i}+{\mathbf{\mu }}_i,{\mathbf{v}}_i\in \{\mathcal{B}\}$$

参考文献

[1] Tarek Hamel, Robert Mahony, Attitude Estimation on SO(3) Based on Direct Inertial Measurements, https://www.researchgate.net/profile/T_Hamel/publication/224635156_Attitude_estimation_on_SO3_based_on_direct_inertial_measurements/links/00b4951a50e818d6be000000/Attitude-estimation-on-SO3-based-on-direct-inertial-measurements.pdf
[2] Robert Mahony, Tarek Hamel, A Study of Non-linear Complementary Filter Design for Kinematic Systems on the Special Orthogonal group, Internal report: ISRN I3S/RR-2006-36-FR.
[3] Robert Mahony, Tarek Hamel, Jean-Michel Pflimlin, Non-linear Complementary Filters on the Special Orthogonal Group, IEEE Transactions on Automatic Control, Volume: 53 , Issue: 5 , June 2008. https://hal.archives-ouvertes.fr/hal-00488376/document
[4] 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