4. 状态向量设计：15维状态向量

好，咱们进入正题。状态向量怎么选？这是EKF设计的第一个关键决策。

我见过不少新手，一上来就拍脑袋定个状态向量，结果飞起来各种飘。说白了，状态向量选错了，后面滤波器调得再好也白搭。

我个人习惯，先想清楚两个问题：我们要估计什么？传感器能提供什么？想明白这两点，状态维数自然就出来了。

4.1 15维状态向量的物理意义

咱们的15维状态向量，长这样：

x = [q0, q1, q2, q3,  // 姿态四元数，4维
     bgx, bgy, bgz,    // 陀螺仪零偏，3维
     px, py, pz,       // 位置，3维
     vx, vy, vz,       // 速度，3维
     bax, bay, baz]    // 加速度计零偏，3维

一共4+3+3+3+3=15维。每个分量的物理意义，我一个个说。

姿态四元数（q0~q3）

四元数描述的是机体坐标系相对于导航坐标系的旋转。为什么用四元数而不是欧拉角？

嗯，这里要注意：欧拉角有万向锁问题。我在做固定翼飞控时就吃过这个亏，俯仰角到90度时，航向和横滚直接乱套。四元数没有这个问题，而且插值平滑，适合做状态估计。

陀螺仪零偏（bgx, bgy, bgz）

陀螺仪测的是角速度，但MEMS陀螺仪都有零偏。这个零偏会随时间缓慢漂移，受温度影响也大。

我记得有一次，无人机起飞前校准得好好的，飞了5分钟后开始偏航。查了半天，原来是陀螺仪被太阳晒热了，零偏漂了0.5度/秒。所以，必须把零偏放进状态向量里实时估计。

位置（px, py, pz）

位置就是无人机在导航坐标系下的三维坐标。一般用GPS或者视觉定位来观测。

速度（vx, vy, vz）

速度是位置的导数。为什么不能只估计位置，然后差分求速度？

你想想看，差分会放大噪声。GPS位置噪声大，差分出来的速度根本没法用。所以速度也要单独估计，用加速度计积分或者光流来辅助。

加速度计零偏（bax, bay, baz）

加速度计也有零偏。这个零偏直接影响速度和位置的积分精度。

我曾经遇到过一架无人机，悬停时高度一直在缓慢下降。排查到最后，发现是加速度计Z轴零偏有0.05g。别小看这0.05g，积分10秒就是0.5m/s的速度误差，再积分10秒就是5米的高度误差。所以加速度计零偏必须在线估计。

4.2 为什么选择15维？

这个问题其实等价于：为什么不多不少，刚好15维？

咱们来做个减法：

• 姿态：最少需要3个自由度（欧拉角），但用四元数需要4维。这是刚需。
• 陀螺仪零偏：3维。不估计的话，姿态会随时间发散。
• 位置：3维。要控制位置，必须估计。
• 速度：3维。要平滑控制，必须估计。
• 加速度计零偏：3维。不估计的话，速度和位置会发散。

加起来就是4+3+3+3+3=16维？等等，我是不是算错了？

其实，四元数虽然用了4个数，但它有一个约束条件：模长必须为1。所以实际自由度为3。但EKF里我们直接用4维状态，然后在更新步骤后做归一化。这样更简单，也避免了奇异性。

所以，15维是最小完备集。少了任何一组，系统都会发散。多了任何一组，计算量增加但收益不大。

4.3 状态向量的可观测性分析

选好了状态向量，还得问一个问题：这些状态能通过传感器观测到吗？

说白了，就是你的传感器能不能提供足够的信息来修正这些状态。

状态	观测来源	可观测性
姿态四元数	加速度计（俯仰、横滚）、磁力计（航向）	强可观测
陀螺仪零偏	姿态误差的长期趋势	弱可观测（需要足够激励）
位置	GPS、视觉定位	强可观测（有GPS时）
速度	GPS多普勒、光流	强可观测
加速度计零偏	速度误差的长期趋势	弱可观测（需要加速度变化）

4.4 状态向量的初始化

状态向量初始化做不好，EKF可能直接发散。我踩过这个坑。

初始化策略：

• 姿态：用加速度计和磁力计做初始对准。加速度计给出俯仰和横滚，磁力计给出航向。
• 陀螺仪零偏：静止时取前100个采样点的平均值。
• 位置：GPS首次定位值，或者设为(0,0,0)。
• 速度：设为0（假设起飞前静止）。
• 加速度计零偏：静止时取前100个采样点的平均值，减去重力加速度。

4.5 知识体系结构图

下面这张图，把15维状态向量的设计逻辑串起来了。我建议你多看几遍，理解每个状态之间的耦合关系。

这张图里，我特意把四个分支用不同颜色区分。你看，姿态和陀螺仪零偏是一对，位置速度和加速度计零偏是另一对。这两对之间通过姿态矩阵耦合在一起。

嗯，这就是15维状态向量的精髓：每个状态都有明确的物理意义，状态之间通过运动学方程和观测方程相互关联。

好了，这一章就到这里。15维状态向量是EKF的骨架，骨架搭好了，后面的肌肉（预测模型）和神经（观测更新）才能长对地方。