4500 字

23 分钟

【卡尔曼滤波】01 - 理论基础入门

2024-12-03

2026-02-25

学习笔记

控制工程

/

算法

1. 状态空间表达式#

$W_k$ ：过程噪声

$V_k$ ：观测噪声

2. 系统框图#

3. 高斯分布#

卡尔曼滤波的噪声分布为高斯分布

$W_k$ , $Q_k$ 的定义：

举个栗子：

一辆以大约5m/s行驶的小车，收到各种阻力的影响，运动速度为 $V + V_r$

实际速度为： $5 + \delta$ (m/s)

$W_k = \delta$ m/s

$\delta \in N(0, 1)$ 的高斯分布， $Q_k = 1$

4. 协方差#

$\Delta_{11}$ 就是 $\text{Cov}(x_1, x_2)$ 的简化形式

二维协方差#
二维协方差（或称为协方差矩阵）是一个扩展了标准协方差概念的工具，适用于多维数据，特别是用于描述两个或更多变量之间的关系。在二维情况下，协方差矩阵可以用于表示两个随机变量 $X$ 和 $Y$ 的联合分布中的协方差。
协方差矩阵的表示#
对于二维数据 $(X, Y)$ ，协方差矩阵表示为：
其中：

$\text{Cov}(X, X)$ 是 $X$ 的方差，表示 $X$ 自身的波动程度。

$\text{Cov}(Y, Y)$ 是 $Y$ 的方差，表示 $Y$ 自身的波动程度。

$\text{Cov}(X, Y)$ 是 $X$ 和 $Y$ 之间的协方差，表示它们之间的线性关系。

$\text{Cov}(Y, X)$ 与 $\text{Cov}(X, Y)$ 相等，因为协方差是对称的。

因此，二维协方差矩阵包含了两个变量的方差和它们之间的协方差。协方差矩阵的对角线元素是方差，非对角线元素是协方差。
二维协方差矩阵的计算#
假设有数据集 $X = [x_1, x_2, \ldots, x_n]$ 和 $Y = [y_1, y_2, \ldots, y_n]$ ，则计算步骤：

计算每个变量的均值 $\overline{X}$ 和 $\overline{Y}$ 。

计算每对变量之间的协方差：

$\text{Cov}(X, Y) = \frac{1}{n} \sum_{i=1}^{n} (X_i - \overline{X})(Y_i - \overline{Y})$

构建协方差矩阵。

应用#
二维协方差矩阵在以下领域有广泛应用：

多元统计分析

机器学习算法（如主成分分析）

金融分析

5. 超参数#

卡尔曼滤波器主要调节两个参数：

$Q$ ：过程噪声的方差

$R$ ：观测噪声的方差

卡尔曼滤波#

$X_{k-1}$ ：最优估计值，后验，就是卡尔曼滤波器的输出值

$X_k^{-}$ ：基于 $X_{k-1}$ 的估计值，先验估计值

$y_k$ ：传感器的测量值，当前时刻的观测值

$X_k$ ：当前的最优估计值

当前的最优估计值可以理解为先验估计值和测量值重合相关得到的结果。

卡尔曼滤波#

一、实现过程#

使用上一次的最优结果，预测当前的值，同时用观测值修正当前值，得到最优结果。

1
# -*- coding: utf-8 -*-
2
"""
3
@对理想的一维匀加速直线运动模型，配有不精确的imu和不精确的gps，进行位置观测，协方差均使用矩阵的方式表示，以适配多维特征
4
"""
5
import numpy as np
6
import matplotlib.pyplot as plt
7

8
t = np.linspace(1,100,100) # 在1~100s内采样100次
9
u = 0.6 # 加速度值，匀加速直线运动模型
10
v0 = 5 # 初始速度
11
s0 = 0 # 初始位置
12
X_true = np.array([[s0], [v0]])
13
size = t.shape[0] + 1
14
dims = 2 # x, v, [位置, 速度]
15

