A Double-Stage Kalman Filter for Orientation Tracking With An Integrated Processor in 9-D IMU 论文精读

个人翻译仅供参考，如有不到位敬请谅解

卡尔曼：

卡尔曼滤波器：

卡尔曼滤波器（估计器）1 - 知乎

增益公式推导：

卡尔曼增益推导 - 知乎

基于EKF的姿态解算：

基于EKF的姿态解算 - 知乎

MATLAB复现

GitHub - waihekor/EKF_AHRS: It is a EKF implemention of ADIS16470 and RM3100

用于9D IMU中集成处理器定向跟踪的双阶段卡尔曼滤波器

摘要

—本文介绍了一种用于9D惯性测量单元的角度估计系统的专用集成处理器。 专用指令集处理器（ASIP）在现场可编程门阵列上实现，并与陀螺加加速度计6-D传感器和磁罗经接口。 输出数据记录在个人计算机上，还用于执行实时演示。 在系统建模和设计期间，选择使用四元数表示角位置数据，并使用扩展的卡尔曼滤波器作为传感器融合算法。为此目的，设计了一种新颖的两级滤波器：第一级使用加速度计数据，第二级使用磁罗盘数据进行角度位置校正。这允许灵活性，较少的计算需求以及对磁场异常的鲁棒性。这项工作的最终目标是实现升级的专用集成电路，该集成电路可控制微机电系统（MEMS）传感器并集成ASIP。这将使MEMS传感器陀螺仪和加速度计以及角度估计系统包含在一个封装中；该系统可以选择使用外部磁罗经。

索引词-角位置，专用指令集处理器（ASIP），惯性测量单元（IMU），卡尔曼滤波器，方向跟踪，四元数，传感器融合。

一、引言

当前，由于其低成本，小尺寸和低功耗，微机电系统（MEMS）传感器被广泛用于各种消费应用中。在工业研究中，人们对MEMS系统的其他功能和更高的精度产生了广泛的兴趣。在科学文献中，传感器融合算法引起了很多关注，因为它们允许处理来自多个传感器的原始数据以获得更多信息或提高精度。

估计角度位置的IMU单位。从测得的加速度中减去预期的重力矢量，并以双积分计算IMU的位置。主要问题是，随着双重集成，误差会迅速增长，因此，如果没有GPS信号的帮助或没有速度或运动的限制，则该系统在几秒钟内是不可靠的。例如，在导航应用中，可以在建筑地图的约束范围内计算传感器的运动； 在里程表和英尺计数器中，如果将传感器放置在鞋子上，则加速度计将测量脚着地时的峰值，可以将其总和视为此时的速度为零，因此积分误差可以定期纠正。

在处理现有航位推算单元的工作中[1]分析表明，此类系统中的主要误差源不是加速度计噪声，而是从测得的加速度中减去的不正确的重力矢量（由于角度估计不准确）。因此，这项工作集中在改进的角度估计系统上。该系统的主要问题是对陀螺仪数据使用积分器会导致一段时间内陀螺仪误差之和，因此必须引入校正因子。特别是，从理论上可以知道，陀螺仪角度随机游走，即高斯白噪声，会在角度估计中引入误差，该误差会随时间的平方根增长，而由于校准误差，热误差或热误差而导致的陀螺仪偏置不稳定和恒定偏置。焊接效果会导致误差随时间线性增长。

图1

