05 状态估计与传感器融合

从滤波到估计

前面几章的滤波算法有一个共同特点：它们只看"测量值本身的历史"。一阶滞后用上一次输出和当前输入做加权；滑动平均用最近 N 个测量值求均值。它们不关心"系统接下来大概会怎样变化"。

状态估计多问一个问题：如果我知道系统的行为规律，能不能先预测当前状态，再用测量值来修正？

要理解这个问题，先要分清几个概念：

状态。 系统当前的真实情况，通常不是传感器直接读到的值。例如物体的位置和速度、陀螺仪的角度和零偏、电池的 SOC（剩余电量）和内阻。状态是"你想知道但无法直接测量的东西"。

模型。 对系统行为规律的描述。例如"位置会按速度变化"、"陀螺角度会积分漂移"、"电池电压会随 SOC 下降"。模型不需要完美，但要能给出一个合理的预测。

测量。 传感器读数。它不等于真实状态——带有噪声、可能有偏差、可能被干扰。测量是对状态的一次"带噪声的观察"。

残差。 测量值和预测值之间的差。它不是简单的误差，而是更新的依据：残差大，说明预测可能偏了，需要多修正；残差小，说明预测和测量一致，可以少修正。

状态估计和普通低通滤波的区别。 低通滤波只看历史测量值的加权平均，本质是"过去的数据告诉我现在大概是什么"。状态估计会先用模型预测"现在应该是什么"，再用测量修正。这个"先预测、再修正"的框架，是本章所有算法的共同基础。

测量值会抖
  → 普通滤波只看历史测量
  → 状态估计会先预测系统怎么变
  → 再用测量修正预测
  → 修正多少，取决于模型可信度和测量可信度

预测-更新框架

状态估计的核心是一个两步循环：

预测（Predict）：根据模型，从上一时刻的状态推算当前状态。
更新（Update）：拿到当前测量值，用它修正预测结果。

修正多少，取决于两个因素：

模型有多可信。 模型越可信，预测越准，修正越少。
测量有多可信。 测量越可信，修正越多。

这两个因素的权衡，是本章所有算法的核心。α-β 滤波用固定的 α 和 β 来权衡；卡尔曼滤波用不确定度来动态计算权衡；互补滤波用频率来分配信任。

α-β 滤波：最轻量的位置-速度估计

α-β 滤波器估计两个量：位置（或被测量的当前值）和值的变化速度。它可以看作最轻量的预测-更新滤波器。

适用场景

距离测量、液位、慢速运动跟踪、带趋势的温度或压力。凡是"值在变化，但变化速度不会突然跳变"的场景，都可以考虑 α-β。

前提条件

采样周期基本稳定（dt 不频繁跳变）。
被测量的变化速度不会突然从一个值跳到另一个值（速度是平滑变化的）。
如果 dt 可能为 0 或负数，必须在代码中做边界保护。

预测-更新结构

预测式：根据上一时刻的速度，推算当前位置。

\[ x_{\text{pred}} = x[n-1] + v[n-1] \cdot dt \]

残差：测量值和预测值之间的差。

\[ r = z - x_{\text{pred}} \]

更新：用残差的一部分修正位置和速度。

\[ x[n] = x_{\text{pred}} + \alpha \cdot r \]

\[ v[n] = v[n-1] + \frac{\beta}{dt} \cdot r \]

alpha 和 beta 的工程含义

\(\alpha\)：修正位置时对残差的信任程度。\(\alpha\) 大，位置修正快，但测量噪声会直接传到输出；\(\alpha\) 小，位置更稳，但对真实变化的跟踪慢。
\(\beta\)：修正速度时对残差的信任程度。\(\beta\) 大，速度估计变化快，能快速跟踪加减速，但噪声大；\(\beta\) 小，速度更平滑，但对加减速反应慢。

和一阶滞后的关系

α-β 可以看作一阶滞后的扩展：一阶滞后只估计当前值（位置），α-β 多估计了一个速度项。如果令 \(\beta = 0\)（不修正速度），α-β 就退化为一阶滞后。

参数怎么选

\(\alpha\) 和 \(\beta\) 通常满足 \(\beta \ll \alpha\)。一种经验关系是：

\[ \beta \approx \frac{\alpha^2}{2 - \alpha} \]

