卡尔曼滤波入门：从1D标量到2D矩阵¶

对标「一阶互补滤波 → Mahony → Madgwick」的递进学习路线， 从最朴素的标量卡尔曼开始，一步步建立对「预测-更新」、「协方差」、「卡尔曼增益」的直觉。 本文分析蓝本：xiahouzuoxin/kalman_filter（纯C实现，无矩阵库依赖，适合嵌入式） GitHub 仓库：https://github.com/xiahouzuoxin/kalman_filter

先搞懂：卡尔曼滤波到底在干什么？¶

用一句话说清楚¶

卡尔曼滤波 = 用「模型预测」和「传感器测量」加权平均，得到比两者都准的估计。

举个例子：你要估计一辆车的位置 - 🔮 预测：根据上一时刻的位置和速度，推算现在在哪（但速度有误差，越推越不准） - 📡 测量：GPS 直接告诉你现在在哪（但有噪声，跳来跳去） - ✅ 融合：把预测值和测量值按可信度加权，得到最优估计

卡尔曼滤波的两大步¶

           ┌─────────────┐         ┌─────────────┐
  输入 ──▶│   预测步     │──────▶│   更新步     │─────▶ 输出
           │ Prediction  │         │   Update    │
           └─────────────┘         └─────────────┘
                ▲                         │
                └─────────────────────────┘
                     用上一轮结果迭代

第一部分：1D 标量卡尔曼滤波¶

先从最简单的一维情况入手，建立所有核心概念。

1.1 五个核心公式¶

预测步（Predict）¶

1. 状态预测：\(\hat{x}_{k|k-1} = A \cdot \hat{x}_{k-1|k-1}\)
2. 协方差预测：\(P_{k|k-1} = A^2 \cdot P_{k-1|k-1} + Q\)

更新步（Update）¶

1. 卡尔曼增益：\(K_k = \frac{P_{k|k-1} \cdot H}{H^2 \cdot P_{k|k-1} + R}\)
2. 状态更新：\(\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \cdot (z_k - H \cdot \hat{x}_{k|k-1})\)
3. 协方差更新：\(P_{k|k} = (1 - K_k \cdot H) \cdot P_{k|k-1}\)

1.2 变量含义速查表¶

💡 最关键的直觉：Q 和 R 的比值决定了 K

• \(Q \gg R\) → K 大 → 更相信测量值
• \(Q \ll R\) → K 小 → 更相信预测模型

卡尔曼增益 K 本质上就是「可信度分配比例」。

1.3 代码逐行解析：kalman1_filter()¶

（kalman_filter.c 第 50-62 行）

float kalman1_filter(kalman1_state *state, float z_measure)
{
    /* Predict */
    state->x = state->A * state->x;                    // 第53行
    state->p = state->A * state->A * state->p + state->q;  // 第54行

    /* Measurement */
    state->gain = state->p * state->H / 
        (state->p * state->H * state->H + state->r);    // 第57行
    state->x = state->x + state->gain * 
        (z_measure - state->H * state->x);              // 第58行
    state->p = (1 - state->gain * state->H) * state->p; // 第59行

    return state->x;
}

让我一行一行掰开揉碎：

第 53 行：状态预测¶

state->x = state->A * state->x;

大白话：用上一时刻的最优估计，乘以状态转移系数，得到这一时刻的预测值。

举例： - 如果是恒温系统，A=1（温度不变） - 如果是匀速运动，A=1 + 速度项（但1D只有位置，所以简化为A=1） - 大多数1D滤波场景 A=H=1

第 54 行：协方差预测¶

state->p = state->A * state->A * state->p + state->q;

大白话：预测的不确定性 = 上一轮的不确定性经过模型放大 + 过程噪声。

🎯 核心直觉

每次预测，不确定性 P 都会变大（因为加了 Q）。 就像你闭着眼睛走路，走得越久，对自己位置越不确定。

第 57 行：计算卡尔曼增益¶

state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);

大白话：计算我应该相信「预测」多一点，还是相信「测量」多一点。

分子是预测的不确定性，分母是预测+测量的总不确定性。 - 如果预测很不确定（P很大）→ K 大 → 多信测量 - 如果测量很不准（R很大）→ K 小 → 多信预测

当 A=H=1 时，公式简化为：\(K = \frac{P}{P + R}\) 是不是一眼就看出是「加权平均」的权重了？

第 58 行：状态更新（核心！）¶

state->x = state->x + state->gain * (z_measure - state->H * state->x);

大白话：新估计 = 旧预测 + 增益 × (测量值 - 预测值)

括号里的 \(z_k - H\hat{x}_{k|k-1}\) 叫 新息（Innovation）， 就是「测量值和预测值的差」，代表我们有多"惊讶"。

