一篇文章了解卡尔曼滤波器，原理加代码的超详细讲解

原理篇

专业名词解释

---

1. 状态 (State, xxx)

• 专业定义：描述系统在某一时刻的内部变量（可能是位置、速度、温度等），我们希望估计它。
• 通俗解释：你真正想知道的“真实情况”。
  👉 比如：小车的真实位置、无人机的真实速度。

---

2. 测量 (Measurement, $z(k)$)

• 专业定义：传感器在第 k 时刻读到的值，包含真实状态 + 测量噪声。
• 通俗解释：仪器上显示的数据，可能抖动不准。
  👉 比如：GPS 显示位置、电子秤显示的体重。

---

3. 先验估计 (A priori estimate, ${\hat{x}}^{-}(k))$

• 专业定义：在看到第 kkk 次测量之前，仅根据模型预测得到的状态估计。
• 通俗解释：还没测量前，凭经验/公式算出来的“预估值”。
  👉 比如：昨天体重 70kg，今天没吃饭 → 预测还是 70kg。

---

4. 后验估计 (A posteriori estimate, $\hat{x}(k)$)

• 专业定义：融合了预测和测量之后的状态估计，通常比先验更准确。
• 通俗解释：结合了“经验 + 测量”的最终判断。
  👉 比如：预测 70kg，秤显示 71kg → 更新后 70.6kg。

---

5. 估计误差协方差 (Estimation error covariance, $P(k)$)

• 专业定义：描述估计值与真实值的差异不确定性（方差/置信度）。
• 通俗解释：你对自己的估计有多大把握。数越小，越有信心。
  👉 比如：±0.5kg 的误差范围。

---

6. 测量噪声协方差 (Measurement noise covariance, $R$)

• 专业定义：测量过程的噪声大小，用方差表示。
• 通俗解释：传感器本身的抖动程度。
  👉 比如：GPS 定位精度 ±3 米。

---

7. 过程噪声协方差 (Process noise covariance, $Q$)

• 专业定义：预测模型的不确定性（系统演化时的随机性）。
• 通俗解释：你预测不可能完全准，总会有点意外变化。
  👉 比如：小车行驶时路面摩擦不同、风力影响。

---

8. 卡尔曼增益 (Kalman Gain, $K(k)$)

• 专业定义：一个在 0~1 之间的权重系数，决定了更新时“更信测量”还是“更信预测”。
• 通俗解释：判断到底该听“传感器”还是听“模型”。
  👉 传感器准 → K 大；预测准 → K 小。

---

9. 创新 / 残差 (Innovation, $z(k)-{\hat{x}}^{-}(k)$)

• 专业定义：测量值和预测值的差异。
• 通俗解释：现实和预期的差距。
  👉 预测 70kg，秤上 71kg → 创新 = 1kg。

---

10. 递推 (Recursion)

• 专业定义：卡尔曼滤波每次估计都是基于上一次结果递推得到的。
• 通俗解释：一步一步迭代更新，不需要存所有历史数据。
  👉 今天的估计用昨天的结果来算，明天继续用今天的。

卡尔曼滤波器的计算步骤

---

1. 卡尔曼滤波的两个阶段

卡尔曼滤波的每一步都分成两步：

1. 预测（Prediction，先验）
   • 用系统模型把上一步的状态 $\hat{x}(k-1)$ 推算到下一步 → 得到${\hat{x}}^{-}(k)$
   • 它是“更新前”的估计（带有预测误差方差 $P^{-}(k)$）
2. 更新（Update，后验）
   • 当第 $k$ 次测量 $z(k)$ 到来后，把它和预测值 ${\hat{x}}^{-}(k)$ 融合 → 得到新的估计 $\hat{x}(k)$
   • 这个就是“更新后”的估计（带有新的误差方差 $P(k)$）

---

2. 数学符号区分

• 更新前（先验）

  ${\hat{x}}^{-}(k),P^{-}(k)$

  （仅由模型预测得到的）

• 更新后（后验）

  $\hat{x}(k),P(k)$

  （融合了测量信息的，更可靠的）

---

3. 举个通俗例子

你要估计一个人的体重：

• 预测阶段：你知道他昨天是 70kg，今天没怎么吃饭，预测大概还是 70kg 左右 → 这是 ${\hat{x}}^{-}(k)$
• 测量阶段：你用一个有点噪声的秤称了一下，显示 71kg → 这是 $z(k)$
• 更新后：你把预测（70kg）和测量（71kg）加权平均，得出一个新估计，比如 70.6kg → 这就是 $\hat{x}(k)$

下一次循环，就会拿这个“更新后的值”作为新的起点。

---

