• 问答
  • 技术
  • 实践
  • 资源
【夯实基础】卡尔曼滤波
技术讨论

【夯实基础】卡尔曼滤波

来源 | 宅码
编辑 | 极市平台

前段时间,我参加Kaggle谷歌手机定位赛,第一次了解到卡尔曼滤波。本文基于知乎问答:如何通俗并尽可能详细地解释卡尔曼滤波?[1]和MathWorks的系列视频《Understanding Kalman Filters》[2],整理了卡尔曼滤波的内容。不足之处,还望批评指正。

一、背景

卡尔曼滤波(Kalman Filter,KF)是一种优化估算算法(Optimal Estimation Algorithm),常用于制导与导航控制系统、计算机视觉系统和信号处理领域。实际作用主要是:从受误差影响的传感器测量中估算出最佳的系统状态

在正式了解KF之前,我们先了解下状态观察器。

二、状态观察器

状态观察器(State Observers):用于优化估算一些无法直接测量但可以间接测量的状态。

1.场景:观察火箭喷射器的内部温度

现在有一只火箭要从地球飞往火星,火箭使用液态氢作为燃料,但过高温度会导致火箭喷射器的机械部件损坏,因此监控喷射器内部温度就十分重要,可传感器不支持放置在喷射器内部,否则会被融化掉。但我们可以在喷射器外部的放置温度传感器,间接测量内部温度,但间接测量值和真实值存在误差,为此,我们可以使用状态观察器去基于间接测量值估算出真实值。

图1:估算火箭喷射器的内部温度

2.应用:状态观察器

状态观察器怎么“观察”出火箭喷射器的内部温度状态呢?

图2:温度测量和估算示意图

我们可以通过如图3的控制器K,控制测量和估计的外部温度之间的误差为0。

图3:带控制器K的状态观察器

现在,我们透过数据公式来解释下状态观察器,如图4。为了方便理解公式,我先解释下变量:

  • $u$ :控制向量。这里是燃料流量,它控制了温度。
  • $B:$ 控制矩阵。
  • $A$ 和 $C:$ 矩阵 $\mathrm{A}$ 和 $\mathrm{C}$ 。
  • $\dot{x}:$ 内部温度对时间的导数。
  • $x$ 和 $y:$ :测量的内部温度和外部温度。
  • 加了\wedge的变量都是预测的变量。

图4:状态观察器\(附数据公式\)

三、最佳状态估计器

最佳状态估计器(Optimal State Estimator):从受误差影响的传感器测量中估算出最佳的系统状态。

卡尔曼滤波本身便是一个最佳状态估计器,它跟前面讲的状态观察器差不多,不同的是KF是为随机系统设计的。区别如下:

图5:状态观测器和最佳状态估计器\(KF\)的预测值公式对比

1.场景:估算隧道内汽车的位置

如果汽车进入隧道,汽车上的GPS接收器很难接收到卫星信号。如果在周围有高建筑物遮挡的街道上行驶,因为多路径效应(Multipath Effect,即信号失真),导致定位噪声很大。我们可以简单看作这是一个传感器测量受误差影响的案例,如果我们想估算出汽车真实的位置状态,建议使用卡尔曼滤波。

图6:估算隧道内汽车的位置

2.应用:最佳状态估计器

接下来,我们来看下最佳状态估计器是如何估算汽车下一时间点会定位在哪的。

如图7所示,KF的目标是将GPS测量值(Measurement)和数学模型预测值(Prediction)相结合去找到汽车位置的最优估计状态(即汽车位置)

图7:位置测量和估算的示意图

同样地,我先解释下图7中的变量:

  • $u_{k}:$ 时刻 $\mathrm{k}$ 下的控制向量。这里是速度。

  • $x$ 和 $y$:测量的位置和真正的位置。

  • $w:$ 过程噪声,随机变量,可以代表风的影响或汽车速度的变化。服从正态分布 $w \sim N(0,Q),\quad Q$ 为协方差。

  • $v:$ 测量误差,随机变量,服从正态分布 $v \sim N(0,R),R$ 为测量值的协方差,由于我们这里是单输出系统,协方差R等同于测量误差的方差。

  • $A 、 B$ 和 $C:$ 矩阵变量。

  • 加了\wedge的变量都是预测的变量。

在图7中,我们用了概率密度函数去描述KF的中心思想,假设我们用数据模型得到时间点 $k-1$ 下汽车状态 $\hat{x}{k-1}$,由于是预估值,所以会存在一定的方差,现在我们使用汽车数学模型再次预测时间 $\mathrm{k}$ 下的位置 $\hat{x}{k}$,由 于误差累积,$\hat{x}{k}$ 会有更大方差,同时我们也可以获取到测量值 $\hat{y}{k}$ 。现在我们有了预测和测量,我们便可以通 过概率密度函数相乘的方式得到最优状态估计函数,而新的概率密度函数的平均值便是最优状态估计值(即我们 综合测量值和预测值得到的最佳定位点)。