先前已经公开了许多角度估计系统。大多数论文使用四元数来表示角度位置，因为它们比欧拉角更灵活地表示角度位置。而且，通过使用四元数，不存在角度奇异性的问题，并且可以使系统更好地线性化。如果需要，四元数可以轻松转换为其他旋转表示方法，例如旋转矩阵或一系列Euler角。为了消除由于陀螺仪误差引起的角度估计漂移的影响，所有研究都使用了某种传感器融合算法。卡尔曼滤波器是最常用的。在[2]中，扩展卡尔曼滤波器用于三个速率陀螺仪和三个加速度计，状态方程由四元数以及角速度和陀螺仪漂移组成。所提出的系统对侧倾角和俯仰角具有良好的估计，但是对偏航角的校正很小。为了对偏航角进行完整的校正，需要一个磁罗盘。实际上，在[3]和[4]中，完整的磁性，角门和重力传感器用于角度估计。为了简化卡尔曼滤波器的设计，在这两项工作中，提出了一种额外的算法，可以从加速度计和磁性罗盘数据估计表示角位置的四元数，从而可以使用线性卡尔曼滤波器。特别是，在[3]中提出了一种高斯-牛顿法，在[4]中提出了一种四元数估计（QUEST）算法。卡尔曼滤波器的自适应增益在[5]中用于在传感器处于高加速度模式或安静状态时调整角度校正。代替扩展的卡尔曼滤波器，[6]使用了无味的卡尔曼滤波器，因为该滤波器被认为更准确且实现成本更低。然而，在[7]中，在扩展过滤器和无味过滤器之间进行了比较，尽管无味过滤器需要更多的计算时间，但发现的精度却相当。

之前引用的所有著作均使用离散传感器系统，并为陀螺仪，加速计和磁罗经分别配备传感器，并且滤波算法在微控制器或个人计算机（PC）上进行处理。 这种角度估计系统的商业示例可以在[8]中找到，其中使用三个独立的传感器和一个微控制器进行角度估计。在某些模型中，还可以将该系统与GPS结合起来，用于航位推算系统。

在本文中，我们专注于在单个程序包中实现完整的系统。因此，从SensorDynamics SD746 6-D IMU的现有原型开始，开发了一个系统，该系统只能使用陀螺仪和加速度计数据来运行滤波器，并且可以选择与外部磁罗盘连接。 两级滤波器提供了更大的操作灵活性，因为可以打开或关闭第二级以包括或排除电磁罗盘校正。使用加速度计数据，可以仅正确估计侧倾角和俯仰角，而使用电磁罗盘时，也可以正确估计偏航角。这种方法还可以简化操作：将系统分为两个阶段，可以使用较小的矩阵进行操作，而所需的计算能力则较小。这种两级滤波器还可以分离磁异常对侧倾角和俯仰角的估计精度的影响，因为在这种情况下，仅影响偏航角估计[11]。我们的最终目标是在专用于控制陀螺仪和加速度计传感器的集成电路（ASIC）中开发和集成该角度估计系统。外部磁罗经可以通过内部集成电路或两线接口（I2C）总线连接。为了实现这一点，我们将面积限制为0.5 mm2。

本文的组织如下。在本导论之后，在第二节中，将解释用于角度估计系统模型的数学理论，在第三节中，说明专用指令集处理器（ASIP）的设计和体系结构，最后在第四节中， 介绍了现场可编程门阵列（FPGA）原型和测试结果。

二、系统建模理论

系统模型是使用MathWorks Simulink开发的。这允许使用人造数据集和实际采集的数据进行快速的系统开发和测试。

该系统具有三种不同类型的输入数据，如图1所示：陀螺仪数据测量三个轴中每一个轴上的每秒角速度，加速度计数据测量以克为单位的加速度，以及磁通 测量高斯磁场的数据。随后将解释如何将陀螺仪数据用作更新角位置的输入。 同时，将加速度计和磁罗经数据作为固定参考来校正角度位置估计中的误差。

根据卡尔曼滤波器理论（见[14]），有必要定义一个离散时间状态方程，该方程描述系统状态xk的演化，从上一步xk-1的系统状态开始，由矩阵Ak描述的演化定律，并用演化定律Bk响应某些系统输入uk

等式（1）将给出所谓的“先验”系统状态估计。因此，系统状态xk具有超级脚本“减”，而帽子表示真实的系统状态未知，而卡尔曼滤波器提供了估计。

在状态方程中仅使用表示角位置作为系统状态的四元数。尽管添加其他变量不会显着提高角度估计的精度，但它需要显着更大的处理能力。在系统状态中包含额外的变量会导致更大的矩阵运算。使用它们可能可以解释陀螺仪的偏向，但是根据我们的模拟，结果导致精度提高不到1°。

角位置用四元数q表示