✅ 所以，“更新后”就是指：
融合了预测结果和当前测量之后，得到的最终估计。
数学上叫 后验估计 (a posteriori estimate)，对应的“更新前”叫 先验估计 (a priori estimate)。

理解卡尔曼滤波器

---

🧭 1. 我们在解决什么问题？

现实里，我们测量一个量（比如位置、温度、电压），总是带噪声：

$z_k=x+v_k$

• $x$：真实值（我们想知道的东西）
• $z_k$：传感器测到的值
• $v_k$：噪声（随机抖动、误差）

如果我们连续测量很多次，希望得到一个更接近真实值的估计。

---

⚖️ 2. 最简单的思路：加权平均

我们有两个来源：

1. 预测/模型值（先验）：来自之前的估计，带有不确定性
2. 测量值：来自传感器，带有噪声

最自然的融合方式：

$\hat{x}(k)=(1-K){\hat{x}}^{-}(k)+Kz(k)$

• ${\hat{x}}^{-}(k)$：预测值
• $z(k)$​：测量值
• $K$：加权系数（卡尔曼增益 Kalman Gain）

👉 就是一个加权平均：到底是更信预测，还是更信测量？

---

📊 3. 谁更准，信谁多一点

这里要用到两个量：

• 估计误差方差$e_{\text{EST}}$：预测的不确定性
• 测量误差方差 $e_{\text{MEA}}$：传感器的噪声大小

直觉：

• 如果传感器很准$e_{\text{MEA}}$，就多信测量
• 如果预测模型很准$e_{\text{EST}}$，就多信预测

---

📐 4. 卡尔曼增益公式是怎么来的？(完整推导过程见附录)

目标：让更新后的估计 误差最小。

数学推导结果就是：

$K(k)=\frac{e_{\text{EST}}(k-1)}{e_{\text{EST}}(k-1)+e_{\text{MEA}}(k)}$

解释：

• 分母：总的不确定性（预测 + 测量）
• 分子：预测的不确定性
• 所以 $K$ 在 0 和 1 之间变化，自动决定权重。

---

🔄 5. 动态过程（每一步都更新）

1. 先验误差更新：

   $e_{\text{EST}}(k)=(1-K(k))e_{\text{EST}}(k-1)$

   → 每次融合后，不确定性减小

2. 卡尔曼增益更新：
   随着估计越来越准，K(k)越来越小
   → 表示“我已经有把握了，不再过度依赖单次测量”

---

🏖️ 6. 对于静态量时的变化趋势

• 测量误差 $e_{\text{MEA}}$：固定，由传感器决定
• 估计误差 $e_{\text{EST}}$：逐步下降（越测越准）
• 卡尔曼增益 $K(k)$：逐步下降，趋近于 0（后期几乎只依赖已有估计）

如果考虑系统可能有小漂移（过程噪声 Q>0），那$e_{\text{EST}}$和 $K$ 不会归零，而是收敛到某个稳定值。

代码篇

源码

kalman_filter.h

/*
 * kalman_filter.h
 *
 *  Created on: Jul 14, 2024
 *      Author: 废话文学创始人
 *     version: v1.1
 *  @note 可实现对单变量的动态跟踪和稳定时的滤波
 */

#ifndef __kalman_filter_H_
#define __kalman_filter_H_

#include "main.h"

// 估计值
typedef struct
{
    float last;
    float current;
} Kalman_Estimate;

typedef struct
{
    float measure; // 观测值
    Kalman_Estimate estimate;
} Kalman_Value;
typedef struct
{
    float measure;  // 测量误差
    float estimate; // 估计误差
} Kalman_Error;

typedef struct
{
    Kalman_Value value;
    Kalman_Error error;
    float Gain;               // 卡尔曼系数
    float error_estimate_buf; // 添加动态估计误差缓冲区
} Kalman;

void Kalman_Error_Set(Kalman *kalman, float error_estimate, float error_measure);
void Kalman_calculate(Kalman *kalman, float value_measure);
float Kalman_Value_Estimate_Get(Kalman kalman);
void Kalman_Init(Kalman *kalman, float initial_estimate, float error_estimate, float error_measure);

#endif /* __kalman_filter_H_ */

kalman_filter.c

/*
 * kalman_filter.c
 *
 *  Created on: Jul 14, 2024
 *      Author: 废话文学创始人
 *     version: v1.1
 */
#include "kalman_filter.h"
#include <stdlib.h>
#include <math.h>

/**
 * 完整的卡尔曼滤波器初始化
 */
