基于四元数扩展卡尔曼滤波的IMU姿态解算

姿态解算用于求解云台系到惯性系的坐标变换矩阵与姿态欧拉角，准确的姿态信息对目标运动估计和云台运动控制至关重要。本系统利用四元数微分方程进行姿态更新，通过扩展卡尔曼滤波（Extended Kalman Filter, EKF）融合陀螺仪与加速度计信息估计重力加速度，利用加速度计修正姿态并估计陀螺仪 $x,y$ 轴零偏，利用估计得到的重力加速度以非线性约束的方式对四元数进行修正，并借助卡方检验剔除运动加速度过大时的加速度计量测以降低运动加速度对滤波准确性的影响。利用重力加速度估计值而非加速度计测量值进行姿态修正，可较大程度减小运动加速度对姿态解算准确性的影响。

四元数

定义

姿态描述的是机体坐标系 (b 系) 相对惯性坐标系 (n 系) 的旋转关系。常见的描述方法有三种，每种各有其优缺点，包括欧拉角，方向余弦矩阵和四元数。

四元数的定义与复数非常相似，主要区别是复数只有一个虚部，而四元数有三个，故四元数可以写成下面这种形式：
$$
\mathbf q=q_{0}^{}+q_{1}^{} i+q_{2}^{} j+q_{3}^{} k \quad(q_{0}^{}, q_{1}^{}, q_{2}^{}, q_{3}^{} \in \mathbb{R})
$$
其中：

$i^{2}=j^{2}=k^{2}=i j k=-1\\$

四元数可以看作基 {1，i，j，k}的线性组合，同样的，四元数也可以写成向量形式：
$$
\mathbf q=\left[\begin{array}{l}
{q_{0}^{}} \
{q_{1}^{}} \
{q_{2}^{}} \
{q_{3}^{}}
\end{array}\right]
$$
值得注意的是，只有单位四元数可用于描述姿态。

另外的，也可以将实部与虚部分开，即通过一个三维向量表示虚部，从而将四元数表示为标量与向量的有序对形式：

$q=[s, \mathbf{v}] \quad\left(\mathbf{v}=\left[\begin{array}{l}{x} \\{y} \\{z}\end{array}\right], s, x, y, z \in \mathbb{R}\right)\\$

姿态表示

利用四元数表示姿态：该部分省略推导过程，具体可以参考https://krasjet.github.io/quate

通过罗德里格旋转可推导出四元数表示旋转的定理：任意向量 v 绕着以单位向量定义的旋转轴 u 旋转 θ 度后的 v’ 可以使用四元数乘法来获得：

$v^{\prime}=q v q^{*}=q v q^{-1}\\$

其中：

$v=[0, \mathbf{v}], \quad q=\left[\cos \left(\frac{1}{2} \theta\right), \sin \left(\frac{1}{2} \theta\right) \mathbf{u}\right]\\\\$

得出四元数表示旋转的一般形式，即三个四元数相乘，通过四元数乘法的矩阵表示形式，可以将四元数旋转公式表示为矩阵形式：

$q v q^{*}=\left[\begin{array}{cccc} {1} & {0} & {0} & {0} \\ {0} & {1-2 q_{2}^{2}-2 q_{3}^{2}} & {2 q_{1} q_{2}-2 q_{0} q_{3}} & {2 q_{0} q_{2}+2 q_{1} q_{3}} \\ {0} & {2 q_{1} q_{2}+2 q_{0} q_{3}} & {1-2 q_{1}^{2}-2 q_{3}^{2}} & {2 q_{2} q_{3}-2 q_{0} q_{1}} \\ {0} & {2 q_{1} q_{3}-2 q_{0} q_{2}} & {2 q_{0} q_{1}+2 q_{2} q_{3}} & {1-2 q_{1}^{2}-2 q_{2}^{2}} \end{array}\right] v\\$

矩阵的第一行和第一列不会对v进行任何变换，所以可以将其压缩为3维方阵，即可得到矩阵旋转公式：任意向量 v 沿着以单位向量定义的旋转轴 u 旋转 θ 角度后的 v’ 可以使用矩阵乘法来获得：

