课题学习(二十二)---《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor ...》-优快云博客

本文链接：https://blog.youkuaiyun.com/weixin_44412311/article/details/137877927

声明：本人水平有限，博客可能存在部分错误的地方，请广大读者谅解并向本人反馈错误。
论文全称：《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》

一、介绍

1.1 摘要

选择用四元数表示角度位置数据，并使用扩展卡尔曼滤波作为传感器融合算法。为此，设计了一种新的两级滤波器:第一级利用加速度计数据，第二级利用磁力计数据进行角位置校正。这允许灵活性，更少的计算需求，以及对磁场异常的鲁棒性。

1.2 介绍

四元数的使用不存在角奇异性问题，可以更好地线性化系统。如果需要，四元数可以很容易地转换为其他旋转表示方法，如旋转矩阵或欧拉角序列。

二、系统建模理论

在这里插入图片描述

2.1 第一阶段：基于加速度计的扩展卡尔曼滤波

系统有三种不同类型的输入数据:陀螺仪数据测量在三个轴上的角速度(以rad/s为单位)，加速度计数据测量加速度(以g为单位)，磁罗盘数据测量磁场(以Gauss为单位)。同时，利用加速度计和磁力计数据作为固定参考，修正了角度位置估计中的误差。
根据卡尔曼滤波，需要定义一个离散时间状态方程来描述系统状态 $x_k$ 的演化，从前一步 $x_{k-1}$ 处的系统状态开始，用矩阵 $A_k$ 描述状态变化的规律，用矩阵 $B_k$ 响应某些系统输入 $u_k$ :
$\hat{x_k}^-=A_K\hat{x_{k-1}}+B_ku_k------------(1)$
式(1)将给出所谓的“先验”系统状态估计。由于这个原因，系统状态 $x_k$ 有一个上标“-”，而" $\hat{}$ "表示实际系统状态是未知的。
在状态方程中只使用表示角度位置的四元数作为系统状态,增加其他变量不会显著提高角度估计的精度，而且需要显著提高的处理能力。在系统状态中包含额外的变量会导致更大的矩阵运算。
角位置用四元数q表示：
$q=\begin{bmatrix}q_0,q_1,q_2,q_3\end{bmatrix}^T-------------(2)$
四元数由实数和矢量组成: $q_0$ 表示实数， $v=\begin{bmatrix}q_1,q_2,q_3\end{bmatrix}^T$ 表示矢量。
旋转是由旋转轴和绕轴旋转的角度来定义的。向量部分v表示旋转轴，实部表示旋转角α，根据 $q=cos(\frac{\alpha}{2})+sin(\frac{\alpha}{2})*v------------(3)$
为了正确地表示旋转，四元数必须有单位范数。四元数范数是用 $|q|=\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}------------(4)$
采用标准公式将四元数形式的角旋转表示转换为欧拉角形式。选取欧拉角旋转顺序未X–>Y–>Z(作者并未说明参考坐标系，但是应该是北-东-地)，横摇角、俯仰角、偏航角由下式给出:
在这里插入图片描述
在NASA的论文：Euler angles, quaternions, and transformation matrices for space shuttle analysis 中，对各顺序旋转的矩阵进行了推导，此论文的旋转顺序输出为：

