1. 状态转移矩阵

从当前状态到下一个状态的转换.

例子: 假设跟踪车辆形式

  • 车辆的状态矩阵, 位置变量 $p_t$; 速度变量 $v_t$.

    $$ x_t=\begin{bmatrix} p_t \\ v_t \end{bmatrix} $$

  • 假设车辆有加速度变量 $u_t$; 这里 $\Delta t$ 是单位时间; 这里就可以从上一个单位时间的 $p_{t-1}v_{t-1}$ 推导出当前的 $p_tv_t$.

    $$ \begin{aligned}p_t=&p_{t-1}+v_{t-1}\times\Delta t+u_t\times\frac{\Delta t^2}{2}\\v_t=&v_{t-1}+u_t\times\Delta t\end{aligned} $$

  • 这里可以发现输出变量是输入变量的线性组合, 所以卡尔曼滤波器是线性滤波器, 只能描述状态与状态之间的线性关系. 由于是线性关系, 所以可以将公式写成矩阵的形式.

    $$ \left[\begin{array}{c}p_t \\v_t\end{array}\right]=\left[\begin{array}{cc}1 & \Delta t \\0 & 1\end{array}\right]\left[\begin{array}{c}p_{t-1} \\v_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{\Delta t^2}{2} \\\Delta t\end{array}\right] u_t $$

  • 提取公式中带时间参数矩阵之后, 就可以简化公式获得卡尔曼滤波器中的状态预测公式.

    $$ \begin{aligned}&F_t=\left[\begin{array}{cc}1 & \Delta t \\0 & 1\end{array}\right], B_t=\left[\begin{array}{c}\frac{\Delta t^2}{2} \\\Delta t\end{array}\right]\\&\hat{x}_t^{-}=F_t \hat{x}_{t-1}+B_t u_t\end{aligned} \quad\quad \text{(1)} $$

  • 这里的 $\hat{x}_t^{-}$ 表示, 这个值有 hat 符号是预测值, 并不是真实值; 右上角减号表示这个值是从 t-1 时刻的参数估计而来, 还需要 t 时刻的观测值进行修正.

2. 协方差矩阵

协方差描述的是变量之间的相关程度.

  • 首先, 可以使用中心值核方差来描述一个变量的分布情况.

    Untitled

  • 而对于存在两个变量的分布情况, 需要用到二维坐标系. 可以在两个轴分别描述两个变量的中心值和方差. 如果两个变量没有明显线性关系时, 分布情况图为左一; 两变量分布正相关时, 图为中间; 分布为负相关时, 图为右一. 可以发现这三种情况用中间值和方差是无法体现的.

    Untitled

  • 为了描述两变量之间的相关程度, 使用了协方差. 协方差矩阵的公式如下

    $$ \operatorname{cov}(x, x)=\left[\begin{array}{ll}\sigma_{11} & \sigma_{12} \\\sigma_{12} & \sigma_{22}\end{array}\right] $$
    • 其中, $\sigma_{12}$ 表示的就是两变量的协方差.

  • 还是拿车辆速度举例, 由 $P^-_t$ 来表示不确定性在各个时间段内的传递关系, 是当前时刻先验估计协方差.

    $$ P^-_t=F_tP_{t-1}F_t^T+Q_t \quad\quad \text{(2)} $$
    • $F_t$ 表示的是状态转换矩阵里带时间变量的 $F_t$ 矩阵. 之所以两边都乘, 是因为协方差矩阵的性质. 协方差的更新方法, 如果要将每一个点都乘以矩阵 $A$, 公式如下:
    $$ cov(x)=\sum \\cov(Ax)=A\sum A^T $$
    • $Q$ 是状态转移协方差, 用来描述状态转换矩阵和实际过程之间的误差. 这个值是可以设置的参数, 值越大, 表示我们对预测值的信任值越高. 因为这样会增大 $P^-_t$.

3. 观测矩阵