四元数由实数和向量组成：q0代表实数，而v = [q1，q2，q3] T代表向量。

旋转由旋转轴和围绕该轴的旋转角度α定义。向量部分v代表旋转轴，而实数部分代表根据

为了正确表示旋转，四元数必须具有单位范数。

四元数范数用

使用标准公式将角旋转表示从四元数转换为欧拉角形式。选择了欧拉角的XY Z序列，并且滚动，俯仰和偏航角由以下公式给出：

时间连续形式的状态方程为

其中，qbn是代表与惯性n框架相关联的IMU传感器的车身框架旋转的四元数，而Ωnnb是从四元数的属性导出的旋转矩阵（请参见[12]和[13]  ）

陀螺仪测得的角速度ωx，ωy和ωz用于填充矩阵（9）。在这个特定的系统中，不存在具有演化定律B的明确的外部输入uk [请参阅（1）]。陀螺仪数据是隐式输入，因为它们用于确定从前一状态派生系统演化的矩阵。由此获得了时变的ATC ＝（1/2）Ωnb的矩阵。对于数字卡尔曼滤波器的实现，有必要将（8）从该时间连续版本转换为时间离散版本。通过明确微分运算，连续时间系统可以写成

为了将该方程转换为离散时间方程，使用了数字系统中算法每次执行之间的时间步长T。通过这种方式，我们获得

离散时间矩阵Ak具有如下形式

四元数范数不保留在状态方程中。由于只能使用单元四元数正确表示角度位置，因此引入了标准化单元以确保数据完整性。

在（1）中较早描述的第一部分是仅使用陀螺仪数据更新角度位置的预测方程或“先验”方程。 现在必须定义卡尔曼滤波器的校正方程式。 在我们的系统中，有两个校正方程式，每个阶段都有一个校正方程式：第一个使用加速度计数据运行，第二个使用磁罗盘数据运行。使用卡尔曼滤波理论，校正方程的结果如下

其中zk代表实际测量值，在建议的系统中，它是加速度计或指南针数据。  Hxk是从“先验”系统状态计算得出的预期测量值。 实际和预期测量之间的差异称为残差。必须用Kk增益对残差进行加权，以获得校正因子并计算系统状态xˆk的“后验”估计，这是对该概率问题的最小二乘解。在我们的系统中，不可能使用线性关系Hxˆ-k来计算估计的重力或磁场，但是有必要使用非线性关系h（ˆx-k）和扩展的卡尔曼滤波器。 这是因为我们具有二阶多项式[请参阅（17）和（21）]。

卡尔曼增益Kk的计算是一个非常复杂的任务，涉及许多矩阵运算。首先，有必要仅使用“先验”状态方程来计算代表状态估计中误差的“先验”误差协方差P -k。

在（14）中，Pk-1矩阵是先前滤波器迭代时的“后验”误差协方差矩阵[见（16）]，而Qk-1是过程噪声协方差矩阵。在此系统中，Qk-1协方差矩阵直接取决于陀螺仪噪声和其他陀螺仪误差源，因为系统演化是由包含陀螺仪数据项的Ak矩阵描述的。但是，Qk-1值不代表陀螺仪噪声，而是与系统演进过程中的噪声有关。

在扩展卡尔曼滤波器中，卡尔曼增益的计算公式为

其中Rk是测量噪声协方差矩阵，直接取决于加速度计和磁传感器的噪声以及其他被视为噪声的误差源。Hk和Vk是相对于四元数和非线性方程h1和h2 [[17]和（21）]的偏导数的Jacobian矩阵。这些方程将四元数与估计的重力和估计的磁场相关联。由于这些方程式的非线性，需要使用扩展的卡尔曼滤波器。

最后，必须计算“后验”误差协方差矩阵Pk，以用于后续的滤波器迭代

既然已经说明了卡尔曼滤波器理论，就有可能看到它如何应用于本文所述的系统。第一校正阶段使用来自加速度计的数据来校正系统状态：预期重力矢量是从估算的角位置计算出来的，并从加速度计测得的值中减去，以获得所谓的残差。 假定重力加速度| g |，使用方向余弦矩阵Rbn计算估计的重力矢量gˆ。 不变