卡尔曼滤波的本质就是：按比例 K，把预测往测量的方向拉一点。

第 59 行：协方差更新¶

state->p = (1 - state->gain * state->H) * state->p;

大白话：更新后，不确定性变小了（因为 1-KH < 1）。

🎯 核心直觉

每次更新，不确定性 P 都会变小（因为乘了小于1的系数）。 就像你睁开眼睛看了一眼，对自己的位置更确定了。

1.4 完整迭代过程动画理解¶

想象一个温度估计系统： 1. 初始：x=25°C，P=10（很不确定） 2. 预测：x不变，P变大（P = 10 + Q） 3. 测量：温度计显示 27°C，R=2 4. 更新：K = P/(P+R)，x = x + K*(27-x)，P = (1-K)*P 5. 下一轮：回到预测步...

P (不确定性)
  ↑
  │      预测→P变大         更新→P变小
  │         /^\               /
  │        /   \             /
  │       /     \           /
  │      /       \         /
  │     /         \       /
  └───────────────┴───────┴──→ 时间
        预测   更新   预测   更新

预测时不确定性上升，更新时不确定性下降，一升一降，动态平衡。

第二部分：2D 卡尔曼滤波（矩阵入门）¶

1D 太简单，实际工程中大多是多维状态。比如： - 位置 + 速度（2D） - 角度 + 角速度（2D） - 位置 + 速度 + 加速度（3D）

2.1 为什么需要矩阵形式？¶

多维状态下，状态之间是耦合的。比如： - 位置的不确定性会影响速度的估计 - 速度的不确定性会影响下一次位置预测

用矩阵可以一次性处理所有状态的耦合关系，形式也更简洁。

2.2 矩阵形式的卡尔曼滤波公式¶

预测步¶

更新步¶

是不是和1D版本长得一模一样？只是标量变成了矩阵，乘法变成了矩阵乘法。

2.3 匀速运动模型：F 矩阵的物理意义¶

最常用的2D模型：匀速运动模型（Constant Velocity Model）

状态向量：\(\mathbf{x} = \begin{bmatrix} x \\ \dot{x} \end{bmatrix} = \begin{bmatrix} \text{位置} \\ \text{速度} \end{bmatrix}\)

运动方程：

写成矩阵形式：

这就是状态转移矩阵 F 的物理意义——它描述了系统怎么从一个状态变到下一个状态。

2.4 观测矩阵 H 的物理意义¶

如果我们只能测量位置，测不到速度：

H 矩阵把「状态空间」映射到「测量空间」。 H = [1, 0] 的意思是：我只测第一个状态量（位置），不测第二个（速度）。

💡 卡尔曼滤波的神奇之处

即使你不测速度，卡尔曼滤波也能估计出速度！ 因为位置的变化规律隐含了速度信息，滤波器会从位置测量的历史中"猜"出速度。 这叫「状态观测」——用间接测量量估计不可直接测量的状态。

2.5 代码逐行解析：kalman2_filter()¶

（kalman_filter.c 第 113-147 行）

这个实现的精妙之处在于：没有用任何矩阵库，把2D矩阵运算完全展开成标量运算。 对于理解矩阵运算的底层含义，简直是完美教材。

Step 1：状态预测¶

// 第120-121行
state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1];
state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1];

对应公式：\(\hat{\mathbf{x}}_{k|k-1} = \mathbf{F} \cdot \hat{\mathbf{x}}_{k-1|k-1}\)

展开后：

代入默认的匀速模型 A = [[1, 0.1], [0, 1]]： - 新位置 = 旧位置 + 0.1 × 旧速度 - 新速度 = 旧速度（匀速假设）

⚠️ 代码小瑕疵提醒

第121行有个bug！计算 x[1] 时用了已经更新过的 x[0]，而不是旧的 x[0]。 正确的做法应该先把旧状态存起来，再计算新状态。 这是嵌入式实现中常见的错误，实际使用时要注意。

Step 2：协方差预测¶

// 第123-126行
state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0];
state->p[0][1] = state->A[0][0] * state->p[0][1] + state->A[1][1] * state->p[1][1];
state->p[1][0] = state->A[1][0] * state->p[0][0] + state->A[0][1] * state->p[1][0];
state->p[1][1] = state->A[1][0] * state->p[0][1] + state->A[1][1] * state->p[1][1] + state->q[1];

对应公式：\(\mathbf{P}_{k|k-1} = \mathbf{F} \cdot \mathbf{P}_{k-1|k-1} \cdot \mathbf{F}^T + \mathbf{Q}\)

这是2×2矩阵乘法的完整展开。让我们看看每个元素的含义：

💡 协方差是什么？