描述系统实际状态, 与观测到的数值之间的关系.

  • 例1, 地球上的汽车位置为 [经度, 纬度, 海拔], 但是地图导航软件常常默认海拔为 0, 也就是不需要观测数据中的海拔信息.

    那么地图需要的观测数据 $Z_t$ 如下:

    $$ Z_t=\begin{vmatrix} 经度 & 纬度 & 海拔 \\ 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0 & 0 \end{vmatrix}.\begin{vmatrix}经度值\\纬度值\\海拔值 \end{vmatrix} $$

  • 例2, 还是车辆速度问题, 如果有个测距仪在每个时间段进行测距, 获得标量 $Z_t$.

    Untitled

    这时候, 观测矩阵为 $H=\begin{vmatrix}1&0\end{vmatrix}$, 因为车辆的状态矩阵中有用的只有位置变量 $p_t$.

    获得 $Z_t$ 公式如下:

    $$ Z_t=Hx_t+c $$

    • $c$ 描述观测值和预测值的残差, 因为观测值也不一定准确. 这时候, $Z_t$ (观测值)和 $H\hat{x}_t^-$ (预测值) 都是已知的, 所以可以算出:

      $$ c = Z_t-H\hat{x}_t^- $$

    • 测量噪声的协方差为 $R$, 可以手动设定的参数.

4. 状态更新

  1. 计算出卡尔曼系数 $K_t$, 也叫滤波增益系数, 它的作用有两个

    1. 权衡预测状态协方差 $P^-_t$ 和 观察值的残差协方差 $R$ 的大小, 来决定观测值的权重. (相信预测模型还是观测值, 如果相信预测模型多一点, 那么 $K_t$ 就会小一点; 反之则 $K_t$ 就会大一点)

      $$ K_t=\frac{P_t^{-} H^T}{H P_t^{-} H^T+R}\quad\quad \text{(3)} $$

    2. 将残差的表现形式从观测域转换为状态域. 因为, 观察值和状态值的维度可能就是不同的, 所以需要转换. 举车辆速度的例子来说, 观测值只有车辆的位置没有速度, 但是车辆状态有速度和位置. 这时候, $K_t$ 中包含了协方差矩阵 $P^-_t$ 的信息, 所以可以利用位置和速度的相关性, 从位置的残差 $c$ 推算出速度的残差.

  2. 计算当前状态, 由 状态预测公式 $\hat{x}_t^{-}$+ 卡尔曼系数 $K_t$ * 观测值和预测值的残差 $c$ 获得.

    $$ \hat{x}_t=\hat{x}_t^{-}+K_t\left(z_t-H \hat{x}_t^{-}\right) \quad\quad \text{(4)} $$

  3. 更新最佳估计值的噪声协方差矩阵 $P_t$, 在下一时刻作为 $P_{t-1}$ 用于 $P^-_t$ 的计算.

    $$ P_t=(I-K_tH)P^-_t \quad\quad \text{(5)} $$

5. 实际使用

  • 需要调试的参数

    • $\Delta t$: 其实修改的是转态转移矩阵, 不同的运动状态可以使用不同的矩阵

      Untitled

    • $R$: 测量噪声的协方差, $R$ 取值越小收敛越快, $R$ 取值越大收敛越慢;

    • $Q$: 状态转移协方差矩阵, 值越大, 表示我们对预测值的信任值越高; 当状态转换过程为已确定时, $Q$ 的取值越小越好; 转换状态不确定的时候, $Q$ 可以去随机值, 随时间变化, 这时滤波器变为自适应卡尔曼滤波器.

  • 使用测距仪获得单位时间车辆距离测距仪的距离, 预测车辆速度.

    import numpy as np
import matplotlib.pyplot as plt

N = 100
gt_data = np.arange(0, N)
noise = np.random.randn(1, N)  # 制造一些观测误差
noise = noise * 20
gt_data_n = (noise + gt_data).reshape(N)
gt_data_n_list = (noise + gt_data).tolist()[0]