如果系统处于动态状态，则加速度计不仅会测量重力，而且还会测量外部加速度，因此所测得的重力会遭受重大误差。因此，必须使用卡尔曼增益Kk1对残差进行加权，该系数是根据系统的噪声协方差矩阵的统计数据计算得出的。为了计算卡尔曼增益，有必要计算雅可比矩阵Hk1

加速度计（以及电磁罗盘）的噪声被认为与当前角度位置无关。因此，雅可比矩阵Vk是单位矩阵。

通过重力矢量测量，可以仅校正侧倾角和俯仰角。为了确保在适用的校正中不受偏航角的影响，将校正四元数q 1的第三矢量部分设置为零

滤波器的第二阶段与第一阶段相似：第二卡尔曼增益Kk2用相同的算法但使用不同的噪声协方差矩阵来计算。归一化为1的磁场被认为仅沿y轴定向，而垂直分量则被忽略，因为地球磁场的强度在0.25至0.65 G之间变化，并且垂直分量在地理位置上是相关的。在滤波器中，磁场的估计公式如下：

雅可比矩阵Hk2为：

在这种情况下，为了确保磁异常不影响滚动和俯仰估计，而仅影响偏航角，将校正四元数q 2的前两个矢量部分设置为零

图2

完整的算法总结如下（示意性轮廓另请参见图2）

开始“先验”系统估算：

1. 读取陀螺仪传感器并获得角速度关系ωx，ωy和ωz；
2. 计算离散时间状态转移矩阵Ak = I +（1/2）Ωnnb T；
3. 计算“先验”系统状态估计qk- = Ak qk-1；
4. 计算“先验”噪声协方差矩阵P-k = Ak Pk-1 ATk + Qk。

校正阶段1的开始：

1. 计算雅可比矩阵Hk1 =

 2. 卡尔曼增益的计算:Kk1 = P −k HTk1(Hk1P −k HTk1 + Vk1Rk1V Tk1)−1;

 3. 读取加速度计数据:zk1 = axay，az;

 4. 计算

  5. 校正因子的计算q1 = Kk1(zk1 − h1(ˆq−k )),  q1、3等于零；

  6. 后验状态估计的计算

qˆk1 = qˆ−k + q1;

   7. 后验误差协方差矩阵的计算Pk1 = (I − Kk1Hk1)P −k .

校正阶段2的开始：

1. 计算雅可比矩阵的计算Hk2 =

2. 卡尔曼增益Kk2的计算 Kk2 = P −k HTk2(Hk2P −k HTk2 + Vk2Rk2V Tk2)−1;

3. 读取磁力计数据zk2 = mxmy, mz;

4. h2（ˆq-k）的计算

5. 校正因子的计算

q2 = Kk2(zk2 − h2(ˆq−k ))，  q 2,1和q 2,2等于零；

6. 后验状态估计的计算qˆk = qˆk1 + q2;

7. 后验误差协方差矩阵的计算Pk = (I − Kk2Hk2)Pk1.

三、 简化算法以实现硬件

为了创建一个占地面积小，功耗低的集成系统，简化了第二节中介绍的卡尔曼滤波算法。目的是在只能进行乘法和求和运算的简化算术逻辑单元（ALU）上处理滤波器。

第一种简化包括用“后验”误差协方差Pk近似“先验”误差协方差P -k。如果可以用校正后的角位置来估计校正前的角位置，则这是逻辑上的简化。如果滤波器是高频迭代的，则该假设是正确的，并且每次迭代的校正都非常低。

为了减少矩阵乘法的次数，使用了Pk的预计算值。假设类似的传感器具有类似的噪声统计信息，则可以离线分配其值作为滤波器的调整参数，而不是根据传感器的噪声统计信息在线计算该参数。在正常的滤波器操作期间，由于不确定的初始角位置，Pk的初始值较大。 然后收敛到较低的值。 P的最终值是在Matlab Simulink系统模型上通过不同的模拟实验计算得出的。为了模拟P的初始值较高的事实，并且即使系统上下颠倒启动，也可以进行快速的初始位置校正，选择使用较高的Pk矩阵值初始化滤波器，并使用 在前128次滤波器迭代之后，将步长函数提高到标称值。Pk的相同值用于两个滤波器级。

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