16
# 这两个参数可以理解为Q：对测量器的信任度 R：对系统模型的信任度
17
Q = np.array([[1e1,0], [0,1e1]]) # 过程噪声的协方差矩阵，这是一个超参数
18
R = np.array([[1e10,0], [0,1e10]]) # 观测噪声的协方差矩阵，也是一个超参数。
19

20

21
# R_var = R.trace()
22
# 初始化
23
X = np.array([[0], [0]]) # 估计的初始状态，[位置, 速度]，就是我们要估计的内容，可以用v0，s0填入，也可以默认为0，相差越大，收敛时间越长
24
P = np.array([[0.1, 0], [0, 0.1]]) # 先验误差协方差矩阵的初始值，根据经验给出
25
# 已知的线性变换矩阵
26
F = np.array([[1, 1], [0, 1]]) # 状态转移矩阵
27
B = np.array([[1/2], [1]]) # 控制矩阵
28
H = np.array([[1,0],[0,1]]) # 观测矩阵
29

30
# 根据理想模型推导出来的真实位置值，实际生活中不会存在如此简单的运动模型，真实位置也不可知，本程序中使用真值的目的是模拟观测噪声数据和测量噪声数据
31
# 对于实际应用的卡尔曼滤波而言，并不需要知道真实值，而是通过预测值和观测值，来求解最优估计值，从而不断逼近该真值
32
real_positions = np.array([0] * size) # 这样写的目的是为了创建长序列，用于绘制整个运动过程的图像
33
real_speeds = np.array([0] * size) # 后面要使用for循环，而for循环中需要用到i，而i是索引，所以需要创建一个长度为size的数组，用于存储每个时刻的位置和速度
34
real_positions[0] = s0 # 初始时刻的位置
35

36
# 实际观测值，通过理论值加上观测噪声模拟获得，初值即理论初始点加上观测噪声
37
measure_positions = np.array([0] * size)
38
measure_speeds = np.array([0] * size)
39
measure_positions[0] = real_positions[0] + np.random.normal(0, R[0][0]**0.5)
40
# 最优估计值，也就是卡尔曼滤波输出的真实值的近似逼近，同样地，初始值由观测值决定
41
optim_positions = np.array([0] * size)
42
optim_positions[0] = measure_positions[0]
43
optim_speeds = np.array([0] * size)
44

45
for i in range(1,t.shape[0]+1):
46

47
    # 根据理想模型获得当前的速度、位置真实值（实际应用中不需要），程序中只是为了模拟测试值和比较
48
    w = np.array([[np.random.normal(0, Q[0][0]**0.5)], [np.random.normal(0, Q[1][1]**0.5)]]) # 过程噪声协方差矩阵
49
    X_true = F @ X_true + B * u + w
50
    real_positions[i] = X_true[0]
51
    real_speeds[i] = X_true[1]
52

53

54
    v = np.array([[np.random.normal(0, R[0][0]**0.5)], [np.random.normal(0, R[1][1]**0.5)]]) # 观测噪声协方差矩阵
55
    # 观测矩阵用于产生真实的观测数据，注意各量之间的关联
56
    Z = H @ X_true + v
57
    # 以下是卡尔曼滤波的整个过程
58

59
    # 预测
60
    X_ = F @ X + B * u      # 预测方程
61
    P_ = F @ P @ F.T + Q    # 预测协方差矩阵
62

63
    # 更新
64
    # 注意矩阵运算的顺序
65
    K = P_@ H.T @ np.linalg.inv(H @ P_@ H.T + R) # 计算卡尔曼滤波增益系数
66
    X = X_ + K @ (Z - H @ X_)                    # 计算最优估计值
67
    P = (np.eye(2) - K @ H ) @ P_                # 更新最优估计协方差矩阵
68

69
    # 记录结果
70
    optim_positions[i] = X[0][0]
71
    optim_speeds[i] = X[1][0]