状态方程的连续形式为： $\dot{q}_n^b=1/2\Omega_{nb}^nq_n^b------(8)$
$\Omega_n^n$ 为由四元数性质导出的旋转矩阵：
$\Omega_{nb}^=\begin{bmatrix} 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{bmatrix}----(9)$
由陀螺测量的角速度 $\omega_x$ 、 $\omega_y$ 和 $\omega_z$ 被用来填充矩阵(9)。
陀螺仪数据是隐式输入，因为它们用于确定从前一状态导出系统演化的矩阵。得到时变 $A_{TC}=1/2\Omega_{nb}^n$ 矩阵。对于数字卡尔曼滤波器的实现，需要将(8)从这个时间连续系统变为离散系统
为了将该方程转换为离散时间方程，在数字系统中使用了算法每次执行之间的时间步长T。通过这种方式，我们得到： $q_n^b(t+T)=q_n^b(t)+A_{TC}q_n^b(t)T=(I+A_{TC}T)q_n^b(t)--(11)$
离散时间矩阵 $A_k$ 的形式为:
$A_k=(I+A_{TC}T)=(I+1/2\Omega_{nb}^nT)------(12)$
由于只有单位四元数才能正确表示角度位置，为了保证数据的完整性，引入了归一化单元。
在前面的(1)中描述第一部分是“先验的”，方程更新角位置仅使用陀螺仪数据。现在有必要定义卡尔曼滤波器的校正方程。在我们的系统中，有两个修正方程，每个阶段一个:第一个是用加速度计数据，第二个是用磁力计数据。利用卡尔曼滤波理论，修正方程为：
$\hat{x}_k=\hat{x}_k^-+K_k(z_k-H*\hat{x}_k^-)-------(13)$
其中 $z_k$ 表示实际测量值，在我们的目标系统中，是加速度计或磁力计数据。 $H*\hat{x}_k^-$ 是从“先验”系统状态计算出来的预期测量值。实际测量值与期望测量值之间的差称为残差(residual)。残差必须用 $K_k$ 增益加权，以获得校正因子，并计算系统状态 $\hat{x_k}$ 的“后验”估计，这是该概率问题的最小二乘解。在我们的系统中，不可能使用线性关系 $H*\hat{x}_k^-$ 来计算估计的重力或磁场，但有必要使用非线性关系 $h(\hat{x}_k^-)$ 和扩展卡尔曼滤波器。
卡尔曼增益 $K_k$ 的计算是一个相当复杂的任务，涉及到许多矩阵运算。首先，有必要计算“先验”误差协方差 $P_k^-$ ，该协方差表示仅使用“先验”状态方程进行状态估计的误差:
$P_k^-=A_kP_{k-1}A_k^T+Q_{k-1}-------(14)$
式(14)中， $P_{k-1}$ 矩阵为前一次滤波迭代时的“后验”误差协方差矩阵， $Q_{k-1}$ 为过程噪声协方差矩阵。在该系统中， $Q_{k-1}$ 协方差矩阵直接依赖于陀螺噪声和其他陀螺误差源，因为系统演化是由包含陀螺数据项的 $A_k$ 矩阵描述的。但是， $Q_{k-1}$ 值并不代表陀螺噪声，而是与系统演化过程中的噪声有关。
在扩展卡尔曼滤波器中，卡尔曼增益用：
$K_k=P_k^-H_k^T(H_kP_k^-H_k^T+V_kR_kV_k^T)^{-1}----(15)$
其中 $R_k$ 为测量噪声协方差矩阵，直接取决于加速度计和磁传感器的噪声，以及被认为是噪声的其他误差源。 $H_k$ 和 $V_k$ 是相对于四元数和非线性方程h1和h2噪声的偏导数的雅可比矩阵。这些方程将四元数与估计的重力和估计的磁场联系起来。由于这些方程的非线性，需要使用扩展卡尔曼滤波器。
最后，需要计算“后验”误差协方差矩阵Pk，用于后续的滤波器迭代:
$P_k=(I+K_kH_k)P_k^--------(16)$
既然已经解释了卡尔曼滤波理论，就可以看到它是如何应用于本文所描述的系统的。
第一个校正阶段使用加速度计的数据来校正系统状态:从估计的角度位置计算期望的重力矢量，并从加速度计的测量值中减去，得到所谓的残差。假设重力加速度|g|为常数，使用方向余弦矩阵 $R_n^b$ 来计算估计的重力矢量g :
$h_1(q_k)=\hat{g}=R_n^b\begin{bmatrix}0\\0\\|g| \end{bmatrix}=|g|\begin{bmatrix}2(q_{1}q_{3}-q_{2}q_{0})\\ 2(q_{2}q_{3}+q_{1}q_{0})\\ q^2_{0}-q^2_{1}-q^2_{2}+q^2_{3}\end{bmatrix}-----(17)$
其中 $R_n^b=\begin{bmatrix} q^2_{0}+q^2_{1}-q^2_{2}-q^2_{3} &2(q_{1}q_{2}+q_{3}q_{0}) &2(q_{1}q_{3}-q_{2}q_{0})\\ 2(q_{1}q_{2}-q_{3}q_{0})&q^2_{0}-q^2_{1}+q^2_{2}-q^2_{3}&2(q_{2}q_{3}+q_{1}q_{0})\\ 2(q_{1}q_{3}+q_{2}q_{0})&2(q_{2}q_{3}-q_{1}q_{0})&q^2_{0}-q^2_{1}-q^2_{2}+q^2_{3} \end{bmatrix}--(18)$
当系统处于动态状态时，由于加速度计不仅测量重力，而且还测量外部加速度，因此测量的重力误差很大。因此，残差必须用卡尔曼增益 $K_k$ 加权，该系数是根据系统噪声协方差矩阵的统计量计算出来的。为了计算卡尔曼增益，需要计算雅可比矩阵 $H_{k1}$ :
$H_{k1}=\frac{\partial{h_{1[i]}}}{\partial{q_{[j]}}}=\begin{bmatrix} -2q_2&2q_3&-2q_0&2q_1 \\2q_1&2q_0&2q_3&2q_2 \\2q_0&-2q_1&-2q_2&2q_3 \end{bmatrix}----(19)$
加速度计(以及磁罗经)的噪声被认为与当前的角度位置无关。因此，雅可比矩阵 $V_k$ 是一个单位矩阵。
通过重力矢量测量，可以只校正滚转和俯仰角。为了保证偏航角不受适用修正量的影响，将修正四元数 $x_{\in1}$ 的第三个矢量部分设为零:
$x_{\in 1}=x_{\in {1,0}}+x_{\in {1,1}}+x_{\in {1,2}}+0*x_{\in {1,3}}---(20)$
下图是卡尔曼滤波算法的流程图：