但这只是起点，实际应根据系统测试调整。

参数过大的风险： 输出追噪声，估计值抖动大。参数过小的风险： 对真实变化响应慢，输出滞后。

MCU C 实现

#include <stdint.h>

typedef struct {
    float x;     /* 估计的位置或被测量 */
    float v;     /* 估计的变化速度 */
    uint8_t init; /* 是否完成初始化 */
} AlphaBeta;

void alphabeta_init(AlphaBeta *f)
{
    f->x = 0.0f;
    f->v = 0.0f;
    f->init = 0;
}

/**
 * alpha-beta 滤波更新。
 * z:     当前测量值
 * dt:    采样周期（秒），必须为正
 * alpha: 位置修正系数（0~1）
 * beta:  速度修正系数（通常远小于 alpha）
 */
float alphabeta_update(AlphaBeta *f, float z,
                       float dt, float alpha, float beta)
{
    if (!f->init) {
        f->x = z;
        f->v = 0.0f;
        f->init = 1;
        return z;
    }

    /* dt 必须为正，否则速度修正无意义 */
    if (dt <= 0.0f) return f->x;

    /* 钳位参数。alpha 在 0~1 之间；beta 通常远小于 alpha，
     * 过大会导致速度估计不稳定。 */
    if (alpha < 0.0f) alpha = 0.0f;
    if (alpha > 1.0f) alpha = 1.0f;
    if (beta  < 0.0f) beta  = 0.0f;
    if (beta  > 1.0f) beta  = 1.0f;

    /* 预测：按上一时刻速度推算当前位置 */
    float x_pred = f->x + f->v * dt;

    /* 残差：测量值和预测值之间的差 */
    float r = z - x_pred;

    /* 更新：用残差的一部分修正位置和速度 */
    f->x = x_pred + alpha * r;
    f->v = f->v + (beta / dt) * r;

    return f->x;
}

代价和权衡。 只需要 2 个浮点状态变量，计算量极小（几次乘加）。但它假设速度是平滑变化的，如果被测量有突变加速度，α-β 会暂时跟不上。

验证方法。 用带有已知趋势的测试数据（比如匀速运动、匀加速运动），对比 α-β 输出和真实值的误差。用纯噪声数据测试，确认参数不会让输出追噪声。

一维卡尔曼滤波：用不确定度决定相信谁

α-β 把速度显式估计出来，用固定的 α 和 β 修正位置和速度。一维卡尔曼不显式估计速度，而是估计不确定度，用不确定度来动态计算"该相信预测多少、该相信测量多少"。

预测-更新结构

一维卡尔曼的状态只有位置（没有显式的速度项），预测阶段假设状态不主动变化，只增加不确定度：

预测：

\[ x_{\text{pred}} = x[n-1] \]

\[ P_{\text{pred}} = P[n-1] + Q \]

更新：

\[ K = \frac{P_{\text{pred}}}{P_{\text{pred}} + R} \]

\[ x[n] = x_{\text{pred}} + K \cdot (z - x_{\text{pred}}) \]

\[ P[n] = (1 - K) \cdot P_{\text{pred}} \]

变量含义

变量	含义	单位
\(x\)	当前状态估计值	和被测量同单位（如 m、°C）
\(P\)	预测的不确定度（方差）	单位的平方（如 m²、°C²）
\(Q\)	过程噪声：每步系统自身变化的不确定度	单位的平方/步
\(R\)	测量噪声：传感器读数的不确定度	单位的平方
\(K\)	卡尔曼增益：0~1，决定修正多少	无量纲

Q 和 R 怎么影响输出

Q 大，R 小： 滤波器更相信测量。系统变化快，测量精度高，所以多修正。输出更灵敏，但噪声也更明显。
Q 小，R 大： 滤波器更相信预测。系统变化慢，测量噪声大，所以少修正。输出更平滑，但对真实变化响应慢。

和一阶滞后的关系

一维卡尔曼的更新式可以写成：

\[ x[n] = x[n-1] + K \cdot (z - x[n-1]) \]

这和一阶滞后 \(y[n] = y[n-1] + \alpha \cdot (x[n] - y[n-1])\) 的形式完全一样。区别在于：