72
    measure_positions[i] = Z[0]
73
    measure_speeds[i] = Z[1]
74

75
t = np.concatenate((np.array([0]), t))
76
plt.plot(t,real_positions,label='real positions')
77
plt.plot(t,measure_positions,label='measured positions')
78
plt.plot(t,optim_positions,label='kalman filtered positions')
79

80
plt.legend()
81
plt.show()
82

83
plt.plot(t,real_speeds,label='real speeds')
84
plt.plot(t,measure_speeds,label='measured speeds')
85
plt.plot(t,optim_speeds,label='kalman filtered speeds')
86

87
plt.legend()
88
plt.show()

这段代码实现了一个一维匀加速运动中的卡尔曼滤波，用于估计物体的真实位置和速度。下面我们详细解释一下代码中每部分的含义，帮助你理解卡尔曼滤波的工作原理：

1. 问题背景#

模拟的是一个匀加速直线运动的物体，其真实的运动受过程噪声（比如发动机噪声、风阻等）影响。
同时，测量设备（如 GPS 和 IMU）也会引入观测噪声，导致测量的位置和速度不够准确。
我们希望使用卡尔曼滤波方法，通过结合运动模型（预测值）和观测值，逐步估计物体的真实位置和速度。

2. 状态定义#

状态包含两个变量：位置 $x$ 和速度 $v$ ：

$X = \begin{bmatrix} x \\ v \end{bmatrix}$

3. 系统模型#

状态转移模型 $X_{k+1} = F \cdot X_k + B \cdot u + w$ ：
- $F = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}$ ：描述如何从当前的状态 $X_k$ 预测下一时刻的状态。
  - 假设一个单位时间 $\Delta t = 1$ ，位置通过公式 $x_{k+1} = x_k + v_k \cdot \Delta t$ 预测。
  - 速度保持不变： $v_{k+1} = v_k$ 。
- $B = \begin{bmatrix} 1/2 \\ 1 \end{bmatrix}$ ：控制矩阵，用于描述加速度对系统状态的影响。
  - 加速度 $u$ 对位置的影响是 $\frac{1}{2} u \Delta t^2$ ，对速度是 $u \Delta t$ 。
- $w$ ：过程噪声，用于模拟运动模型中的不确定性。
观测模型 $Z_k = H \cdot X_k + v$ ：
- $H = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}$ ：观测矩阵，表示测量设备直接观测位置和速度。
- $v$ ：观测噪声，模拟测量设备的误差。

4. 噪声和协方差矩阵#

过程噪声协方差矩阵 $Q$ ：描述运动模型中的不确定性。
- $Q = \begin{bmatrix} 10 & 0 \\ 0 & 10 \end{bmatrix}$ ：这里假设位置和速度上的过程噪声是独立的。
观测噪声协方差矩阵 $R$ ：描述测量设备的不准确性。
- $R = \begin{bmatrix} 10000 & 0 \\ 0 & 10000 \end{bmatrix}$ ：这里假设位置和速度测量噪声也是独立的。

5. 卡尔曼滤波的步骤#

卡尔曼滤波由两步组成：预测和更新。

(1) 预测步骤#

状态预测：

$X_{k|k-1} = F \cdot X_{k-1|k-1} + B \cdot u$
- 利用状态转移方程，预测当前位置和速度。
协方差预测：

$P_{k|k-1} = F \cdot P_{k-1|k-1} \cdot F^T + Q$
- 协方差矩阵的预测，反映噪声和不确定性的传播。

(2) 更新步骤#

卡尔曼增益：

$K_k = P_{k|k-1} \cdot H^T \cdot (H \cdot P_{k|k-1} \cdot H^T + R)^{-1}$
- 卡尔曼增益决定了如何平衡预测值和观测值的权重。
状态更新：

$X_{k|k} = X_{k|k-1} + K_k \cdot (Z_k - H \cdot X_{k|k-1})$
- 利用测量值 $Z_k$ ，修正预测值。
协方差更新：