delta_t = 1
X = np.zeros((2, 1))  # 状态矩阵
P = np.array([[1, 0], [0, 1]])  # 状态协方差矩阵
F = np.array([[1, delta_t], [0, 1]])  # 状态转移矩阵, 初始单位时间为1
Q = np.array([[0.0001, 0], [0, 0.0001]])  # 状态转移协方差矩阵
H = np.array([[1, 0]])  # 观测矩阵 <- 注意这里数组必须套在数组里面, 不然无法转置
R = 10  # 观测噪声方差矩阵 <- 由于距离的噪声是一维的所以这里不用矩阵了
B = np.array([[0.5 * np.sqrt(delta_t)], [delta_t]])  # 控制量
U = 0

distances = []
speeds = []

for i in range(N):
    X_ = F @ X + B * U  # 状态预测公式
    P_ = F @ P @ F.T + Q  # 预测状态协方差公式
    K = (P_ @ H.T) @ np.linalg.inv(H @ P @ H.T + R)  # 更新卡尔曼系数
    X = X_ + K @ (gt_data_n_list[i] - H @ X_)  # 更新状态预测公式
    P = (np.eye(2) - K @ H) @ P_  # 更新状态协方差矩阵

    distances.append(float(X[0]))
    speeds.append(float(X[1]))

# 画图
plt.scatter(distances, speeds)
plt.xlabel('distances')
plt.ylabel('speeds')
plt.axhline(1)  # 画一条辅助线方便观察
plt.show()

    Untitled

  • 对于有车有固定加速度的情况

    import numpy as np
import matplotlib.pyplot as plt

N = 100
gt_data = np.arange(0, N)
gt_data = np.square(gt_data)
noise = np.random.randn(1, N)  # 制造一些观测误差
noise = noise * 20
gt_data_n = (noise + gt_data).reshape(N)
gt_data_n_list = (noise + gt_data).tolist()[0]

delta_t = 1.42
X = np.zeros((2, 1))  # 状态矩阵
P = np.array([[1, 0], [0, 1]])  # 状态协方差矩阵
F = np.array([[1, delta_t], [0, 1]])  # 状态转移矩阵, 初始单位时间为1
Q = np.array([[0.0001, 0], [0, 0.0001]])  # 状态转移协方差矩阵
H = np.array([[1, 0]])  # 观测矩阵 <- 注意这里数组必须套在数组里面, 不然无法转置
R = 1  # 观测噪声方差矩阵 <- 由于距离的噪声是一维的所以这里不用矩阵了
B = np.array([[0.5 * np.sqrt(delta_t)], [delta_t]])  # 控制量
U = 1

distances = []
speeds = []

for i in range(N):
    X_ = F @ X + B * U  # 状态预测公式
    P_ = F @ P @ F.T + Q  # 预测状态协方差公式
    K = (P_ @ H.T) @ np.linalg.inv(H @ P @ H.T + R)  # 更新卡尔曼系数
    X = X_ + K @ (gt_data_n_list[i] - H @ X_)  # 更新状态预测公式
    P = (np.eye(2) - K @ H) @ P_  # 更新状态协方差矩阵

    distances.append(float(X[0]))
    speeds.append(float(X[1]))

# 画图
# 画预测值
plt.plot(distances, speeds, "x", label="prec")
# 画真实值 y=(2x)^(1/2)
true_y = np.sqrt(2 * gt_data)
plt.plot(gt_data, true_y, label="gt")
# 画观测值
true_y_n = np.sqrt(2 * gt_data_n)
plt.plot(
    gt_data_n,
    true_y_n,
    "o",
    label="obs",
)
plt.xlabel('distances')
plt.ylabel('speeds')
plt.legend()
plt.show()
print()

    Untitled

  • 如果车的加速度是非线性变化 (前50s, a=1; 后50s, a=3) 的情况, 无法准确预测

    import numpy as np