$\mathbf{v}^{\prime}=\left[\begin{array}{ccc} {1-2\left(q_{2}^{2}+q_{3}^{2}\right)} & {2\left(q_{1} q_{2}-q_{0} q_{3}\right)} & {2\left(q_{1} q_{3}+q_{0} q_{2}\right)} \\ {2\left(q_{1} q_{2}+q_{0} q_{3}\right)} & {1-2\left(q_{1}^{2}+q_{3}^{2}\right)} & {2\left(q_{2} q_{3}-q_{0} q_{1}\right)} \\ {2\left(q_{1} q_{3}-q_{0} q_{2}\right)} & {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} & {1-2\left(q_{1}^{2}+q_{2}^{2}\right)} \end{array}\right]\mathbf{v}\\$

改写为矩阵乘法形式可得到通过四元数表示的 b 系到 n 系的坐标变换矩阵：
$$
\boldsymbol C_{b}^{n}=\left[\begin{array}{lll}
{1-2\left(q_{2}^{2}+q_{3}^{2}\right)} & {2\left(q_{1} q_{2}-q_{0} q_{3}\right)} & {2\left(q_{1} q_{3}+q_{0} q_{2}\right)} \
{2\left(q_{1} q_{2}+q_{0} q_{3}\right)} & {1-2\left(q_{1}^{2}+q_{3}^{2}\right)} & {2\left(q_{2} q_{3}-q_{0} q_{1}\right)} \
{2\left(q_{1} q_{3}-q_{0} q_{2}\right)} & {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} & {1-2\left(q_{1}^{2}+q_{2}^{2}\right)}
\end{array}\right]
$$
其中：

$q_{0}=\cos \left(\frac{1}{2} \theta\right), q_{1}=\sin \left(\frac{1}{2} \theta\right) u_{x}, q_{2}=\sin \left(\frac{1}{2} \theta\right) u_{y}, q_{3}=\sin \left(\frac{1}{2} \theta\right) u_{z}\\$

也可写成有序数对形式，即上面旋转公式中的：

$\quad q=\left[\cos \left(\frac{1}{2} \theta\right), \sin \left(\frac{1}{2} \theta\right) \mathbf{u}\right]\\ 其中\mathbf{u} = \left[\begin{array}{l} {u_{x}} \\ {u_{y}} \\ {u_{z}} \end{array}\right]\\$

得到四元数后，可以通过四元数的值反解出机体坐标系的欧拉角，同样的这里省略推导过程直接给出公式：