2.2 第二阶段：基于磁力计的扩展卡尔曼滤波

滤波器的第二阶段与第一阶段类似:用相同的算法计算第二个卡尔曼增益 $K_{k2}$ ，但使用不同的噪声协方差矩阵。归一化为1的磁场被认为只沿y轴方向，而垂直分量被忽略，因为地球磁场的强度从0.25到0.65 G不等，垂直分量与地理位置有关。在滤波器中，用下式估计磁场:
$h_2(q_k)=\hat{m}=R_n^b\begin{bmatrix}0\\1\\0 \end{bmatrix}=\begin{bmatrix}2(q_{1}q_{2}+q_{0}q_{3})\\ q^2_{0}-q^2_{1}+q^2_{2}-q^2_{3}\\ 2(q_{2}q_{3}-q_{1}q_{0})\end{bmatrix}---(21)$
雅可比矩阵 $H_{k2}$ 如下:
$H_{k2}=\frac{\partial{h_{2[i]}}}{\partial{q_{[j]}}}=\begin{bmatrix} 2q_3&2q_2&2q_1&2q_0 \\2q_0&-2q_1&2q_2&-2q_3\\-2q_1&-2q_0&2q_3&2q_2 \end{bmatrix}---(22)$
在这种情况下，为了确保磁异常不影响横摇和俯仰估计，而只影响偏航角，校正四元数 $x_{\in2}$ 的前两个矢量部分被设置为零:
$x_{\in 2}=x_{\in {2,0}}+0*x_{\in {2,1}}+0*x_{\in {2,2}}+x_{\in {2,3}}---(23)$
要注意的是，原论文有俩公式写错了：
在这里插入图片描述