一阶滞后的 \(\alpha\) 是固定值。
卡尔曼的 \(K\) 会随 \(P\) 和 \(R\) 动态变化。在固定 Q/R 的一维卡尔曼中，\(P\) 会收敛到一个稳定值，\(K\) 也会收敛到近似稳定的增益——不会因为某次残差大就自动变大。想让大残差触发更大增益，需要自适应 Q/R、异常检测或重置 P，这属于更高级的变体。

\(K = P/(P+R)\) 只是一维简化形式。 多维卡尔曼的增益计算涉及矩阵求逆，不能用这个简单公式。但"增益随不确定度变化"的核心思想是通用的。

参数怎么选

\(Q\)：初始可以从测量噪声的 1/10 到 1/100 开始。如果滤波器对真实变化响应太慢，增大 Q；如果输出太抖，减小 Q。
\(R\)：可以从传感器数据手册的噪声指标估计，或者在稳态下测量传感器输出的方差。
\(P\) 的初始值：通常设一个较大的值（比如 1.0 或 100），表示"一开始不太确定"。几拍之后会自动收敛。

MCU C 实现

#include <stdint.h>

typedef struct {
    float x;    /* 当前估计值 */
    float p;    /* 当前估计不确定度 */
    float q;    /* 过程噪声 */
    float r;    /* 测量噪声 */
    uint8_t init; /* 是否完成初始化 */
} Kalman1D;

void kalman1d_init(Kalman1D *k)
{
    k->x = 0.0f;
    k->p = 1.0f;
    k->q = 0.01f;
    k->r = 0.1f;
    k->init = 0;
}

float kalman1d_update(Kalman1D *k, float z)
{
    if (!k->init) {
        k->x = z;
        k->p = 1.0f;
        k->init = 1;
        return z;
    }

    /* 参数前提：q >= 0，r > 0。r <= 0 会导致除零或增益溢出。 */
    if (k->q < 0.0f) k->q = 0.0f;
    if (k->r <= 0.0f) k->r = 1e-6f;

    /* 预测阶段：状态不主动变化，只增加过程不确定度 */
    k->p += k->q;

    /* 卡尔曼增益：不确定度越大，越相信测量 */
    float gain = k->p / (k->p + k->r);

    /* 更新估计值：gain 越大，越靠近测量值 z */
    k->x += gain * (z - k->x);

    /* 更新不确定度：融合测量后，不确定度下降 */
    k->p *= (1.0f - gain);

    return k->x;
}

代价和权衡。 只比一阶滞后多了一个乘法和加法，计算量可以忽略。但多了一组参数（Q、R、P 初始值）需要调节。参数选得不好，效果不一定比一阶滞后好。

什么时候不该用。 系统模型明显非线性时（比如姿态估计），一维卡尔曼不够用。测量噪声明显不是高斯分布时（比如有规律的周期干扰），卡尔曼的最优性前提不成立。

验证方法。 对比一维卡尔曼和固定 alpha 的一阶滞后：在稳态数据上，两者输出应该接近（卡尔曼收敛后的增益和固定 alpha 差不多时）。如果两者效果差不多，说明问题本身不需要卡尔曼——固定参数的一阶滞后更简单。如果能根据工况调整 Q/R，或模型本身能表达不确定度变化，卡尔曼才有优势。

互补滤波：用两个传感器互相补短板

互补滤波常用于姿态估计。它的核心思想是：不同传感器在不同频率范围内各有优势，把它们的优点组合起来。

典型场景：陀螺仪 + 加速度计

陀螺仪：短时间很平滑，角速度积分后能给出连续的角度变化。但积分会累积漂移——时间一长，误差越来越大。
加速度计：长期能指示重力方向（从而推算倾角），不会漂移。但震动、加速运动时，加速度计读数不只反映重力，还会被运动加速度污染。

一句话原理：短期信陀螺仪，长期信加速度计。

数学定义

陀螺仪积分给出短时间角度预测：

\[ \theta_{\text{gyro}} = \theta[n-1] + \omega \cdot dt \]

互补滤波再把它与加速度计角度加权融合：

\[ \theta[n] = \alpha \cdot \theta_{\text{gyro}} + (1 - \alpha) \cdot \theta_{\text{acc}} \]

\(\alpha\) 接近 1 表示短期更相信陀螺仪，长期慢慢拉回加速度计角度。

alpha 的工程含义

\(\alpha\) 不只是"一个常数"，它对应一个等效时间常数：

