姿态与状态估计：互补滤波、Mahony、Madgwick、EKF、UKF、MSCKF、粒子滤波
姿态与状态估计：互补滤波、Mahony、Madgwick、EKF、UKF、MSCKF、粒子滤波
做机器人，尤其是四足、无人机这类要「知道自己在哪、姿态什么样」的机器，状态估计是绕不过去的一关。
IMU 很便宜，但它给的是角速度和加速度，不是姿态、更不是速度。你想知道机身现在朝哪、在往哪动、走多快，都得靠「估」。于是就有了一整排算法：互补滤波、Mahony、Madgwick、EKF、UKF、MSCKF、粒子滤波……名字一堆，经常分不清谁是谁、什么时候该用谁。
这篇文章把它们从头到尾捋一遍，讲清每个算法在解决什么问题、核心思想是什么、数学上怎么推出来的、彼此什么关系，最后落到四足机器人的实际场景。我自己在四足机器人上用 BMI088 做过 Mahony 姿态估计，互补滤波那部分会带上真实代码。
先理清：状态估计到底在估什么
以四足机器人为例，状态估计要回答三类问题：
- 姿态：机身当前的 roll / pitch / yaw，或者用四元数、旋转矩阵表示。
- 线速度 / 位置：机身往哪动、走多快、到哪了。
- 零偏：陀螺和加速度计都有零偏（bias），而且随温度漂移，得在线估出来，否则积分一下就废。
这三类量里，姿态最容易，光靠 IMU 就能估个八九不离十；线速度和位置难，单靠 IMU 积分会漂，必须引入别的观测（腿部里程计、视觉、GPS）来校正。理解了这条主线，下面每个算法该放在哪就清楚了。
互补滤波：把两个传感器「拼」起来
互补滤波是所有姿态估计里最朴素的思想，先把它搞懂，后面的 Mahony、Madgwick 都是它的变种。
核心矛盾在于：陀螺和加速度计各有各的毛病。
- 陀螺：短时准、高频响应好，但积分会漂（零偏被积分成线性增长的误差）。
- 加速度计：能测重力方向，长期不漂，但噪声大、对振动和机动加速度敏感。
互补滤波的思路就是：对陀螺高通、对加速度计低通，把两者的优势拼起来。
从频域看这件事最清楚。设真实姿态为 $\theta$，陀螺积分给出的估计记为 $\hat\theta_{gyro}$，它的误差主要集中在低频（漂移），加速度计反算的角度 $\hat\theta_{acc}$ 的误差主要集中在高频（噪声）。互补滤波就是让最终的估计等于：
$$
\hat\theta(s) = G_{high}(s),\hat\theta_{gyro}(s) + G_{low}(s),\hat\theta_{acc}(s)
$$
其中 $G_{high}$ 是高通滤波器（放行陀螺的高频部分），$G_{low}$ 是低通滤波器（放行加速度计的低频部分），并且满足 $G_{high} + G_{low} = 1$。这样，低频段信加速度计（压掉陀螺漂移），高频段信陀螺（压掉加速度计噪声），两头都占了便宜。
离散形式的一维互补滤波长这样：
$$
\theta_k = \alpha,(\theta_{k-1} + \omega_k \Delta t) + (1-\alpha),\theta_{acc}
$$
$\theta_{acc}$ 是加速度计算出的角度（比如 $\arctan(a_y / a_z)$），$\alpha$ 一般取 0.9 以上，表示更信陀螺。物理含义很直白：角度主要靠陀螺积分递推，加速度计偶尔把它往回拉一点，防止漂走。
Mahony：四元数版的互补滤波
一维互补滤波只能管一个轴，真实姿态是三维的，得用四元数。Mahony 就是「四元数 + PI 修正」的互补滤波，2008 年 Robert Mahony 那篇经典论文提出的。
误差怎么来
核心是构造一个「姿态误差」。设当前四元数 $q$ 对应的旋转矩阵为 $R(q)$，那么它推算出的重力方向（假设重力在导航系是 $[0,0,1]$）是：
$$
\hat{v} = R(q)^T \begin{bmatrix}0\0\1\end{bmatrix}
$$
而加速度计测到的是真实的重力方向 $a$（静止时）。两者不一致，就说明姿态估计有误差。误差向量取叉乘：
$$
e = a \times \hat{v}
$$
这个 $e$ 的方向是「要把 $\hat{v}$ 转到 $a$ 需要绕的轴」，大小是夹角的正弦，本质就是一个姿态误差的旋转向量表示。
PI 修正
Mahony 用 PI 控制器把误差转成陀螺的修正量：
$$
\omega_{corr} = K_p, e + K_i \int e, dt
$$
这里的 $K_p$ 是比例项，负责快速拉回误差；$K_i$ 是积分项，负责把陀螺零偏慢慢估计出来——这正是互补滤波「加速度计低频校正」的体现，零偏被当成一个慢变量积分掉。
四元数更新
用修正后的角速度 $\omega = \omega_{meas} + \omega_{corr}$ 更新四元数，用四元数运动学：
$$
\dot{q} = \frac{1}{2} q \otimes \omega = \frac{1}{2}\begin{bmatrix}0\ \omega_x\ \omega_y\ \omega_z\end{bmatrix} \otimes q
$$
写成离散递推就是那套经典的四元数一阶更新公式。
我在四足机器人 V1 上就是这么做的：BMI088 出角速度和加速度，Mahony 出姿态角，拿去做机身姿态闭环。核心代码：
// 归一化加速度 |
Mahony 的好处是轻量、好调，两个参数 $K_p$、$K_i$，在 MCU 上跑毫无压力。缺点是它本质是「启发式」的互补，没有显式建模噪声，你没法说它是「最优」的。
Mahony 为什么收敛：一个 Lyapunov 视角
Mahony 论文里一个漂亮的地方是，它的收敛性可以用 Lyapunov 函数严格证明。这里给一个简化版思路（完整证明看原论文）。
设真实姿态是 $R$，估计是 $\hat{R}$，误差旋转 $\tilde{R} = \hat{R}R^T$。用旋转误差角 $\phi$（即 $\tilde{R}$ 的旋转角，满足 $\operatorname{tr}(\tilde{R}) = 1 + 2\cos\phi$）构造 Lyapunov 函数：
$$
V = 2 - 2\cos\phi = \operatorname{tr}(I - \tilde{R}) \ge 0
$$
$V = 0$ 当且仅当 $\tilde{R} = I$（$\phi = 0$，估计完全正确）。对 $V$ 求导，代入误差动力学（含陀螺修正项 $\omega_{corr} = K_p e$），可以推出：
$$
\dot{V} \le 0
$$
而且只在 $\tilde{R} = I$ 处取等号。由 LaSalle 不变集原理，估计姿态会收敛到正确姿态（只靠重力向量时，收敛到「roll/pitch 正确、yaw 不确定」的集合——因为重力方向不包含 yaw 信息）。
这就是为什么 Mahony 不是「拍脑袋」的互补滤波，而是有严格收敛保证的。
Madgwick：用梯度下降求四元数
Madgwick 和 Mahony 是同一类东西（都是互补滤波），区别在「怎么修正」。
Madgwick 把姿态估计写成一个最优化问题：找到一个四元数 $q$，让它推算出的重力方向尽量接近加速度计测到的方向。误差函数可以写成：
$$
f(q) = \hat{v}(q) - a
$$
然后用梯度下降去最小化它，每一步沿着负梯度方向走：
$$
q_{k+1} = q_k - \mu \frac{\nabla f(q_k)}{\lVert \nabla f(q_k) \rVert}
$$
其中 $\nabla f$ 是误差函数对四元数的雅可比，$\mu$ 是步长。这个梯度下降的估计，再和陀螺积分的结果做一次融合，得到最终姿态。
Madgwick 的优势是参数少、对调参不敏感（主要就是一个步长 $\mu$），而且它天然能把磁力计信息加进来校正 yaw。很多无人机飞控里 Madgwick 比 Mahony 更常见。
两条怎么选：追求简单可控、想自己理解每一步，用 Mahony；想要开箱即用、少调参，用 Madgwick。性能上两者半斤八两，都是「够用」级别。
卡尔曼滤波：从「启发式」到「最优估计」
互补滤波够用，但它是「凑出来」的，没有最优性保证。卡尔曼滤波（KF）则是严格意义上在线性高斯假设下的最小方差最优估计。这一节把 KF 的推导完整走一遍，后面 EKF、UKF 都是它的延伸。
模型假设
线性高斯系统：
$$
x_k = F x_{k-1} + w_k, \quad w_k \sim \mathcal{N}(0, Q)
$$
$$
z_k = H x_k + v_k, \quad v_k \sim \mathcal{N}(0, R)
$$
$x$ 是状态，$z$ 是观测，$F$ 是状态转移，$H$ 是观测矩阵，$Q$、$R$ 分别是过程噪声和观测噪声的协方差。
预测
先根据模型往前推，得到先验估计和先验协方差：
$$
\hat{x}k^- = F \hat{x}{k-1}
$$
$$
P_k^- = F P_{k-1} F^T + Q
$$
第二项 $Q$ 是「模型本身不可靠」的那部分，预测越不靠谱，$P^-$ 就越大。
更新与增益的推导
更新就是拿观测去修正先验，形式设为：
$$
\hat{x}_k = \hat{x}_k^- + K(z_k - H \hat{x}_k^-)
$$
$z_k - H\hat{x}_k^-$ 叫新息（innovation），是「观测比预测多了什么」。问题在于增益 $K$ 怎么取。
我们把后验误差协方差写出来，再对 $K$ 求最小化。设后验误差 $e_k = x_k - \hat{x}_k$，代入更新式并展开，得到后验协方差：
$$
P_k = (I - KH) P_k^- (I - KH)^T + K R K^T
$$
展开：
$$
P_k = P_k^- - KHP_k^- - P_k^- H^T K^T + K(H P_k^- H^T + R) K^T
$$
记 $S = H P_k^- H^T + R$（这就是新息的协方差）。我们想最小化 $P_k$ 的迹（等价于最小化均方误差）。用矩阵求导 $\frac{\partial}{\partial K}\operatorname{tr}(P_k) = 0$：
$$
-2(P_k^- H^T)^T + 2 K S = 0
$$
解出：
$$
K = P_k^- H^T S^{-1} = P_k^- H^T (H P_k^- H^T + R)^{-1}
$$
这就是卡尔曼增益。把 $K$ 代回去，后验协方差简化为：
$$
P_k = (I - KH) P_k^-
$$
增益的直觉
这个 $K$ 不是拍脑袋定的，它自动权衡了两头：
- 如果 $P^-$ 很大（预测很不确定），$K$ 就大，更多信观测；
- 如果 $R$ 很大（观测很吵），$K$ 就小，更多信预测。
这就是卡尔曼滤波比互补滤波高明的地方：噪声是显式建模的，增益是推导出来的。
EKF：把卡尔曼滤波搬到非线性系统上
问题在于，姿态和运动模型都是非线性的——四元数更新、旋转矩阵、腿部运动学，没一个是线性的。标准 KF 只处理线性系统，于是有了 EKF（扩展卡尔曼滤波）。
EKF 的思路粗暴但有效：把非线性函数在当前估计点处做一阶泰勒展开，用雅可比矩阵代替线性系统里的 $F$ 和 $H$。
设状态转移和观测分别是非线性函数 $f$、$h$：
$$
x_k = f(x_{k-1}) + w_k, \quad z_k = h(x_k) + v_k
$$
在当前估计点线性化：
$$
F = \frac{\partial f}{\partial x}\bigg|{\hat{x}}, \quad H = \frac{\partial h}{\partial x}\bigg|{\hat{x}}
$$
拿到雅可比之后，预测-更新两步和标准 KF 一模一样：
$$
\hat{x}k^- = f(\hat{x}{k-1}), \quad P_k^- = F P_{k-1} F^T + Q
$$
$$
K = P_k^- H^T (H P_k^- H^T + R)^{-1}
$$
$$
\hat{x}_k = \hat{x}_k^- + K(z_k - h(\hat{x}_k^-)), \quad P_k = (I - KH) P_k^-
$$
姿态估计里，EKF 的状态向量通常是 $[q, \omega_{bias}, v, p]$ 这类，观测是加速度计、磁力计、腿部里程计给的量。
EKF 的毛病也来自这个「一阶近似」：系统非线性很强时，一阶截断误差会累积，导致协方差被低估、滤波发散。而且雅可比矩阵要手推、要维护，状态维数一大，计算量也上来了。
一个完整的姿态 EKF 实例
上面是通用公式，落到姿态上，常用的是误差状态 EKF（ES-EKF）。原因很实际：四元数有 4 个参数但只有 3 个自由度，直接在四元数上做 EKF 会破坏单位模约束，所以大家在「误差」上做滤波，误差是 3 维的，不会有约束问题。
状态：误差状态 $\delta x = [\delta\theta^T,\ \delta b_g^T]^T \in \mathbb{R}^6$，其中 $\delta\theta$ 是姿态误差（角轴表示），$\delta b_g$ 是陀螺零偏误差。名义状态（被传播的）是四元数 $q$ 和零偏 $b_g$。
传播：名义状态用测量角速度积分，零偏当作慢变量：
$$
\dot{q} = \frac{1}{2} q \otimes (\omega_m - b_g), \quad \dot{b}_g = 0
$$
误差状态的线性动力学：
$$
\delta\dot{\theta} = -[\hat\omega]_\times,\delta\theta - \delta b_g, \quad \delta\dot{b}g = n{bg}
$$
其中 $\hat\omega = \omega_m - b_g$，$[\cdot]_\times$ 是叉乘的反对称矩阵：
$$
[\omega]_\times = \begin{bmatrix} 0 & -\omega_z & \omega_y \ \omega_z & 0 & -\omega_x \ -\omega_y & \omega_x & 0 \end{bmatrix}
$$
所以误差状态转移矩阵是：
$$
F = \begin{bmatrix} -[\hat\omega]\times & -I{3\times3} \ 0 & 0 \end{bmatrix}
$$
离散化 $\Phi \approx I + F\Delta t$，协方差预测：
$$
P_k^- = \Phi P_{k-1}\Phi^T + Q
$$
观测：加速度计测到的是比力，静止时就是重力方向。预测观测 $\hat{a} = R(\hat{q})^T g$，残差 $r = a_m - \hat{a}$。观测对误差状态的雅可比（结构如此，具体符号随约定）：
$$
H = \begin{bmatrix} [R(\hat{q})^T g]\times & 0{3\times3} \end{bmatrix}
$$
更新：按标准 EKF 更新算出 $\delta\hat{x} = K r$，再把误差「合」回名义状态：
$$
\hat{q} \leftarrow \hat{q} \otimes \begin{bmatrix}1\ \frac{1}{2}\delta\theta\end{bmatrix}, \quad \hat{b}_g \leftarrow \hat{b}_g + \delta\hat{b}_g
$$
这一套就是四足、无人机上 IMU 姿态估计的标准做法。它比 Mahony 重，但能显式建模噪声、能顺带估零偏，而且容易扩展到「IMU + 腿部里程计」融合线速度——把状态向量扩成 $[\delta\theta, \delta v, \delta p, \delta b_g, \delta b_a]$，把腿部里程计的速度当观测加进去就行。
UKF：别线性化了，直接采样
既然 EKF 的痛点是「线性化丢掉高阶信息」，UKF（无迹卡尔曼滤波）干脆不线性化了。
它的做法是：在当前状态附近按规则撒一组 sigma 点，把这些点原封不动地送进非线性函数，再根据这些点变换后的分布重新算出均值和协方差。这套流程叫无迹变换（Unscented Transform）。
sigma 点的选取
对 $n$ 维状态、均值 $\hat{x}$、协方差 $P$，取 $2n+1$ 个点：
$$
\chi_0 = \hat{x}, \quad
\chi_i = \hat{x} + \left(\sqrt{(n+\lambda)P}\right)i, \quad
\chi{i+n} = \hat{x} - \left(\sqrt{(n+\lambda)P}\right)_i
$$
其中 $\left(\sqrt{(n+\lambda)P}\right)_i$ 是矩阵平方根的第 $i$ 列（通常用 Cholesky 分解）。缩放参数：
$$
\lambda = \alpha^2(n+\kappa) - n
$$
$\alpha$ 控制 sigma 点散布的范围，$\kappa$ 是次级缩放，$\beta$ 用来纳入先验分布信息（高斯时取 2）。
权重：
$$
W_0^{(m)} = \frac{\lambda}{n+\lambda}, \quad W_0^{©} = \frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta), \quad W_i^{(m)} = W_i^{©} = \frac{1}{2(n+\lambda)}
$$
传播
每个 sigma 点过一遍非线性函数 $\chi_i’ = f(\chi_i)$，然后按权重重新合成均值和协方差：
$$
\hat{x}’ = \sum_i W_i^{(m)} \chi_i’, \quad P’ = \sum_i W_i^{©}(\chi_i’ - \hat{x}‘)(\chi_i’ - \hat{x}')^T + Q
$$
因为不做泰勒展开，UKF 对非线性的逼近精度相当于「二阶」，通常比 EKF（一阶）更准，尤其是强非线性系统。代价是每一拍要多算 $2n+1$ 次状态传播，计算量更大。好处是不用推雅可比，工程实现反而省心。
一句话：EKF 推雅可比、便宜；UKF 采样、更准但更贵。状态维数不高（姿态 + 零偏，6~9 维）时，UKF 是性价比很高的选择。
EKF 和 UKF 到底差多少
一句话概括精度差异：EKF 是一阶近似（泰勒展开到一阶），UKF 是二阶近似（无迹变换对均值和协方差的传播精确到二阶矩）。对高斯分布的非线性传播，UKF 算出的后验均值和协方差比 EKF 更接近真实值。
具体到姿态：当初始姿态误差较大（比如刚上电、不知道朝哪）、或者角速度很大（强非线性）时，EKF 的线性化误差会让协方差被低估、估计偏掉，严重时发散；UKF 因为直接采样传播，能扛住更大的非线性和不确定性。
代价也明确：UKF 每一拍要传播 $2n+1 = 13$ 个 sigma 点（对 6 维状态），计算量大约是 EKF 的几倍。所以工程选型很现实——状态维数低（≤ 9 维）、精度要求高、又不想手推雅可比，用 UKF；追求实时性、状态维数高、雅可比能推出来，用 EKF。
MSCKF：视觉惯性里程计（VIO）的核心
前面都是「IMU + 少量观测」的姿态/速度估计。一旦上了视觉，问题就升级成 VIO（视觉惯性里程计），而 VIO 里最有名的滤波框架就是 MSCKF（多状态约束卡尔曼滤波）。
为什么需要它
纯 EKF 做 VIO 时，如果想把特征点的 3D 位置也放进状态向量，状态维度会随特征点数量爆炸——几百个点就是上千维，协方差矩阵大到没法实时算。
MSCKF 的关键想法是：不把特征点放进状态，而是维护一个「IMU 状态 + 最近 N 个相机位姿」的滑动窗口。
状态向量长这样：
$$
x = \begin{bmatrix} x_{IMU} \ x_{c_1} \ x_{c_2} \ \vdots \ x_{c_N} \end{bmatrix}
$$
其中 $x_{IMU} = [q, p, v, b_g, b_a]$（姿态、位置、速度、陀螺零偏、加速度计零偏），$x_{c_i} = [q_{c_i}, p_{c_i}]$ 是第 $i$ 个相机位姿。
特征点的约束
一个 3D 特征点 $f_j$ 如果被多个相机位姿观测到，它在每个相机里的投影 $z_{ij}$ 都是观测。观测方程：
$$
z_{ij} = h(x_{c_i}, f_j) + n_{ij}
$$
直接用它，$f_j$ 就得进状态。MSCKF 的 trick 是：把残差投影到 $f_j$ 的雅可比矩阵的左零空间上，把 $f_j$ 从方程里消掉。
设观测对 $f_j$ 的雅可比是 $H_{f_j}$，找到它的左零空间 $V$（满足 $V^T H_{f_j} = 0$），对残差方程左乘 $V^T$：
$$
V^T r = V^T H_x \tilde{x} + V^T n
$$
$f_j$ 那一项被消掉了，剩下的只和相机位姿状态 $x$ 有关。这就是「多状态约束」——同一个特征点在多个相机位姿之间建立的约束，用完之后这个特征点就丢弃，不进状态、不占维度。
VINS-Mono、OpenVINS 这些主流 VIO，底层都是 MSCKF（或它的亲戚）。四足和无人机在 GPS 拒止的室内环境里定位，靠的就是它。
测量模型和零空间投影，细看
补一下 MSCKF 的数学细节。设相机投影函数 $\pi(\cdot)$，一个特征点 $f_j$ 在第 $i$ 个相机位姿里的观测：
$$
z_{ij} = \pi\big(R_{c_i}^T(f_j - p_{c_i})\big) + n_{ij}
$$
残差 $r_{ij} = z_{ij} - \hat{z}_{ij}$，线性化：
$$
r_{ij} \approx H_{x_{ij}}\tilde{x} + H_{f_{ij}}\tilde{f}j + n{ij}
$$
$H_{f_{ij}}$ 是 $2\times3$，$H_{x_{ij}}$ 是 $2\times(15+6N)$（IMU 状态 15 维 + N 个相机位姿各 6 维）。把特征点 $f_j$ 被所有相机观测到的残差叠起来：
$$
r_j = H_{x_j}\tilde{x} + H_{f_j}\tilde{f}_j + n_j
$$
$H_{f_j}$ 现在是 $2M \times 3$（$M$ 是这个特征被看到的次数）。关键一步：求 $H_{f_j}$ 的左零空间 $V$（$V^T H_{f_j} = 0$），左乘上去：
$$
V^T r_j = V^T H_{x_j}\tilde{x} + V^T n_j
$$
特征点 $\tilde{f}_j$ 被消掉了，只剩状态 $\tilde{x}$。$V$ 一般用 Givens 旋转或 QR 分解算，是 $2M \times (2M-3)$ 的矩阵。把所有特征投影后的残差堆起来，就得到只含状态的观测方程，直接做标准 EKF 更新。
这就是 MSCKF 的「约束」：它不估计特征点，却用特征点在多帧之间的几何一致性来约束相机位姿。
粒子滤波：把分布换成一堆「粒子」
前面所有卡尔曼系的方法，都假设分布是高斯、系统近似线性。粒子滤波（PF）彻底换了个思路：用一堆带权重的随机样本（粒子）来近似任意概率分布，不管它高不高斯、单峰还是多峰。
流程是经典的「序贯重要性重采样」（SIR）：
- 预测：每个粒子按运动模型各自乱走一步，$x_k^{(i)} \sim p(x_k \mid x_{k-1}^{(i)})$。
- 更新：根据观测给每个粒子算权重 $w^{(i)} = p(z_k \mid x_k^{(i)})$，和观测越像，权重越高，然后归一化。
- 重采样：按权重重新抽粒子，权重高的多复制，低的淘汰，把粒子重新变成等权重的。
粒子滤波擅长的是「全局定位」这种强非线性、多峰的问题——比如机器人被抱起来放到一个陌生位置，用粒子滤波重新找回自己在哪，这就是经典的蒙特卡洛定位（MCL）。
但它有个致命伤：维数诅咒。状态维度一高，要覆盖分布所需的粒子数指数级增长。姿态 + 速度 + 位置这种十几维的问题，粒子滤波直接不划算。所以姿态估计里很少用 PF，它更多用在 2D 定位、目标跟踪这类低维问题上。
四足机器人的辅助校正：光有滤波器不够
滤波器是「融合」的工具，但它得有「观测」可融合。四足机器人上，姿态以外的观测往往来自这些辅助手段。
腿部里程计（Leg Odometry）。四足没有轮子，但每条腿都有关节编码器。利用正运动学算出足端位置，再假设「支撑相足端相对地面不动」，就能反推机身速度。
设足端位置 $p_{foot} = f(q)$，对它求导得到足端速度：
$$
\dot{p}_{foot} = J(q),\dot{q}
$$
$J$ 是足端雅可比。支撑相足端不动，即 $\dot{p}_{foot} = 0$ 相对世界系，反推出机身速度：
$$
v_{body} = -J(q),\dot{q}
$$
这个速度带噪声（地面会滑、接触点会变），但它是四足上最现成的速度观测，和 IMU 的角速度/加速度一起喂给 EKF，就能估计出线速度和位置。
接触检测（Contact Detection）。腿部里程计的「支撑足不动」假设，前提是你知道哪只脚着地。可靠做法是用足端力传感器，或者退而求其次用运动学推断（足端高度 + 触地瞬间的冲击）。接触状态错了，里程计直接废。
零速更新（ZUPT）。机器人停下来（比如趴着不动）时，「速度为零」就是一个强观测，可以拿来校正速度漂移。判断「真的停下来了」本身也要靠 IMU 和接触检测。
零偏在线估计。陀螺零偏随温度漂，加速度计零偏影响姿态和速度。把零偏放进 EKF 的状态向量里一起估，是标准做法。
这些辅助算法本质上都是在「制造观测」，滤波器负责把观测和 IMU 融合。我的四足 V1 当时只用了 Mahony 估姿态，没有做腿部里程计 + EKF 的线速度融合，结果 Sim-to-Real 到真机上，线速度观测缺失、状态估计有误差，机器人走得就不如仿真里自然。后来回头看，缺的那块就是这一节讲的东西。
怎么选：一张图理清
把上面这些算法按「传感器」和「目标」对号入座：
- 只要姿态（单 IMU）：Mahony 或 Madgwick，轻量、够用。
- 姿态 + 速度 + 零偏（IMU + 编码器/里程计）：EKF 或 UKF。
- 视觉 + IMU 定位（VIO）：MSCKF。
- 强非线性、低维全局定位：粒子滤波。
- 维度高、还想要最优：别硬上粒子滤波，回到卡尔曼系。
没有银弹，只有「传感器能给你什么观测」决定「你该用哪个滤波器」。
写在最后
这一排算法的演进，其实是一条很清晰的线：
- 互补滤波：把两个传感器拼起来，朴素但有效；
- Mahony / Madgwick：四元数版的互补滤波，一个 PI、一个梯度下降；
- KF：线性高斯下的最优估计，把噪声显式建模、增益推导出来；
- EKF / UKF：把 KF 搬到非线性系统，一个线性化、一个采样；
- MSCKF：VIO 里的工程解，用滑动窗口和零空间投影避开维度爆炸；
- PF：换一种分布表示，专治强非线性、多峰，但怕高维。
理解这条线，比背一堆公式有用得多。下次再看到一个新名字，先问它「在解决非线性吗、在解决高维吗、有没有视觉」，基本就能猜到它属于哪一档。