$\left\{\begin{array}{c} {\theta=\arcsin [2(q_0 q_2-q_1 q_3)]} \\ {\gamma=\arctan \left(\frac{q_0 q_3+q_1 q_2}{1-2（q_2^{2}+q_3^{2}）}\right)} \\ {\psi=\arctan \left(\frac{q_0 q_1+q_2 q_3}{1-2（q_1^{2}+q_2^{2}）}\right)} \end{array}\right.\\$

实时更新

四元数描述机体坐标系与大地坐标系的旋转关系，因此对于机体的姿态解算需要实时更新四元数。通过构建四元数关于时间的微分方程来研究此问题。

存在单位四元数：

$Q=\cos \frac{\theta}{2}+\vec{u} \sin \frac{\theta}{2}\\$

对时间t进行微分，可得：

$\frac{d Q}{d t}=\frac{1}{2}\left[\begin{array}{cccc} {0} & {-\omega_{x}} & {-\omega_{y}} & {-\omega_{z}} \\ {\omega_{x}} & {0} & {\omega_{z}} & {-\omega_{y}} \\ {\omega_{y}} & {-\omega_{z}} & {0} & {\omega_{x}} \\ {\omega_{z}} & {\omega_{y}} & {-\omega_{x}} & {0} \end{array}\right] \cdot\left[\begin{array}{c} {q_{0}} \\ {q_{1}} \\ {q_{2}} \\ {q_{3}} \end{array}\right]\\$

求解该微分方程即可得到当前四元数的值。但计算机中的计算是离散的，所以需要对该微分方程进行离散化处理，这样才可以有效的通过单片机或其他数字控制器进行求解：

$\left[\begin{array}{l} {q_{0}} \\ {q_{1}} \\ {q_{2}} \\ {q_{3}} \end{array}\right]_{t+\Delta t}=\left[\begin{array}{l} {q_{0}} \\ {q_{1}} \\ {q_{2}} \\ {q_{3}} \end{array}\right]_{t}+\frac{1}{2} \cdot \Delta t \cdot\left[\begin{array}{c} {-\omega_{x} \cdot q_{1}-\omega_{y} \cdot q_{2}-\omega_{z} \cdot q_{3}} \\ {\omega_{x} \cdot q_{0}-\omega_{y} \cdot q_{3}+\omega_{z} \cdot q_{2}} \\ {\omega_{x} \cdot q_{3}+\omega_{y} \cdot q_{0}-\omega_{z} \cdot q_{1}} \\ {-\omega_{x} \cdot q_{2}+\omega_{y} \cdot q_{1}+\omega_{z} \cdot q_{0}} \end{array}\right]\\$

故四元数关于时间的微分方程为：
$$
\dot{ \mathbf q} = \frac{1}{2}\boldsymbol \Omega\mathbf q
$$
其中：
$$
\boldsymbol \Omega=\left[\begin{array}{cccc}
{0} & {-\omega_{x}} & {-\omega_{y}} & {-\omega_{z}} \
{\omega_{x}} & {0} & {\omega_{z}} & {-\omega_{y}} \
{\omega_{y}} & {-\omega_{z}} & {0} & {\omega_{x}} \
{\omega_{z}} & {\omega_{y}} & {-\omega_{x}} & {0}
\end{array}\right]
$$
其中 $\omega_x,\omega_y,\omega_z$ 为 $b$ 系相对 $n$ 系的角速度。

Mahony算法

简介

Mahony算法是一种基于四元数的姿态解算算法。其六轴更新算法利用四元数形式的坐标转换矩阵得到机体(IMU)坐标系下的理论重力加速度向量，并通过加速度计测得机体坐标系下的重力加速度。通过叉乘运算得到两个向量的偏差，并利用该偏差通过PI补偿器对角速度数据进行补偿，从而得到准确的姿态。

该算法相比状态向量维数动辄6、9甚至十几维的最优估计器具有计算量小、实现简单的特点。

原理

首先，对于六轴数据，计算角度有两种方法，一种是通过对角速度积分得到角度，另一种则是通过对加速度进行正交分解得到角度。但这两种方式均存在不足，通过角速度积分得到角度时，角速度的误差会在积分过程中被不断放大从而影响数据准确性。而加速度计是一种特别敏感的传感器，电机旋转产生的震动会给加速度计的数据中带来高频噪声。

不难看出，第一种方法测得的数据中存在结果偏差，而第二种方法测得的数据中存在高频噪声。因此可通过融合两种数据以获得准确姿态，这里通过加速度计补偿角速度。

设有大地坐标下的重力加速度 g，把 g 通过姿态矩阵（坐标转换矩阵）的逆（意味着从地理坐标系 R 到机体坐标 b 系）变换到机体坐标系，得到其在机体坐标系下的**理论重力加速度向量 $\hat{\boldsymbol{v}}$ **，则两者的变换关系可通过前文给出的姿态矩阵得出：

$\hat{\boldsymbol{v}}=C_{R}^{b} \cdot \mathbf{g}=(C_{b}^{R})^{-1} \cdot\left[\begin{array}{l} {0} \\ {0} \\ {1} \end{array}\right]=\left[\begin{array}{c} {2\left(q_{1} q_{3}-q_{0} q_{2}\right)} \\ {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} \\ {1-2\left(q_{1}^{2}+q_{2}^{2}\right)} \end{array}\right]=\left[\begin{array}{c} {v_{ x}} \\ {v_{ y}} \\ {v_{ z}} \end{array}\right]\\$

不难看出，将重力加速度向量变换至机体坐标系后，恰好是矩阵的最后一列。这样一来就得到了由描述刚体姿态的四元数推导出的理论重力加速度向量 $\hat{\boldsymbol{v}}$ 。另外还可以通过加速度计测量出实际重力加速度向量 $\overline{\boldsymbol{v}}$ 。

显然，这里的理论重力加速度向量 $\hat{\boldsymbol{v}}$ 和实际重力加速度向量 $\overline{\boldsymbol{v}}$ 之间必然存在偏差，而这个偏差很大程度上是由陀螺仪数据产生的角速度误差引起的，所以根据理论向量和实际向量间的偏差，就可以补偿陀螺仪数据的误差，进而解算出较为准确的姿态，即将隐含在四元数中的角速度误差显化。

理论重力加速度向量和实际重力加速度向量均是向量，反应向量间夹角关系的运算有两种：内积（点乘）和外积（叉乘），考虑到向量外积模的大小与向量夹角呈正相关，故通过计算外积来得到向量方向差值 $θ$ ：

$|\mathbf{ρ}|=|\overline{\boldsymbol{v}}| \cdot|\hat{\boldsymbol{v}}| \cdot \sin \theta\\$

在进行叉乘运算前，应先将理论向量 $\hat{\boldsymbol{v}}$ 和实际向量 $\overline{\boldsymbol{v}}$ 做单位化处理，有：

$|\mathbf{ρ}|=\sin \theta\\$

考虑到实际情况中理论向量 $\hat{\boldsymbol{v}}$ 和实际向量 $\overline{\boldsymbol{v}}$ 偏差角不会超过45°，而当 $θ$ 在±45°内时， $sinθ$ 与 $θ$ 的值非常接近，因此上式可进一步简化为：

$|\mathbf{ρ}|=\theta\\$

得到向量偏差后，即可通过构建PI补偿器来计算角速度补偿值：

$GyroError=K_{P} \cdot \text { error }+K_{I} \cdot \int \text {error}\\$

其中，比例项用于控制传感器的“可信度”，积分项用于消除静态误差。 $K_P$ 越大，意味着通过加速度计得到误差后补偿越显著，即是越信任加速度计。反之 $K_P$ 越小时，加速度计对陀螺仪的补偿作用越弱，也就越信任陀螺仪。而积分项则用于消除角速度测量值中的有偏噪声，故对于经过零篇矫正的角速度测量值，一般选取很小的 $K_I$ 。最后将补偿值补偿给角速度测量值，带入四元数差分方程中即可更新当前四元数。

考虑到四元数不具备直观几何意义，故最后还需通过四元数反解出欧拉角。省略推导过程直接给出公式：

$\left\{\begin{array}{c} {\theta=\arcsin 2(q_0 q_2-q_1 q_3)]} \\ {\gamma=\arctan \left(\frac{q_0 q_3+q_1 q_2}{1-2（q_2^{2}+q_3^{2}）}\right)} \\ {\psi=\arctan \left(\frac{q_0 q_1+q_2 q_3}{1-2（q_1^{2}+q_2^{2}）}\right)} \end{array}\right.\\$

优化

**Mahony算法将加速度测量结果直接认为是实际重力加速度，无法分辨加速度计测量结果中除重力加速度外的运动加速度，因此运动加速度会严重破坏其姿态解算精度。**为减小运动加速度对解算精度的破坏，需要降低PI补偿器中的增益，即减小加速度计置信度，但这样会导致其对角速度累计误差的补偿速度减缓，同样不利于姿态解算精度。

因此需要寻找一种方法估计重力加速度，利用估计得到的重力加速度而非加速度计原始测量值补偿误差。

扩展卡尔曼

状态向量

定义状态向量为：
$$
\boldsymbol x = \left[\begin{array}{c}
\mathbf q \
\boldsymbol \omega^{bias}
\end{array}\right]
$$
其中 $\mathbf q = [q_0,q_1,q_2,q_3]^T$ 为机体坐标系 ($b$ 系) 相对惯性坐标系 ($n$ 系) 的姿态四元数。考虑一般姿态下 $b$ 系 $z$ 轴通常指天，故难以通过重力加速度估计其零偏，故定义 $\boldsymbol \omega^{bias} =[\omega^{bias}_x,\omega^{bias}_y]^T$ 为陀螺仪 $x,y$ 轴零飘。

过程模型

根据陀螺仪误差模型，存在：
$$
\boldsymbol \omega^{real} = \boldsymbol \omega^{measure} - \boldsymbol \omega^{bias}
$$
则过程模型为：
$$
\begin{aligned}
\boldsymbol x_{k+1} &= f(\boldsymbol x_k) + \boldsymbol {w}{k} \
&= \left[\begin{array}{c}
\mathbf q_k +\frac{1}{2}\left(\boldsymbol \Omega_k - \boldsymbol \Omega_k^{bias} \right)\Delta t \mathbf q_k \
\boldsymbol \omega^{bias} k
\end{array}\right] + \boldsymbol {w}{k}
\end{aligned}
$$
其中白噪声 $\boldsymbol {w}{k} \sim N\left(\boldsymbol 0_{6 \times 1}, \boldsymbol {Q}\right)$，$\boldsymbol \Omega_k^{bias}$ 为陀螺仪零飘组成的矩阵。对非线性函数 $f$ 求其雅可比矩阵得 $\boldsymbol F_k$：
$$
\begin{aligned}
\boldsymbol F_k &=\frac{\partial f({\boldsymbol {x}}{k})}{\partial {\boldsymbol {x}}{k}}\
&=\left[\begin{array}{cc}
\boldsymbol I_{4} + {\frac{1}{2}\left(\boldsymbol \Omega_k - \boldsymbol \Omega_k^{bias} \right) \Delta t} & {\boldsymbol O_k} \
{\boldsymbol 0_{2\times4}} & {\boldsymbol I_2}
\end{array}\right]
\end{aligned}
$$
其中矩阵 $\boldsymbol O_k$ 为：
$$
\boldsymbol O_k = \begin{array}{c}
\left[\begin{array}{cc}
\frac{q_{1|k} \Delta t }{2} & \frac{q_{2|k}\Delta t }{2} \
-\frac{q_{0|k} \Delta t}{2} & \frac{q_{3|k}\Delta t }{2} \
-\frac{q_{3|k} \Delta t}{2} & -\frac{q_{0|k}\Delta t }{2} \
\frac{q_{2|k} \Delta t}{2} & -\frac{q_{1|k}\Delta t }{2}
\end{array}\right]
\end{array}
$$
矩阵 $\boldsymbol I_2$ 为：
$$
\boldsymbol I_2 = \begin{array}{c}
\left[\begin{array}{cc}
1&0 \
0&1 \
\end{array}\right]
\end{array}
$$
Matlab推导：

%% 模型推导
syms q0 q1 q2 q3;
syms gx gy gz bx by;
syms dt;
x = [q0;q1;q2;q3;bx;by];
%% 过程模型
omega_x = gx - bx; %%x轴零飘
omega_y = gy - by; %%y轴零飘
omega_z = gz;
Omega = [0 -omega_x -omega_y -omega_z;
         omega_x 0 omega_z -omega_y;
         omega_y -omega_z 0 omega_x;
         omega_z omega_y -omega_x 0];
Q = [q0;q1;q2;q3];
f = [Q + 0.5*Omega*Q*dt;
    bx;
    by]
F = simplify(jacobian(f,x))

观测模型

根据 $\boldsymbol g=\boldsymbol C_{n}^{b}\left[\begin{array}{ccc}0&0&1\end{array}\right]^T$，在无运动加速度情况下有：
$$
\begin{equation}
\left[\begin{array}{lll}
{2\left(q_{1} q_{3}-q_{0} q_{2}\right)} \
{2\left(q_{2} q_{3}+q_{0} q_{1}\right)} \
{1-2\left(q_{1}^{2}+q_{2}^{2}\right)}
\end{array}\right]=
\left[\begin{array}{c}
a_x \
a_y \
a_z
\end{array}\right]
\end{equation}
$$
其中 $[a_x\ a_y\ a_z]^T$ 为归一化后向量，量测模型为：
$$
\begin{equation}
\boldsymbol z_{k} = h(\boldsymbol x_k) + \boldsymbol {v}{k},\quad \boldsymbol {v}{k} \sim N\left(\boldsymbol 0_{3 \times 1}, \boldsymbol {R}k\right)
\end{equation}
$$
其中非线性函数 $h(\boldsymbol x_k)$ 为：
$$
h({\boldsymbol {x}}{k}) =\left[\begin{array}{lll}
{2\left(q_{1|k} q_{3|k}-q_{0|k} q_{2|k}\right)} \
{2\left(q_{2|k} q_{3|k}+q_{0|k} q_{1|k}\right)} \
{1-2\left(q_{1|k}^{2}+q_{2|k}^{2}\right)}
\end{array}\right]
$$
根据四元数单位性质 $q_0^2+q_1^2+q_2^2+q_3^2=1$ 有：
$$
\begin{equation}
\begin{aligned}
{\boldsymbol {H}}k & = \frac{\partial h({\boldsymbol {x}}{k})}{\partial {\boldsymbol {x}}{k}} \
& =\left[\begin{array}{ccccc}
{-2q{2|k}} & {2q_{3|k}} & {-2q_{0|k}} & {2q_{1|k}} & \boldsymbol 0_{1\times2} \
{2q_{1|k}} & {2q_{0|k}} & {2q_{3|k}} & {2q_{2|k}} & \boldsymbol 0_{1\times2} \
{2q_{0|k}} & {-2q_{1|k}} & {-2q_{2|k}} & {2q_{3|k}} & \boldsymbol 0_{1\times2}
\end{array}\right]
\end{aligned}
\end{equation}
$$
另外，重力加速度向量垂直于惯性系，不能用于修正航向角，即状态 $q_3$ 不可观。为避免修正过程破坏航向角准确性，量测更新中修正值 $\boldsymbol K_k(\boldsymbol z_k - h(\hat{\boldsymbol {x}}^-{k}))$ 不能用于修正状态变量 $q_3$，故定义修正量为：
$$
\begin{equation}
\boldsymbol M\boldsymbol K_k(\boldsymbol z_k - h(\hat{\boldsymbol {x}}^-{k}))
\end{equation}
$$
其中矩阵 $\boldsymbol M$ 为：
$$
\boldsymbol M = \left[\begin{array}{cccccc}
1&0&0&0&0&0\
0&1&0&0&0&0\
0&0&1&0&0&0\
0&0&0&0&0&0\
0&0&0&0&1&0\
0&0&0&0&0&1
\end{array}\right]
$$
另外，为避免状态 $q_3$ 估计方差严重发散，矩阵 $\boldsymbol M$ 作用于修正量而非作用于卡尔曼增益 $\boldsymbol K_k$。

Matlab推导：

%% 模型推导
syms q0 q1 q2 q3;
syms gx gy gz bx by;
syms dt;
x = [q0;q1;q2;q3;bx;by];
%% 量测模型
h = [2*(q1*q3 - q0*q2);
     2*(q2*q3 + q0*q1);
     q0^2 - q1^2 - q2^2 + q3^2];
H = simplify(jacobian(h,x))

卡方检验

量测模型的无偏性仅在不存在运动加速度的情况下成立，运动加速度的存在会导致量测更新出现误差。现实中机器人无运动加速度的稳态情况极少出现，为减小跳跃、撞击等情况下过大的运动加速度影响估计精度甚至稳定性，本算法通过卡方检验对加速度计量测信息进行检验。

定义残差：
$$
\begin{equation}
\boldsymbol e_k =\boldsymbol z_k-h(\boldsymbol {x}{k}^-)
\end{equation}
$$
正常情况下，残差 $\boldsymbol e_k$ 符合期望为 $\boldsymbol 0$ 的正态分布，其方差为：
$$
\begin{equation}
\boldsymbol D_k = E[\boldsymbol e_k\boldsymbol e_k^T]=\boldsymbol H_k\boldsymbol P{k}^-\boldsymbol H_k^T + \boldsymbol R_k
\end{equation}
$$
当突然产生较大运动加速度时，残差的值会显著大于一般情况，定义检测函数：
$$
\begin{equation}
r_k = \boldsymbol e_k^T\boldsymbol D_k^{-1}\boldsymbol e_k
\end{equation}
$$
若 $r_k$ 大于阈值，本次量测存在较大运动加速度，则跳过本次量测更新仅进行预测更新。

滤波器发散保护

若工作环境过于恶劣或卡方检验阈值设置过低会导致滤波器长时间不进行量测更新，进而导致协方差矩阵过大导致载体静止状态下残差仍无法通过卡方检验，这种情况需强制进行量测更新。

针对上述问题，本算法通过加速度与角速度测量值大小判断机体是否处于静止状态，若机体处于静止状态且无法通过卡方检验超过50次则设置ConvergeFlag为0，意味滤波器发散，要求暂时跳过卡方检验强制进行量测更新，直到检测函数值小于一定阈值时ConvergeFlag置1，意味滤波器收敛。

增益自适应

若检查函数值较大，但仍能通过卡方检验，可根据检测函数值与卡方检验阈值的大小关系确定一个小于1的比例，作用于卡尔曼增益，以实现估计器增益的自适应。

零偏修正限幅

为避免特殊情况下零偏估计协方差过大导致量测更新中零篇修正值过大产生突变，本算法对量测更新中零偏修正值进行限幅。

记忆渐消因子

考虑到陀螺仪零偏过程模型不完全精确，为避免随时间推移陀螺仪零偏估计协方差过度收敛，导致估计器对陀螺仪零偏无法适应由温度等因素引起的缓慢变化，进而无法得到无偏估计。为解决上述问题，可通过设置估计协方差矩阵对角线元素下限避免过度收敛，或通过渐消因子对协方差阵进行调整。

本算法采用渐消因子避免协方差矩阵过度收敛，定义渐消因子 $\lambda $ 为常数。考虑到仅需对陀螺仪零偏估计进行记忆渐消，故渐消因子 $\lambda $ 并不作用于整个协方差矩阵，定义矩阵 $\boldsymbol \Lambda$：
$$
\boldsymbol \Lambda = \left[\begin{array}{cccccc}
1&0&0&0&0&0\
0&1&0&0&0&0\
0&0&1&0&0&0\
0&0&0&1&0&0\
0&0&0&0&\frac{1}{\lambda}&0\
0&0&0&0&0&\frac{1}{\lambda}
\end{array}\right]
$$
于先验估计协方差更新前作用于协方差矩阵：
$$
\boldsymbol P^*{k+1}=\boldsymbol \Lambda\boldsymbol P{k}
$$

算法更新流程

试验验证

零偏估计

静止状态下开机 $x,y$ 轴零偏估计结果收敛情况如图所示：

静止状态下对比无额外零偏补偿的Mahony算法对俯仰角与横滚角的估计情况如图所示：

其中两条红色曲线为本文算法估计结果，两条噪声较小绿色曲线为无额外零偏补偿的Mahony算法估计结果，蓝色与噪声较大绿色的绿色曲线为加速度计正交分解得到的俯仰角与横滚角。

综上，陀螺仪零偏会使无零偏补偿的Mahony算法估计结果产生偏差，本文算法在静止状态下开机后可有效估计 $x,y$ 轴零偏并得到俯仰角与横滚角的无偏估计结果。

运动加速度影响抑制

通过手持IMU并频繁甩动的方式施加运动加速度，甩动一段时间后使IMU保持静止，观察甩动结束后姿态角收敛情况，若静止后姿态角缓慢变化并最终收敛至与静止瞬间不同的结果，那么收敛值与静止瞬间值的差可以一定程度反应静止前姿态估计误差。

本文算法对比Mahony算法对俯仰角与横滚角的估计情况如图所示：

局部放大如图所示：

其中两条红色曲线为本文算法估计结果，两条噪声较小绿色曲线为无额外零偏补偿的Mahony算法估计结果，蓝色与噪声较大绿色的绿色曲线为加速度计正交分解得到的俯仰角与横滚角（经过一定低通滤波）。

通过将IMU平放于桌面使其水平加减速平移，将IMU拿起旋转，对比估计得到的重力加速度向量在机体坐标系下沿 $xy$ 两轴的分量 $g_x g_y$ 于加速度计测得的合加速度向量在机体坐标系下沿 $xy$ 两轴的分量 $a_x a_y$ 。两组共四条曲线分别为估计的 $g_x g_y$ 与直接测得的 $a_x a_y$ 。