$P_{k|k} = (I - K_k \cdot H) \cdot P_{k|k-1}$
- 更新后的协方差矩阵，用于描述估计值的不确定性。

6. 代码的具体实现#

生成真实值和观测值：
- X_true：用运动模型生成真实位置和速度。
- Z：观测值通过添加观测噪声模拟。
卡尔曼滤波的预测和更新：
- X_ 和 P_：预测状态和协方差矩阵。
- K：计算卡尔曼增益。
- X 和 P：更新状态和协方差矩阵。
记录结果：
- 用 optim_positions 和 optim_speeds 存储卡尔曼滤波的估计值。

7. 图示结果#

位置图：
- 蓝色：真实位置（real_positions）。
- 橙色：观测位置（measure_positions，带噪声）。
- 绿色：卡尔曼滤波估计的最优位置（optim_positions）。
速度图：
- 蓝色：真实速度（real_speeds）。
- 橙色：观测速度（measure_speeds，带噪声）。
- 绿色：卡尔曼滤波估计的最优速度（optim_speeds）。

8. 关键理解#

噪声影响：
- 观测值受噪声影响波动较大。
- 卡尔曼滤波通过结合模型预测和观测值，显著减小噪声的影响。
滤波过程：
- 早期估计可能与真实值偏差较大，但随着时间推移，滤波器逐渐收敛。
卡尔曼增益的作用：
- 决定了如何在观测值和预测值之间取权重，观测噪声越大，越依赖模型预测。

二、补充说明#

为什么 H 是 2×2 的矩阵#

在卡尔曼滤波中，观测矩阵 $H$ 的维度是由状态向量 $X$ 和观测向量 $Z$ 决定的，其大小为 $m \times n$ ，其中：

$n$ 是状态向量的维度（这里是 2，因为状态 $X$ 包括 [位置, 速度]）。
$m$ 是观测向量的维度（这里是 2，因为观测值 $Z$ 包括 [位置, 速度]）。

代码中定义的状态向量和观测向量为：

状态向量 $X$ ：

$X = \begin{bmatrix} x \\ v \end{bmatrix}$

包含两个变量：位置 $x$ 和速度 $v$ 。
观测向量 $Z$ ：

$Z = \begin{bmatrix} z_x \\ z_v \end{bmatrix}$

包含两个变量：测量的”位置” $z_x$ 和测量的”速度” $z_v$ 。

因此，观测矩阵 $H$ 是一个 $2 \times 2$ 的矩阵，用于从状态 $X$ 映射到观测 $Z$ 。形式为：

$Z = H \cdot X + v$

为什么 H 是 2×2 的单位矩阵？#

在这个例子中，观测矩阵 $H$ 被定义为：

$H = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}$

这表示观测值直接测量了状态中的位置和速度，且没有任何变换。具体来说：

第一行 $[1, 0]$ ：
- 表示观测值的第一个分量 $z_x$ 对应状态中的位置 $x$ ，并且它与速度 $v$ 无关。
$z_x = 1 \cdot x + 0 \cdot v$
第二行 $[0, 1]$ ：
- 表示观测值的第二个分量 $z_v$ 对应状态中的速度 $v$ ，并且它与位置 $x$ 无关。
$z_v = 0 \cdot x + 1 \cdot v$

这就说明测量设备可以分别直接得到位置和速度，且观测值和状态是一一对应的。

如果 H 不是单位矩阵怎么办？#

在实际应用中，观测值可能与状态变量不是一一对应的，比如：

观测值只有位置（没有速度）：

$H = \begin{bmatrix} 1 & 0 \end{bmatrix}$

这里，观测值只包含位置 $x$ ，不包含速度 $v$ 。
观测值是状态变量的某种线性组合：

$H = \begin{bmatrix} 0.5 & 0.5 \\ 0 & 1 \end{bmatrix}$