协方差描述两个变量的"联动"程度： - 正协方差 → 位置变大时速度也变大（正相关） - 负协方差 → 位置变大时速度变小（负相关） - 零协方差 → 两者独立，互不影响

卡尔曼滤波的精华就在于这个 P 矩阵——它不仅记录每个状态有多不确定， 还记录了各个状态之间的不确定是怎么关联的。

Step 3：计算卡尔曼增益¶

// 第130-134行
temp0 = state->p[0][0] * state->H[0] + state->p[0][1] * state->H[1];
temp1 = state->p[1][0] * state->H[0] + state->p[1][1] * state->H[1];
temp  = state->r + state->H[0] * temp0 + state->H[1] * temp1;
state->gain[0] = temp0 / temp;
state->gain[1] = temp1 / temp;

对应公式：\(\mathbf{K} = \mathbf{P} \cdot \mathbf{H}^T \cdot (\mathbf{H} \cdot \mathbf{P} \cdot \mathbf{H}^T + R)^{-1}\)

因为是1维测量（标量 R），所以求逆就是取倒数（除以 temp）。

• temp0 = P 的第一行 × H → 对应位置的增益分子
• temp1 = P 的第二行 × H → 对应速度的增益分子
• temp = 总不确定性（预测不确定性 + 测量不确定性）

每个状态量都有自己的增益 K： - K[0]：位置修正用的增益 - K[1]：速度修正用的增益

🔍 为什么速度也需要修正？

因为我们只测了位置，但位置误差里隐含了速度误差的信息。 卡尔曼滤波通过协方差 P，把位置测量的信息"传递"给速度估计。 这就是协方差矩阵的威力——一个传感器可以校正多个状态。

Step 4：状态更新¶

// 第136-138行
temp = state->H[0] * state->x[0] + state->H[1] * state->x[1];  // 预测的测量值
state->x[0] = state->x[0] + state->gain[0] * (z_measure - temp);  // 位置修正
state->x[1] = state->x[1] + state->gain[1] * (z_measure - temp);  // 速度修正

对应公式：\(\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K} \cdot (z - \mathbf{H} \cdot \hat{\mathbf{x}}_{k|k-1})\)

和1D版本结构完全一样！只是 x 和 K 都变成了向量。

注意：两个状态量用的是同一个新息 (z_measure - temp)， 但乘以不同的增益 gain[0] 和 gain[1]。

Step 5：协方差更新¶

// 第141-144行
state->p[0][0] = (1 - state->gain[0] * state->H[0]) * state->p[0][0];
state->p[0][1] = (1 - state->gain[0] * state->H[1]) * state->p[0][1];
state->p[1][0] = (1 - state->gain[1] * state->H[0]) * state->p[1][0];
state->p[1][1] = (1 - state->gain[1] * state->H[1]) * state->p[1][1];

对应公式：\(\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K} \cdot \mathbf{H}) \cdot \mathbf{P}_{k|k-1}\)

更新后，所有不确定性都减小了。

⚠️ 简化形式 vs 标准形式

这个实现用了简化的标量形式来更新 P，标准的矩阵形式应该是：

\[ P = (I - KH) P \]

对于2D情况完整展开会更复杂。这个简化版本在 H=[1,0] 时是正确的， 但通用性稍差。学习原理没问题，工程使用建议用标准矩阵运算。

第三部分：参数调参指南¶

3.1 三个需要自己填的数¶

3.2 调参口诀¶

Q大调测量，R大调模型；
P0只管开头，稳态不影响。

3.3 具体怎么调？¶

步骤一：先定 R - R 是传感器的测量噪声方差，可以实际测出来 - 把传感器静止放着，读1000个数据，算方差，就是 R 的参考值 - 例如：陀螺仪噪声的方差 = 0.01 deg²/s² → R = 0.01

步骤二：再定 Q - Q 是过程噪声，代表「模型不准的程度」，需要试 - 初始可以取 R 的 1/10 ~ 1/100 - 滤波结果太"跳"就减小 Q，太"滞"就增大 Q

步骤三：设 P₀ - 初始状态比较确定 → P₀ 设小（如 1.0） - 初始状态瞎猜的 → P₀设大（如 100.0），让滤波器快速收敛 - P₀ 只影响前几轮，最终都会收敛到稳态值

3.4 稳态卡尔曼增益¶

当滤波器运行足够长时间后，P 会收敛到一个稳定值，K 也会稳定。 这时候卡尔曼滤波就退化成了一阶互补滤波！

这正是我们学过的一阶互补滤波形式！

🎯 重要结论

卡尔曼滤波 = 「自动计算最优增益的」互补滤波

• 互补滤波：手动设一个固定的 α（增益）
• 卡尔曼滤波：自动计算最优的 K（还随时间变化）

