2、卡尔曼滤波基础：线性卡尔曼滤波的数学原理、状态空间模型、预测与更新步骤
在Edge AI的传感器融合场景中，我们面对的核心问题是如何从带有噪声的观测数据中，实时估计系统内部无法直接测量的状态。无论是无人机姿态解算中的四元数估计，还是车载IMU与GPS组合导航中的位置速度修正，本质上都是在做一件事：用数学模型描述系统演化，用统计方法融合多源信息。线性卡尔曼滤波（Linear Kalman Filter, KF）正是解决这类问题的最优递推估计方法。它不要求存储历史数据，只需保留上一时刻的状态估计和误差协方差，即可在每一时刻完成“预测-更新”的闭环迭代，计算复杂度为O(n³)（n为状态维度），非常适合在资源受限的Edge设备上运行。
上图展示了线性卡尔曼滤波的完整迭代闭环。左侧为预测阶段，利用系统模型（状态转移矩阵A和控制输入B）将上一时刻的最优估计推进到当前时刻，同时更新误差协方差P；右侧为更新阶段，结合传感器观测值zₖ和观测矩阵H，计算卡尔曼增益Kₖ，对预测值进行加权修正，得到当前时刻的最优估计。这个“预测-修正”的循环从k=1开始持续迭代，每一时刻的输出x̂ₖ既是当前最优估计，又是下一时刻预测的输入。
2.1 状态空间模型：从物理系统到数学表达
卡尔曼滤波的起点是将物理系统建模为状态空间描述。状态向量xₖ包含了描述系统完整运动状态所需的最小变量集合。以Edge AI中最常见的IMU+GPS融合为例，若我们要估计一辆AGV小车的平面位置和速度，状态向量可定义为：
x = [px, py, vx, vy]ᵀ其中px、py为二维坐标，vx、vy为对应速度分量。状态空间模型由两个核心方程构成：
- 状态方程（过程模型）：xₖ = A·xₖ₋₁ + B·uₖ + wₖ，描述系统状态如何随时间演化。A为状态转移矩阵，B为控制输入矩阵，uₖ为外部控制量（如电机指令加速度），wₖ为过程噪声，通常假设为零均值高斯白噪声，协方差为Q。
- 观测方程（测量模型）：zₖ = H·xₖ + vₖ，描述传感器如何观测状态。H为观测矩阵，将状态映射到观测空间，vₖ为观测噪声，协方差为R。
对于离散时间系统，若采样周期为Δt，匀速运动模型的A矩阵为：
A = [[1, 0, Δt, 0],
[0, 1, 0, Δt],
[0, 0, 1, 0],
[0, 0, 0, 1]]若GPS直接输出位置坐标，则观测矩阵H为：
H = [[1, 0, 0, 0],
[0, 1, 0, 0]]这意味着GPS只观测位置分量，速度分量需要通过滤波器的预测机制间接估计。Q和R矩阵的取值反映了对模型和传感器的信任程度，是滤波性能的关键调参对象。
2.2 预测步骤：时间更新方程
预测步骤利用系统模型在观测到来之前先“猜”一个状态。其数学表达为：
先验状态估计：x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ
先验误差协方差：Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q第一个方程的含义是：如果系统完全遵循模型运动，那么当前状态应该等于上一状态乘以转移矩阵再加上控制输入。第二个方程则量化了这个预测的不确定性：Pₖ⁻由上一时刻的协方差Pₖ₋₁经过A的线性变换后，叠加过程噪声Q得到。Q越大，Pₖ⁻增长越快，表示模型预测越不可靠，后续滤波会更依赖观测数据。
在Edge设备上实现时，需要注意矩阵乘法的计算效率。对于4维状态向量，A·Pₖ₋₁·Aᵀ涉及三次矩阵乘法，但可以利用A的稀疏结构优化。例如，匀速模型的A矩阵中只有对角线和对角线偏移处有非零元素，实际计算时可展开为标量运算，避免通用矩阵乘法库的开销。
2.3 更新步骤：观测修正与卡尔曼增益
当传感器观测值zₖ到达后，滤波器进入更新阶段。首先计算卡尔曼增益Kₖ：
Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹卡尔曼增益的物理意义是预测误差与观测误差的比值权重。若R很小（传感器精度高），则Kₖ趋近于H⁻¹，滤波器几乎完全信任观测；若Q很小（模型很准），则Pₖ⁻很小，Kₖ趋近于0，滤波器更信任预测值。随后计算后验状态估计和协方差：
后验状态：x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ - H·x̂ₖ⁻)
后验协方差：Pₖ = (I - Kₖ·H)·Pₖ⁻其中(zₖ - H·x̂ₖ⁻)称为新息（Innovation），表示实际观测与预测观测之间的差异。这个差异包含了模型未能预测到的信息，乘以Kₖ后作为修正量叠加到先验估计上。Pₖ的更新公式表明，观测修正后不确定性必然减小（因为(I - Kₖ·H)的特征值在[0,1]区间）。
2.4 完整迭代流程与工程实现要点
线性卡尔曼滤波的完整算法流程可归纳为以下五个步骤，在Edge设备上通常以固定采样周期（如100Hz）循环执行：
- 初始化：设定x̂₀和P₀。若初始状态未知，可设x̂₀=0，P₀取较大值（如100·I），表示初始不确定性很高。
- 预测状态：根据上一时刻最优估计和控制输入，计算x̂ₖ⁻。
- 预测协方差：更新Pₖ⁻，反映预测不确定性。
- 计算增益：结合观测噪声R，计算Kₖ。
- 更新估计：利用实际观测zₖ修正状态和协方差，输出x̂ₖ作为当前最优估计。
| 参数 | 物理含义 | 典型取值方法 | 调参影响 |
|---|---|---|---|
| Q（过程噪声） | 模型未建模动态的强度 | 根据加速度噪声功率谱密度估算 | Q过大→滤波波动大；Q过小→响应迟钝 |
| R（观测噪声） | 传感器测量误差方差 | 从传感器数据手册或静态实验统计 | R过大→信任模型；R过小→信任观测 |
| P₀（初始协方差） | 初始状态不确定性 | 取较大值加速收敛 | 仅影响前几步收敛速度 |
| Δt（采样周期） | 滤波执行间隔 | 与传感器输出频率匹配 | Δt过大→离散化误差增大 |
2.5 案例拆解：AGV小车IMU+GPS融合定位
我们以一个具体的Edge AI应用场景来完整演示线性卡尔曼滤波的运作。某仓储AGV小车配备低成本IMU（加速度计+陀螺仪）和单频GPS模块，需要实现±0.5m以内的定位精度。IMU输出频率100Hz，GPS输出频率10Hz，两者时间不同步。
系统建模：状态向量取[px, py, vx, vy]ᵀ，IMU的加速度计输出经积分后作为控制输入uₖ（即加速度ax, ay），状态方程中的B矩阵为：
B = [[0.5·Δt², 0],
[0, 0.5·Δt²],
[Δt, 0],
[0, Δt]]GPS作为观测源，观测矩阵H取位置观测形式。由于IMU积分存在漂移，过程噪声Q需根据加速度计噪声密度设定，这里取Q = diag(0.01, 0.01, 0.04, 0.04)。GPS的定位精度约2.5m（CEP），换算为方差R = 6.25·I₂。
运行结果：在10秒直线行驶测试中，纯IMU积分位置漂移达3.8m，纯GPS位置跳动标准差1.2m，而卡尔曼滤波输出位置误差始终保持在0.3m以内。关键原因在于：GPS低频高精度信息修正了IMU的长期漂移，IMU高频信息填补了GPS更新间隔内的运动细节。
实现中的关键细节：
- 时间对齐：由于IMU和GPS频率不同，需维护一个缓冲区。当GPS数据到达时，先执行多次IMU预测步骤，再执行一次更新步骤。
- 异常值处理：GPS偶尔出现多路径效应导致跳变，可通过新息卡方检验剔除异常观测。若|zₖ - H·x̂ₖ⁻| > 3·√(H·Pₖ⁻·Hᵀ + R)，则跳过本次更新。
- 数值稳定性：P矩阵需保持对称正定，实际代码中采用Joseph形式更新：Pₖ = (I - KₖH)·Pₖ⁻·(I - KₖH)ᵀ + Kₖ·R·Kₖᵀ，避免因舍入误差导致协方差失去正定性。
2.6 总结与复盘
线性卡尔曼滤波的核心价值在于以解析递推方式实现了最小均方误差意义下的最优状态估计。它巧妙地平衡了模型预测与观测修正之间的权重，且计算量固定、无需存储历史数据，天然适合Edge AI场景。掌握本节内容需要抓住三个关键点：
- 模型是基础：状态空间模型的准确性直接决定滤波上限。A、B、H矩阵必须与实际物理过程匹配，否则再好的调参也无法弥补模型失配。
- 噪声是桥梁：Q和R不是随意设置的参数，它们反映了对模型和传感器的信任程度，是滤波器行为的“旋钮”。
- 迭代是灵魂：预测与更新的交替循环，使得滤波器能够在线实时运行，且每一时刻的输出都包含了历史所有信息的压缩表达。