\[ \tau = -\frac{dt}{\ln(\alpha)} \]

时间常数 \(\tau\) 表示"加速度计修正陀螺仪漂移的速度"——经过一个时间常数后，漂移误差约衰减到 37%（即 \(e^{-1}\)），而不是完全消失。例如 \(dt = 0.01\;\text{s}\)，\(\alpha = 0.98\)：

\[ \tau = -\frac{0.01}{\ln(0.98)} \approx 0.495\;\text{s} \]

这意味着陀螺仪的漂移在约 0.5 秒内会被加速度计修正。当 \(\alpha\) 接近 1 时，由 \(\ln(1-(1-\alpha)) \approx -(1-\alpha)\) 可得近似 \(\tau \approx dt / (1-\alpha)\)。如果想在更高采样率下保持同样的时间常数，\(\alpha\) 要更接近 1。

加速度计的适用边界

加速度计只有在低动态时能代表重力方向。以下情况不能直接相信加速度计：

剧烈加速运动（比如汽车急加速、无人机急转弯）。
震动（比如电机振动、碰撞）。
长时间匀速旋转（离心力会改变表观重力方向）。

在这些情况下，\((1-\alpha)\) 的修正会被运动加速度污染，导致角度估计跳变。工程上可以在 Mahony、Madgwick 或 EKF 中加入可信度判断或权重调节——比如检测加速度模长是否接近 g，偏离太大时降低加速度计权重。

MCU C 实现

#include <stdint.h>

typedef struct {
    float angle; /* 当前角度估计 */
    uint8_t init; /* 是否完成初始化 */
} CompFilter;

void comp_filter_init(CompFilter *f)
{
    f->angle = 0.0f;
    f->init = 0;
}

/**
 * 互补滤波角度更新。
 * gyro_rate: 陀螺仪角速度（rad/s）
 * acc_angle: 加速度计推算的角度（rad）
 * dt:        采样周期（秒）
 * alpha:     陀螺仪信任系数（通常 0.95~0.995）
 */
float comp_filter_update(CompFilter *f, float gyro_rate,
                         float acc_angle, float dt, float alpha)
{
    if (!f->init) {
        f->angle = acc_angle;
        f->init = 1;
        return acc_angle;
    }

    if (alpha < 0.0f) alpha = 0.0f;
    if (alpha > 1.0f) alpha = 1.0f;

    /* dt 必须为正，否则积分无意义 */
    if (dt <= 0.0f) return f->angle;

    /* 陀螺仪角速度积分得到短时间预测角度 */
    float gyro_angle = f->angle + gyro_rate * dt;

    /* alpha 接近 1 表示短期更相信陀螺仪，长期慢慢拉回加速度计角度 */
    f->angle = alpha * gyro_angle + (1.0f - alpha) * acc_angle;

    return f->angle;
}

代价和权衡。 计算量极小（一次积分、一次乘加）。没有协方差、没有矩阵，是最简单的传感器融合方法。但它没有显式处理"加速度计什么时候可信"，只能靠 \(\alpha\) 的选择来隐式权衡。

验证方法。 在静态下观察角度是否稳定（加速度计在工作）。在动态运动下观察角度是否不漂移（陀螺仪在工作）。在震动下观察角度是否不会跳变（\(\alpha\) 的选择是否合理）。

姿态融合的三条路：Mahony、Madgwick、EKF

当需要完整的三维姿态（俯仰、横滚、偏航），互补滤波的一维加权就不够用了。下面三种算法是工程中最常用的三维姿态融合方案。这里不做完整推导，只讲它们的定位、边界和选型关系。

算法	适合	关键参数	主要风险
Mahony	常规 IMU 姿态、MCU 资源有限	Kp（比例增益）、Ki（积分增益）	震动和磁干扰下误差反馈会出问题
Madgwick	低采样率姿态融合	beta（梯度下降步长）	参数过大会把加速度计噪声带入姿态
EKF	多传感器、非线性模型、需要协方差	Q、R、状态模型	建模复杂，调参和单位错误很常见

Mahony 滤波

Mahony 用陀螺仪积分姿态，再用加速度计和磁力计提供误差反馈，校正陀螺仪漂移。核心思想：