import matplotlib.pyplot as plt

N = 100
a_1 = 1
a_2 = 3

def cul_true_data(time1, time2):
    speed1 = a_1 * time1
    speed2 = (a_1 * time1)[-1] + a_2 * time2
    return np.append(speed1, speed2)

gt_time = np.arange(0, N // 2)  # 单位时间 0-50
gt_distance = 0.5 * np.square(a_1 * gt_time)  # 加速度 a1
last_speed = (a_1 * gt_time)[-1]
# gt_time_2 = np.arange(N // 2, N)  # 单位时间 50-100
gt_distance_2 = gt_distance[-1] + last_speed * gt_time + 0.5 * np.square(
    a_2 * gt_time)  # a2
gt_data_merge = np.append(gt_distance, gt_distance_2)
noise = np.random.randn(1, N)  # 制造一些观测误差
noise = noise * 5
gt_data_n = (noise + gt_data_merge).reshape(N)
gt_data_n_list = (noise + gt_data_merge).tolist()[0]

delta_t = 1.5
X = np.zeros((2, 1))  # 状态矩阵
P = np.array([[1, 0], [0, 1]])  # 状态协方差矩阵
F = np.array([[1, delta_t], [0, 1]])  # 状态转移矩阵, 初始单位时间为1
Q = np.array([[0.0001, 0], [0, 0.0001]])  # 状态转移协方差矩阵
H = np.array([[1, 0]])  # 观测矩阵 <- 注意这里数组必须套在数组里面, 不然无法转置
R = 1  # 观测噪声方差矩阵 <- 由于距离的噪声是一维的所以这里不用矩阵了
B = np.array([[0.5 * np.sqrt(delta_t)], [delta_t]])  # 控制量
U = 2

distances = []
speeds = []

for i in range(N):
    X_ = F @ X + B * U  # 状态预测公式
    P_ = F @ P @ F.T + Q  # 预测状态协方差公式
    K = (P_ @ H.T) @ np.linalg.inv(H @ P @ H.T + R)  # 更新卡尔曼系数
    X = X_ + K @ (gt_data_n_list[i] - H @ X_)  # 更新状态预测公式
    P = (np.eye(2) - K @ H) @ P_  # 更新状态协方差矩阵

    distances.append(float(X[0]))
    speeds.append(float(X[1]))

# 画图
# 画预测值
plt.plot(distances, speeds, "x", label="prec")
# 画真实值
true_y = cul_true_data(gt_time, gt_time)
plt.plot(gt_data_merge, true_y, label="gt")
plt.xlabel('distances')
plt.ylabel('speeds')
plt.legend()
plt.show()
print()

    Untitled

  • 更复杂的应用可以看这里

    卡尔曼滤波应用及其matlab实现-腾讯云开发者社区-腾讯云

6. 总结

优点：

  1. 卡尔曼滤波器是一种最优的滤波器，具有最小均方误差，能够有效地消除噪声和抖动。
  2. 卡尔曼滤波器不仅可以估计状态，还可以估计状态的协方差，对于系统的不确定性有一定的鲁棒性。
  3. 卡尔曼滤波器可以实时地进行状态估计，响应速度较快。

缺点：

  1. 卡尔曼滤波器的理论基础是线性高斯系统，对于非线性、非高斯的系统效果可能不佳。
  2. 卡尔曼滤波器需要准确的先验信息，如果先验信息不准确，则滤波结果也会不准确。
  3. 卡尔曼滤波器计算量较大，对计算能力有一定要求。

参考资料

1. 1. 1. 卡尔曼滤波器的原理以及在matlab中的实现_哔哩哔哩_bilibili

深入理解卡尔曼滤波算法_卡尔曼算法-CSDN博客

卡尔曼滤波器的优缺点有哪些？ - 知乎

  • 调参

卡尔曼滤波中关键参数的调整