卡尔曼的优势在于：它能根据不确定性的变化自动调整权重， 而且在多维状态下能处理耦合关系。

第四部分：初始化函数解析¶

4.1 1D 初始化¶

（kalman_filter.c 第 30-38 行）

void kalman1_init(kalman1_state *state, float init_x, float init_p)
{
    state->x = init_x;       // 初始状态估计
    state->p = init_p;       // 初始不确定性
    state->A = 1;            // 默认状态转移系数=1
    state->H = 1;            // 默认观测系数=1
    state->q = 2e2;          // 过程噪声（默认值，需根据应用调整）
    state->r = 5e2;          // 测量噪声（默认值，需根据应用调整）
}

4.2 2D 初始化¶

（kalman_filter.c 第 78-98 行）

void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2])
{
    state->x[0]    = init_x[0];      // 初始位置
    state->x[1]    = init_x[1];      // 初始速度
    state->p[0][0] = init_p[0][0];   // 初始位置方差
    state->p[0][1] = init_p[0][1];   // 初始协方差
    state->p[1][0] = init_p[1][0];   // 初始协方差
    state->p[1][1] = init_p[1][1];   // 初始速度方差

    state->A[0][0] = 1;
    state->A[0][1] = 0.1;            // dt = 0.1s（10Hz采样）
    state->A[1][0] = 0;
    state->A[1][1] = 1;

    state->H[0]    = 1;              // 只测位置
    state->H[1]    = 0;

    state->q[0]    = 10e-7;          // 位置过程噪声
    state->q[1]    = 10e-7;          // 速度过程噪声
    state->r       = 10e-7;          // 测量噪声
}

⚠️ 默认参数提醒

代码里的 q 和 r 默认值（如 2e2、5e2、10e-7）是作者针对特定应用调的， 千万不要直接照搬，一定要根据你的传感器和应用场景重新调试。 Q和R的比值才是关键，绝对值的影响是次要的。

第五部分：卡尔曼滤波 vs 互补滤波 vs Mahony¶

5.1 滤波算法家族谱¶

一阶互补滤波 ── 最简单，固定增益
    │
    ├── 二阶互补滤波 ── 加入积分项，可估计零偏
    │
    └── Mahony 滤波 ── 叉乘误差 + PI 控制器，四元数姿态
         │
         └── Madgwick 滤波 ── 梯度下降优化，更"数学"的表述
              │
              └── 卡尔曼滤波 ── 概率框架下的最优估计，可处理多维耦合
                   │
                   ├── 线性卡尔曼（LKF）── 线性系统
                   ├── 扩展卡尔曼（EKF）── 非线性系统，泰勒展开
                   └── 误差状态卡尔曼（ESKF）── 估计误差而非状态，惯导常用

5.2 什么时候用卡尔曼？¶

用互补滤波/Mahony就够的场景： - 单传感器简单滤波 - 对精度要求不极致 - MCU 算力非常有限（8位机）

应该上卡尔曼的场景： - 多传感器融合（加速度计 + 陀螺仪 + GPS） - 需要估计多个耦合的状态量 - 有明确的系统动力学模型 - 对"最优性"有要求

💡 学习建议

姿态解算入门阶段，先掌握 Mahony 就够用了。 卡尔曼滤波是下一步进阶 INS/GNSS 组合导航的基础， 理解了卡尔曼，再学 EKF 和 ESKF 就是水到渠成。

代码-公式-含义 对照表¶

总结¶

卡尔曼滤波的核心思想其实非常朴素：

1. 预测：用模型往前推一步，不确定性增加
2. 更新：用测量往回拉一点，不确定性减少
3. 加权：卡尔曼增益 K 决定了拉多少，K 由 Q 和 R 的比值决定
4. 迭代：不断重复预测-更新，越来越准

一句话记住卡尔曼滤波：

"预测一下，再用测量修正一下，修正的幅度按可信度加权。"

掌握了1D和2D的基础，接下来学习 EKF（扩展卡尔曼）和 ESKF（误差状态卡尔曼）就只是在这个框架上增加非线性处理而已，核心逻辑永远是这两大步。

参考资料¶

开源代码¶

• xiahouzuoxin/kalman_filter — 本文分析蓝本，纯C实现
• Arduino 卡尔曼滤波库 — 更简单的1D实现

经典教程¶

• 《卡尔曼滤波从入门到精通》系列 — B 站搜索"DR_CAN"，讲得最透彻的中文教程
• An Introduction to the Kalman Filter — Greg Welch & Gary Bishop，经典入门论文
• 《最优状态估计》— Dan Simon，经典教材，数学推导详尽

惯导进阶¶

• 武汉大学牛小骥老师《组合导航原理》课程
• 《捷联惯导算法与组合导航原理》— 严龚敏