2.3 算法步骤

读取陀螺传感器，得到角速度 $\omega_x、\omega_y、\omega_z$ ;
计算离散时间状态转移矩阵： $A_k=I+1/2\Omega_{nb}^nT$ ;
计算“先验”状态估计： $\hat{x_k}^-=A_K\hat{x_{k-1}}$ ;
计算先验噪声协方差矩阵： $P_k^-=A_kP_{k-1}A_k^T+Q_{k}$
计算雅可比矩阵： $H_{k1}=\begin{bmatrix} -2q_2&2q_3&-2q_0&2q_1 \\2q_1&2q_0&2q_3&2q_2 \\2q_0&-2q_1&-2q_2&2q_3 \end{bmatrix}$ ;
计算卡尔曼增益： $K_{k1}=P_k^-H_{k1}^T(H_{k1}P_k^-H_{k1}^T+V_{k1}R_{k1}V_{k1}^T)^{-1}$ ；
读取加速度计数据 $z_{k1}=a_x,a_y,a_z$ ;
计算 $h_1(\hat{x_k^-})$ ： $h_1(\hat{x_k^-})=\begin{bmatrix}2(q_{1}q_{3}-q_{2}q_{0})\\ 2(q_{2}q_{3}+q_{1}q_{0})\\ q^2_{0}-q^2_{1}-q^2_{2}+q^2_{3}\end{bmatrix}$ ;
修正系数的计算 $x_{\in1}=K_{k1}(z_{k1}-h_1(\hat{q_k^-}))$ , $x_{\in{1,3}}=0$ (即 $q_3$ =0);
计算“后验”状态估计： $\hat{x_{k1}}=\hat{x_k^-}+x_{\in1}$ ;
计算“后验”误差协方差矩阵： $P_{k1}=(I-K_{k1}H_{k1})P_k^-$ ；
计算雅可比矩阵： $H_{k2}=\begin{bmatrix} 2q_3&2q_2&2q_1&2q_0 \\2q_0&-2q_1&2q_2&-2q_3\\-2q_1&-2q_0&2q_3&2q_2 \end{bmatrix}$ ;
计算卡尔曼增益： $K_{k2}=P_k^-H_{k2}^T(H_{k2}P_k^-H_{k2}^T+V_{k2}R_{k2}V_{k2}^T)^{-1}$ ；
读取磁力计数据 $z_{k2}=m_x,m_y,m_z$ ;
计算 $h_2(\hat{x_k^-})$ ： $h_2(\hat{x_k^-})=\begin{bmatrix}2(q_{1}q_{2}+q_{0}q_{3})\\ q^2_{0}-q^2_{1}+q^2_{2}-q^2_{3}\\ 2(q_{2}q_{3}-q_{1}q_{0})\end{bmatrix}$ ;
修正系数的计算 $x_{\in2}=K_{k2}(z_{k2}-h_2(\hat{q_k^-}))$ , $x_{\in{2,1}}=0,x_{\in{2,2}}=0$ (即 $q_1=0、q_2=0$ );
计算“后验”状态估计： $\hat{x_{k}}=\hat{x_{k1}}+x_{\in2}$ ;
计算“后验”误差协方差矩阵： $P_{k}=(I-K_{k2}H_{k2})P_{k1}$ ；

上述步骤中，(1)-(4)为获取陀螺仪数据进行先验状态估计；(5)-(11)为算法的第一阶段；(12)-(18)为算法的第二阶段。

三、面向硬件实现的算法简化

3.1 声明

说实话，这部分我其实没太看明白，但是要满足在MCU上实现上述算法的要求，这部分还是比较重要的，所以硬着头皮看一下。
此外说明一点，我会把原论文通过引用的方式放在博客上，并在下面进行注解。

3.2 论文