void Kalman_Init(Kalman *kalman, float initial_estimate, float error_estimate, float error_measure)
{
    kalman->value.estimate.current = initial_estimate;
    kalman->value.estimate.last = initial_estimate;
    kalman->error.estimate = error_estimate;
    kalman->error.measure = error_measure;
    kalman->error_estimate_buf = error_estimate;
    kalman->Gain = 0;
}

/**
 * 设置初始状态的估计误差和观测误差
 * @param error_estimate
 * @param error_measure
 * @note  估计误差动态变化 但会将初始估计误差保留
 * @note  观测误差一般不变
 */
void Kalman_Error_Set(Kalman *kalman, float error_estimate, float error_measure)
{
    kalman->error.estimate = error_estimate;
    kalman->error_estimate_buf = error_estimate;
    kalman->error.measure = error_measure;
}

/**
 * 设置观测量
 * @param value_measure
 * @note  必须预先设置一次
 */
void Kalman_Value_Measure_Set(Kalman *kalman, float value_measure)
{
    kalman->value.measure = value_measure;
}

/**
 * 更新卡尔曼系数
 */
static void Kalman_Gain_Update(Kalman *kalman)
{
    kalman->Gain = kalman->error_estimate_buf / (kalman->error_estimate_buf + kalman->error.measure);
}

/**
 * 更新当前估计值
 */
static void Kalman_Value_Estimate_Update(Kalman *kalman)
{
    kalman->value.estimate.last = kalman->value.estimate.current;
    kalman->value.estimate.current = kalman->value.estimate.last + kalman->Gain * (kalman->value.measure - kalman->value.estimate.last);
}

/**
 * 更新估计误差
 */
static void Kalman_Error_Estimate_Update(Kalman *kalman)
{
    kalman->error_estimate_buf = (1.0f - kalman->Gain) * kalman->error_estimate_buf;
}

/**
 * 重新定位估计值
 * @param kalman
 * @note  此函数是实现动态跟踪的关键
 */
static void Kalman_Clear(Kalman *kalman)
{
    kalman->error_estimate_buf = kalman->error.estimate;
}
/**
 * 卡尔曼滤波算法
 * @param value_measure 测量到的观测值
 * @note  在计算前调用Kalman_Error_Set来设置参数
 * @note  在计算前调用Kalman_Value_Measure_Set来奠定基调
 */
void Kalman_calculate(Kalman *kalman, float value_measure)
{
    Kalman_Value_Measure_Set(kalman, value_measure);
    if (fabsf(kalman->value.estimate.current - kalman->value.measure) >= kalman->error.measure)
    {
        Kalman_Clear(kalman);
    }
    Kalman_Gain_Update(kalman);
    Kalman_Value_Estimate_Update(kalman);
    Kalman_Error_Estimate_Update(kalman);
}

/**
 * 获取当前的估计值
 * @return 本次计算后的估计值
 * @note   如果实际值不变估计值应当有收敛的趋势
 */
float Kalman_Value_Estimate_Get(Kalman kalman)
{
    return kalman.value.estimate.current;
}

伪代码示例

此代码可对有小噪声的一维信号进行滤波处理，不止可以测不变的电压，代码对动态变化的信号进行过处理，有矫正机制，但需要根据自己的实际情况进行调参。

//以简单的测量电压为例，需要预先设置估计误差和测量误差
//--------------------------- init ---------------------------------
//测量误差一般不变，估计误差会决定结果收敛的快慢，随着不断的测量矫正，估计误差会越来越小
Kalman_Error_Set(&kalman1,Error_Estimate,Error_Measure);
//---------------------------update------------------------------
//测量实际电压value进行更新，矫正误差
//此函数需要周期进行，建议配合定时器
Kalman_calculate(&kalman1,(float )value);
//----------------------------read--------------------------
//前两步做好了就可以读取滤波后的电压值了
float result = kalman1.value.estimate.current;

附录

卡尔曼增益系数的完整推导过程

---

1️⃣ 符号说明

1. $x$ ：真实值，你希望估计的量。
2. $\hat{x}(k)$：第 k 次更新后的估计值（后验）。
3. ${\hat{x}}^{-}(k)$：第 k 次更新前的预测值（先验）。
4. $e(k)=x-\hat{x}(k)$：更新后的估计误差。
5. $P(k)=E[e(k{)}^2]$：更新后的估计误差方差（协方差，如果是一维就是方差）。
6. $R$：测量噪声方差。

---

2️⃣ 更新公式

卡尔曼滤波的核心更新公式是：
$$\hat{x}(k)={\hat{x}}^{-}(k)+K(k)(z^{-}(k)-{\hat{x}}^{-}(k))$$

• $z(k)$ = 测量值
• $K(k)$ = 卡尔曼增益

直观：更新 = 预测 + (信任测量的权重 × 创新)

