14、基于卡尔曼滤波的两轴姿态估计

基于卡尔曼滤波的两轴姿态估计

1. 四元数基础

在相关讨论中，四元数的表示有其特定规则。四元数中，$q_0$ 通常被称作“标量部分”，而 $[q_1, q_2, q_3]$ 则组合成一个三维向量，被称为“向量部分”。在将四元数的四个元素写成列向量时，不同作者有不同的写法，有的将标量部分 $q_0$ 置于顶部，有的则放在底部，这可能会造成混淆。

四元数除了满足特定条件外，还存在多种运算，如加法和乘法。四元数加法与标准矩阵加法类似，但四元数乘法的定义与矩阵乘法不同，对于两个 4×1 维的矩阵，矩阵乘法是未定义的。

2. 陀螺仪信号能否指示 IMU 当前姿态

典型的微型惯性测量单元（IMU）中的陀螺仪（“速率陀螺仪”）会为我们提供反映 IMU 绕构成其本体参考系各轴旋转速度的数值。不过，这些数值只是瞬时的旋转速度近似值，单个数值本身并不能代表 IMU 的当前姿态，它们仅能告知我们在某一时刻 IMU 的旋转速度。

要确定 IMU 的实际“当前方向”，需要计算从初始状态（本体坐标系与惯性坐标系对齐）开始累积的总旋转量。这就如同确定百米赛跑中运动员在跑道上的位置一样。如果我们每秒能获取一次运动员的速度估计值，并假设在每个不同的一秒间隔内速度是恒定的，那么在比赛开始后的第一秒，当速度读数为 5 m/s 时，我们可以估计运动员距离起跑线 5 米；两秒后速度读数为 7 m/s，此时可估计运动员距离起跑线 5 + 7 = 12 米；三秒后速度读数为 9 m/s，那么三秒后运动员距离起跑线的估计值为 5 + 7 + 9 = 21 米，以此类推。

卡尔曼滤波模型会表达从一个给定采样时刻到下一个时刻，我们所关注的变量（即 IMU 姿态，用四元