The first simplification consists of approximating the“a priori” error covariance $P_k^-$ with the “a posteriori” error covariance Pk. This is a logical simplification if the angular position estimated before the correction can be estimated with the angular position after the correction. This assumption is valid if the filter is iterated at high frequency, with very low corrections at each iteration.

译文：第一种简化是用“后验”误差协方差Pk近似“先验”误差协方差 $P_k^-$ 。如果校正前估计的角位置可以用校正后的角位置估计，这是一种逻辑简化。如果滤波器以高频率迭代，并且每次迭代的校正非常低，则此假设是有效的。

To reduce the number of matrix multiplications, a precomputed value of Pk is used. Instead of calculating this parameter online from the noise statistics of the sensor, its value is assigned offline as a tuning parameter of the filter, assuming that similar sensors will have similar noise statistics. During normal filter operation, the initial value of Pk is greater because of the uncertain initial angular position. It then converges to lower values. The final value of P was calculated experimentally with different simulations on Matlab Simulink system model. To simulate the fact that the initial value of P is higher and to allow a fast initial position correction even if the system starts upside down, it was chosen to initialize the filter with a higher value of the Pk matrix and to scale down this value with a step function to the nominal value after the first 128 filter iterations. The same value of Pk is used for the two filter stages.

译文：为了减少矩阵乘法的次数，使用预先计算的Pk值。假设相似的传感器具有相似的噪声统计数据，它的值作为滤波器的调优参数离线分配，而不是从传感器的噪声统计数据在线计算该参数。在正常的滤波操作中，由于初始角度位置的不确定，Pk的初始值较大。然后收敛到更低的值。在Matlab Simulink系统模型上通过不同的仿真实验计算了P的最终值。为了模拟P的初始值较高的事实，并允许即使系统倒立启动也能快速初始位置校正，选择以较高的 $P_k$ 值初始化滤波器，并且在第一个128个过滤器迭代之后，用阶跃函数将这个值缩小到标称值。两个过滤级使用相同的Pk值。

The following simplifications are important, because they allow eliminating all the division operations in the filter algorithm. As the matrix Pk is now a fixed parameter, it is possible to use a fixed value as the matrix determinant during matrix inversion. Moreover, it is possible to eliminate the normalization operation on the quaternion when the Kalman filter is working, because it operates as a self-normalization algorithm on the quaternion. This property was experimentally verified with many simulations on the simplified Simulink model of the system, but it does not apply, in general, for the full Kalman algorithm. With these simplifications, further matrix algebra manipulation, and a fine tuning of the operations computed from the hardware, it was possible to reduce the number of multiplications and additions by up to 60% with negligible performance degradation compared to the initial filter algorithm.

译文：下面的简化很重要，因为它们允许消除过滤器算法中的所有除法操作。由于矩阵Pk现在是一个固定参数，所以在矩阵求逆时，可以使用固定值作为矩阵行列式。此外，当卡尔曼滤波工作时，可以消除对四元数的归一化操作，因为它是对四元数的自归一化算法。在简化的系统Simulink模型上进行了多次仿真实验，验证了这一特性，但通常不适用于完整的卡尔曼算法。通过这些简化，进一步的矩阵代数操作，以及对硬件计算的操作进行微调，与初始过滤算法相比，可以将乘法和加法的数量减少多达60%，而性能下降可以忽略不计。

In this simplified system, the error covariance matrices Q,R1, and R2 are considered constant and have the following values:

在此简化系统中，误差协方差矩阵Q、R1、R2被认为是常数，其值如下:
在这里插入图片描述

The error covariance matrix Pk has the value P0 at the filter start-up and for the first 128 iterations, while its value is decreased to P1 with a step response for normal filter operation

译文：在滤波器启动和前128次迭代时，误差协方差矩阵Pk的值为P0，而对于正常的滤波器运行，其值随着阶跃响应减小为P1。
在这里插入图片描述