这里，观测值的第一个分量是 $0.5 \cdot x + 0.5 \cdot v$ ，第二个分量是速度 $v$ 。
非线性观测：如果观测值和状态的关系是非线性的，可能需要使用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）来处理。

为什么 Q=10 和 R=10000#

在卡尔曼滤波中，过程噪声协方差矩阵 $Q$ 和 观测噪声协方差矩阵 $R$ 是滤波器的重要参数，它们用来描述系统模型和测量设备的不确定性。

$Q = \begin{bmatrix} 10 & 0 \\ 0 & 10 \end{bmatrix}, \quad R = \begin{bmatrix} 10000 & 0 \\ 0 & 10000 \end{bmatrix}$

是人为设定的超参数，代表对过程噪声和观测噪声强度的假设。

Q 为什么是 10#

含义： $Q$ 是过程噪声协方差矩阵，用于描述系统动态模型中的不确定性。它反映了模型对自身预测的信任程度。
设置为 10 表示每秒钟，位置和速度的预测值可能会受到方差为 10 的噪声影响。假设运动模型存在较小的不确定性（与观测噪声相比）。
如果 $Q$ 设得太小（接近 0），滤波器会过于相信模型而忽视观测值；如果设得太大，滤波器会对模型预测缺乏信任，可能过度依赖观测值。

R 为什么是 10000#

含义： $R$ 是观测噪声协方差矩阵，用于描述测量设备的不准确性。
设置为 10000 表示测量设备（比如 GPS 或 IMU）在每次测量时的噪声方差为 10000，意味着测量设备相对不精确，观测数据波动较大。
通常，测量设备引入的噪声比运动模型的内在不确定性大得多。将观测噪声方差设为远大于过程噪声方差，表示测量数据不太可信，卡尔曼滤波器会更多地依赖运动模型预测。

两者的相对大小#

如果 $Q \ll R$ （过程噪声远小于观测噪声）：滤波器更相信运动模型的预测值。
如果 $Q \gg R$ （过程噪声远大于观测噪声）：滤波器更依赖观测值，而弱化对模型的信任。

状态更新公式 `X_true = F @ X_true + B * u + w` 的结果#

公式 $X_{\text{true}} = F \cdot X_{\text{true}} + B \cdot u + w$ 是用来模拟系统的真实状态更新的。

符号解释#

$X_{\text{true}} = \begin{bmatrix} x \\ v \end{bmatrix}$ ：系统的真实状态向量，包括位置 $x$ 和速度 $v$ 。
$F = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}$ ：状态转移矩阵。
$B = \begin{bmatrix} 1/2 \\ 1 \end{bmatrix}$ ：控制矩阵。
$u$ ：加速度（设定为 0.6）。
$w = \begin{bmatrix} w_x \\ w_v \end{bmatrix}$ ：过程噪声，其中 $w_x \sim \mathcal{N}(0, Q[0,0])$ ， $w_v \sim \mathcal{N}(0, Q[1,1])$ 。

更新过程#

逐项展开公式：

位置更新：

$x_{\text{true,new}} = x_{\text{true}} + v_{\text{true}} + \frac{1}{2} u + w_x$
速度更新：

$v_{\text{true,new}} = v_{\text{true}} + u + w_v$

示例计算#

假设当前 $X_{\text{true}} = \begin{bmatrix} 0 \\ 5 \end{bmatrix}$ ，加速度 $u = 0.6$ ，噪声 $w_x = 2.0$ ， $w_v = -1.0$ ：

位置更新： $x_{\text{true,new}} = 0 + 5 + \frac{1}{2} \cdot 0.6 + 2.0 = 7.3$
速度更新： $v_{\text{true,new}} = 5 + 0.6 - 1.0 = 4.6$

最终结果： $X_{\text{true,new}} = \begin{bmatrix} 7.3 \\ 4.6 \end{bmatrix}$