姿态预测 = 陀螺仪积分
误差 = 估计重力方向 与 测量重力方向 的叉乘
陀螺仪修正 = Kp × 误差 + Ki × 误差积分

Kp 决定纠偏速度，Ki 用来慢慢补偿陀螺零偏。优点是结构清晰、计算量低、参数直观。适合 MCU 姿态估计、云台、平衡车、低成本 AHRS。

Madgwick 滤波

Madgwick 把姿态误差写成优化问题，用梯度下降修正四元数。与 Mahony 相比，Madgwick 在低采样率下也能有不错效果。参数主要是 beta：越大越相信加速度计和磁力计修正，越小越相信陀螺仪积分。

扩展卡尔曼滤波（EKF）

EKF 用于非线性系统。姿态、导航、机器人运动经常是非线性的，普通卡尔曼的线性假设不够用。EKF 用雅可比矩阵在当前点附近做线性近似，再套用卡尔曼的预测-更新框架。

EKF 的难点不在代码行数，而在建模：状态选什么、测量量是什么、噪声协方差怎么设、单位是否统一。对初学者来说，建议路径是：先写懂一维卡尔曼，再写 α-β，再理解互补滤波，最后再进入 EKF。 不要一开始就复制完整姿态 EKF。

选型建议

项目只需要俯仰和横滚、MCU 资源有限、采样率够高（>100 Hz）：先试 Mahony。
采样率偏低（<50 Hz），或需要更平滑的四元数输出：试 Madgwick。
需要融合多传感器（GPS + IMU + 气压计）、需要协方差信息、或模型明显非线性：考虑 EKF。
不确定用哪个：先从互补滤波开始，确认传感器数据没问题，再升级到 Mahony 或 Madgwick。

什么时候不要用状态估计

状态估计不是万能的。以下场景用普通滤波更合适：

传感器只有一个，且没有可信赖的系统模型。 没有模型就没有"预测"，状态估计退化为普通的加权平均。
噪声主要是确定性干扰（比如 50 Hz 工频）。 应该用陷波或 FIR 滤掉干扰频率，而不是靠卡尔曼。
系统变化非常快，模型完全跟不上。 模型的预测没有参考价值时，卡尔曼的增益会趋近于 1，退化为直接相信测量。
参数没有数据支撑，只能盲调。 盲调出来的卡尔曼不一定比一阶滞后好，而且更难调试。

参数怎么验证

状态估计算法的参数不能只靠"看起来差不多"来判断。以下是工程上常用的验证方法：

静态测试。 传感器不动，观察输出是否稳定。状态估计的输出抖动应该比原始数据小。
阶跃响应。 给系统一个已知的突变（比如突然改变角度），观察输出是否能在合理时间内跟上。
长时间漂移。 连续运行数小时，观察输出是否有累积漂移。互补滤波或带零偏状态的卡尔曼/EKF 应该能抑制陀螺仪漂移。本章的一维卡尔曼示例没有显式建模零偏，不能直接抑制陀螺漂移。
和参考对比。 如果有更高精度的参考传感器（比如光学编码器、高精度 IMU），可以对比状态估计的输出和参考值的误差。
参数敏感度。 把 Q 或 R 改变 2~5 倍，观察输出变化大不大。如果变化很大，说明参数对这个场景很敏感，需要更仔细地校准。

小结：模型、测量和代价

算法	估计什么	核心权衡	主要代价
α-β 滤波	位置 + 速度	α 控制位置修正，β 控制速度修正	速度假设平滑，突变加速度跟不上
一维卡尔曼	位置（不确定度自适应）	Q 和 R 决定相信模型还是测量	需要噪声参数，参数选不好不一定比一阶滞后好
互补滤波	角度（双传感器）	短期信陀螺，长期信加速度计	加速度计在高动态时不可信
Mahony / Madgwick / EKF	三维姿态	精度 vs 计算量 vs 调参难度	EKF 建模复杂，Mahony/Madgwick 对磁干扰敏感

状态估计的本质不是"更高级的低通滤波"，而是把"系统会怎么变"这个信息也用上。模型给了预测，测量给了修正，不确定度决定了两者之间的权衡。

下一章进入频域滤波。频域滤波的思路不同：它不预测系统怎么变，而是从频率角度分析信号——哪些频率是噪声，哪些是真实信号，然后在频域里把噪声分量压掉。