四、最佳状态估计器算法

现在,我们来详细看下KF是如何实现最佳状态估计器算法。如图8所示,我先解释下各变量:

  • $\hat{x}^{-}$:先验状态,即预测值。
  • $\hat{x}:$ 后验状态,即最佳状态估计值。
  • $P^{-}:$先验协方差,即预测值的协方差。
  • $P:$ 后验协方差,即最佳状态估计值的协方差。
  • $R:$ 测量值噪声的协方差。
  • $Q:$ 过程噪声的协方差。
  • $K$ :卡尔曼增益。
  • $A 、 B$ 和 $C:$ 矩阵。

1.当K=1/C时,测量值=最佳状态估计值

若测量值=最佳状态估计值,说明测量值协方差R趋向于0,那么如图9所示。
图9:当K=1/C时,测量值=最佳状态估计值

2.当K=0时,预测值=最佳状态估计值

若预测值=最佳状态估计值,说明预测值协方差趋向于0,那么如图10所示。

图10:当K=0时,预测值=最佳状态估计值

五、样例代码

simdkalman库[3]支持卡尔曼滤波,以下是参考[4],给出的样例代码:

import numpy.random as random
import simdkalman
import numpy as np
import matplotlib.pyplot as plt

def make_shifted_matrix(vec):
    '''Define shifted matrix'''
    matrix = []
    size = len(vec)
    for i in range(size):
        row = [0] * i + vec[:size-i]
        matrix.append(row)
    return np.array(matrix)

def make_state_vector(T, size):
    '''Define state vector'''
    vector = [1, 0]
    step = 2
    for i in range(size - 2):
        if i % 2 == 0:
            vector.append(T)
            T *= T / step
            step += 1
        else:
            vector.append(0)
    return vector

def make_noise_vector(noise, size):
    '''Define noise vector'''
    noise_vector = []
    for i in range(size):
        if i > 0 and i % 2 == 0:
            noise *= 0.5
        noise_vector.append(noise)
    return noise_vector

# KF的参数,可用optuna去做搜索
T = 1
size = 4
noise = 2.1714133956113952e-06
obs_noise = 1.719317114286542e-05

# 获取State transition matrix A
vec = make_state_vector(T, size)
state_transition = make_shifted_matrix(vec)

# 获取Process noise (state transition covariance) matrix Q
process_noise = np.diag(make_noise_vector(noise, size)) + np.ones(size) * 1e-9

# 获取Observation model (measurement model) matrix C
observation_model = np.array([[1] + [0] * (size - 1)])

# 获取Observation noise (measurement noise covariance) matrix R
observation_noise = np.diag([obs_noise] * 1) + np.ones(1) * 1e-9

kf = simdkalman.KalmanFilter(
        state_transition = state_transition,
        process_noise = process_noise,
        observation_model = observation_model,
        observation_noise = observation_noise)

# 随机化一组样例数据
x = np.linspace(0, 10, 100)
y = np.sin(x)
data = np.array([v + random.normal(0.1, 0.3, 1) for v in y])
data = data.reshape(1, len(data), 1)

smoothed = kf.smooth(data)
smoothed_data = smoothed.states.mean[0, :, 0]

# 可视化
fig = plt.figure()
plt.scatter(range(len(data[0,:,0])), data[0,:,0], c = 'black', label = 'samples')
plt.plot(range(len(smoothed_data)), smoothed_data, c = 'r', label = 'samples after KF')
plt.legend()
plt.show()

图11:KF样例代码输出结果

六、扩展资料

卡尔曼滤波是线性的,而实际中,很多数据模型是非线性的,此时可以考虑扩展卡尔曼滤波,另外,有兴趣的朋友也可以去了解无迹卡尔曼滤波和粒子滤波,如图12所示。

图12:状态估计器之间的对比

个人在查阅资料的时候,发现还有很多蛮深入的资料,适合深入了解卡尔曼滤波的数据原理,这里分享给有需要的朋友们:

1.MathWorks制作的《Understanding Kalman Filters》视频

2.Bzarg写的《How a Kalman filter works, in pictures》博文

3. Automatic Addison写的《Extended Kalman Filter (EKF) With Python Code Example》博文

参考资料

[1]《如何通俗并尽可能详细地解释卡尔曼滤波?》-知乎,问答:https://www.zhihu.com/question/23971601

[2]《Understanding Kalman Filters》- MathWorks,系列视频:https://ww2.mathworks.cn/videos/series/understanding-kalman-filters.html

[3]卡尔曼滤波库simdkalman,Github:https://github.com/oseiskar/simdkalman

[4]《Kalman filter - hyperparameter search with BO》-Trinh Quoc Anh,代码:https://www.kaggle.com/tqa236/kalman-filter-hyperparameter-search-with-bo

Ai现居 | 广东深圳现岗位 | 数据分析工程师兴趣 | 数据挖掘、机器学习、深度学习

  • 0
  • 0
  • 1960
收藏
暂无评论