---

3️⃣ 更新误差公式

更新后的估计误差：
$$e(k)=x-\hat{x}(k)=x-[{\hat{x}}^{-}(k)+K(k)(z(k)-{\hat{x}}^{-}(k))]$$
展开：
$$e(k)=\underset{e^{-}(k)}{\underbrace{x-{\hat{x}}^{-}(k)}}-K(k)(z(k)-{\hat{x}}^{-}(k))$$

定义：

• $e^{-}(k)=x-{\hat{x}}^{-}(k)$（预测误差，先验）
• $v(k)=z(k)-x$（测量噪声）

所以：
$$z(k)-x^{-}(k)=(x+v(k))-x^{-}(k)=e^{-}(k)+v(k)$$

代回：
$$\begin{array}{rl}e(k)& =e^{-}(k)-K(k)(e^{-}(k)+v(k))\\ & =(1-K(k))e^{-}(k)-K(k)v(k)\end{array}$$

✅ 这是更新误差的关键公式。

---

4️⃣ 更新误差方差 P(k)

假设预测误差 $e^{-}(k)$ 与测量噪声 $v(k)$互不相关，则更新后的方差为：
$$\begin{array}{rl}P(k)& =E[e(k{)}^2]\\ & =E[((1-K(k))e^{-}(k)-K(k)v(k){)}^2]\\ & =(1-K(k){)}^{2}E[(e^{-}(k){)}^2]+K(k{)}^{2}E[v(k{)}^2]\\ & =(1-K(k{)}^2)P^{-}(k)+K(k{)}^{2}R\end{array}\text{\hspace{1em}(1)(2)}$$

• $P^{-}(k)=E[(e^{-}(k){)}^2]=$ 预测误差方差
• $R=E[v(k{)}^2]=$ 测量误差方差

注意：交叉项 $E[e^{-}v]$为 0，因为它们不相关。

PS: $(1)\to (2)的完整推导如下$

起点 (1)

$$P(k)=E[((1-K(k))e^{-}(k)-K(k)v(k){)}^2]\text{\hspace{1em}(1)}$$

第一步：代数展开平方

$$((1-K)e^{-}-Kv)2=(1-K{)}^2(e^{-}{)}^2-2K(1-K)e^{-}v+K^2{v}^2$$

代入期望：
$$E[(1-K{)}^2(e^{-}{)}^2-2K(1-K)e^{-}v+K^2{v}^2]\text{\hspace{1em}(3)}$$

---

第二步：利用期望的线性性

常数可以提出，期望可以分拆：
$$(3)=(1-K{)}^{2}E[(e^{-}{)}^2]-2K(1-K)E[e^{-}v]+K2E[v^2]$$

---

第三步：去掉交叉项

在标准 卡尔曼滤波假设 下：

• 预测误差$e^{-}(k)$ 与测量噪声 $v(k)$ 不相关（独立更强）；
• 且二者均为零均值：$E[e^{-}(k)v(k)]=E[e^{-}(k)]E[v(k)]=0$。

于是
$$E[e^{-}(k)v(k)]=E[e^{-}(k)]E[v(k)]=0$$
所以交叉项 消失。

---

得到 (2)

$$\begin{gathered} (1-K(k){)}^{2}E[(e^{-}(k){)}^2]+K(k{)}^{2}E[v(k{)}^2] \\ \text{\hspace{1em}(2)} \end{gathered}$$

---

5️⃣ 最优增益 K(k)

我们希望选择 $K(k)$ 使 $P(k)$ 最小。

5.1 对 $K(k)$ 求导

$$P(k)=(1-K{)}^2{P}^{-}+K^{2}R$$

对 K 求导：

$$\frac{dP}{dK}=2(1-K)(-1)P^{-}+2KR=-2(1-K)P^{-}+2KR$$

令导数为 0：

$$-2(1-K)P^{-}+2KR=0$$

---

5.2 化简

$$\begin{gathered} -2P^{-}+2K{P}^{-}+2KR=0 \\ K(P^{-}+R)=P^{-} \\ K=\frac{P^{-}}{P^{-}+R} \end{gathered}$$

✅ 这就是卡尔曼增益的经典公式。

6️⃣ 总结

1. 更新误差：
   $$e(k)=(1-K(k))e^{-}(k)-K(k)v(k)$$

2. 更新误差方差：
   $$P(k)=(1-K(k){)}^2{P}^{-}+K(k{)}^{2}R$$

3. 最优卡尔曼增益：

$$K(k)=\frac{P^{-}}{P^{-}+R}$$

核心思想：给预测和测量按“可信度”加权，使更新后的估计误差最小。。