在此简化的系统中，误差协方差矩阵Q，R1和R2被认为是常数，并具有以下值：

误差协方差矩阵Pk在滤波器启动时和前128次迭代中具有值P0，而对于正常的滤波器操作，其阶跃响应将其值减小至P1

四、ASIP设计与合成

在开始数字设计之前，必须定义表示数据所需的位数，并从浮点算术的简化模型中得出定点算术的位真模型。目的是使用16-b算法来匹配来自陀螺仪和加速度计的输入数据。但是，在我们的自定义设计中，为指定的操作使用量身定制的位数并不是问题。

通过可重复的测试获取数据，并使用系统的Matlab Simulink模型离线处理数据。该模型允许仿真将在FPGA或CMOS标准单元技术中实现的集成处理器中使用的位真算法。Simulink模型可以对系统的行为进行全面仿真，并对处理器体系结构的关键参数进行微调。在这种情况下，它用于模拟使用不同位数表示内部数据的定点算法的效果。使用在测试过程中获得的数据，可计算出侧倾角的RMS误差。表I中报告了定点算法的结果。为了进行比较，浮点算术模型导致RMS误差为0.17°。

如表I所示，将16 b用于定点运算会导致角度估计的精度降低。尽管使用更多位数可获得最佳结果，但根据比率精度/成本来评估最佳值很有用。用处理单元实现处理器的成本是n2的函数，其中n是所需的位数。对于定点算法，使用20 b可获得精度和成本之间的最佳折衷。为了进一步优化系统，在仿真过程中使用了不同数量的位来表示不同种类的数据。将数据分为几组，每组具有参数化的精度位数：四元数，状态更新矩阵A，K矩阵，估计的重力和磁场以及简化的Kalman滤波器的其他内部变量。仿真表明，精度的关键数据是四元数和A矩阵。将定点算法中使用的位数减少到16不会导致RMS误差大幅增加。使用20 b表示四元数和状态更新矩阵A数据，使用16 b表示所有其他数据，导致RMS误差为0.27，而将所有数据用作20 b则为0.26。

要考虑的第二个问题是要实现的体系结构。考虑了完全并行的实现，但由于占用面积而被认为不合适。最好有一个灵活的系统，以便可以为将来的系统扩展或算法改进轻松地更新当前设计。选择了具有通用ALU和精简指令集计算机（RISC）架构的ASIP架构。可以实现RISC架构是因为要执行的指令数量远远少于可用的时钟周期数量：估计的ASIP频率为1 MHz，合适的滤波器更新速率为1 kHz。

控制单元选择了硬连线和编程解决方案之间的混合方法，定义了自定义指令集体系结构，并将要执行的指令存储在ROM中。将来，仅通过更改ROM即可更新系统。

该系统分为三个主要部分：控制单元，带ALU的计算单元以及由regbank组成的存储器（见图3）。在正常运行期间，从IMU传感器输入的数据，陀螺率和加速度在regbank中获取；然后执行滤波算法，并将输出四元数存储在regbank中。

该计算单元能够对20位输入数据进行求和，乘法，乘法器和累加器运算。 计算单元的每条指令分两个步骤执行：获取和解码操作以及执行和写操作。  regbank由两个寄存器组组成。计算单元从每个组中读取一个寄存器。计算单元的输出数据在20b上。对于非关键数据，截断单元允许将数据存储在16位寄存器中，从而节省了面积并降低了功耗。

控制单元允许在设置完成时（即在开始新的滤波器迭代之前）为系统设置特定的滤波器频率并为系统设置低功耗状态。大多数时候，集成处理器处于低功耗状态，因此时钟门控既可以减少开关活动，又可以降低功耗。 包含指令的ROM是按顺序读取的，但是也可以为将来的升级设置一些跳转条件。