卡尔曼滤波学习笔记

里先森

已于 2022-03-15 11:17:33 修改

阅读量870

点赞数 1

分类专栏：啊~学习文章标签：卡尔曼滤波算法

于 2021-07-10 18:58:05 首次发布

本文链接：https://blog.csdn.net/sements/article/details/118639107

版权

啊~学习专栏收录该内容

5 篇文章 0 订阅

订阅专栏

对了，我最近开通了微信公众号，计划是两边会同步更新，并逐步的会将博客上的文章同步至公众号中。
感兴趣的朋友可以扫描下方的二维码或者搜索“里先森sements”来关注，欢迎来玩~！
在这里插入图片描述

本文是在学习卡尔曼滤波过程中所作的个人笔记，由于个人数学功底较差，文中的一些观点与理解仅供读者参考，如有疏漏还请诸位不吝赐教。文中的部分参考文献来源如下：

《Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation》
《GNSS与惯性及多传感器组合导航系统原理》
无人驾驶汽车系统入门（一）——卡尔曼滤波与目标追踪 https://blog.csdn.net/AdamShan/article/details/78248421
卡尔曼滤波 - 维基百科，自由的百科全书: https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2
协方差矩阵 - 维基百科，自由的百科全书: https://zh.wikipedia.org/wiki/%E5%8D%8F%E6%96%B9%E5%B7%AE%E7%9F%A9%E9%98%B5
卡尔曼滤波中的噪声协方差矩阵（R和Q）应该怎么取值,和噪声分布之间的关系是什么？ - 知乎 “茶渣”的回答
https://www.zhihu.com/question/53788909?sort=created
卡尔曼滤波五个公式推导 geerniya的博客-CSDN博客
https://blog.csdn.net/geerniya/article/details/104804916

卡尔曼滤波的过程，实质上是使用“预测模型”的推导值与测量值的概率密度函数（pdf,Probability Distribution Function）相乘，得到滤波结果值的概率密度函数。当然，理论上来说，不使用预测模型，仅使用两个或者多个传感器测量值也可以进行卡尔曼滤波。以推断小车的位置为例，我们知道它 $x_k$ 时刻的状态（位置、速度等），通过预测模型可以推导得到 $x_{k+1}$ 时刻的状态。通过传感器，我们可以测量得到一个距离值 $z_k$ 。但误差（噪声）无处不在，这导致我们的推导值和测量值都与实际值有一定的偏差，使得小车位置的推导与测量值都为一个概率范围。而卡尔曼滤波的想法，就是利用两种方式得到的概率范围进行推断，得到一个更贴近于实际值的概率范围。

在这里插入图片描述

概率密度相乘得到新的概率密度 ^[1]

为便于查阅，这里先将通常意义的卡尔曼滤波计算过程，也就是大家耳熟能详的几个公式贴出，图片与后面的文字叙述结合起来可能更便于理解。

在这里插入图片描述

卡尔曼滤波计算过程 ^[3]

1.1 - 噪声与误差

如果没有噪声与误差，那么无论是通过预测模型得到的预测值，还是通过传感器得到的测量值，两者的结果都应该与实际值一致，即可以100%的得到小车当前的位置。可惜事与愿违，误差与噪声的存在导致我们对于小车的位置“猜”不准也“测”不准，使得预测与测量得到的结果呈现一个概率分布。打个比方，小车有43.75%的可能在离起始点1.56m的位置，也有12.28%的可能在离起始点1.35m的位置。预测模型的误差通常来源于建立的理论模型与实际模型之间的偏差，这部分的误差通常是在所难免的，毕竟你不可能将所有的影响因素具象化的考虑周全。传感器的测量误差则相对来说更加单纯，它通常与你的预算有关，越昂贵的传感器通常精度越高，越能提供精确的测量值。

尽管实际的误差可能较难使用一个规范的数学方法（例如白噪声、高斯-马尔可夫过程）或者他们的组合，进行表示，但是我们可以让模型化的误差充分的包含实际的误差。换句话来说，无论实际的误差多么难以量化，只要它的范围不超过我们模型化的误差范围即可^[2]。在对误差进行建模时，我们通常将其作为高斯噪声进行考虑，假定其均值为0，服从正态分布，并使用一个协方差矩阵^[2,4,5]对其进行表示。

在卡尔曼滤波中，误差与噪声被分为了三个部分来进行考虑：通过使用建立的预测模型得到的预测值（估计值）与实际值（真值）之间的误差，预测误差；传感器或者其他方式进行测量时引发误差的噪声，测量噪声；由于一些不可测的运动过程或者未知因素引发误差的噪声，过程噪声（或者叫系统噪声）。

预测误差的协方差矩阵 $P$ （Error covariance matrix）是一个对称矩阵，矩阵对角线上的元素是对各个状态（例如位置的x、y坐标以及x与y方向上的速度）与实际值之间偏差的方差的估计，非对角线上的元素则是不同状态的预测误差之间的相关性。当可以独立的估计状态时，非对角线上的元素则为0，而当我们建模的信息较少，不能独立的估计我们需要的各个状态时，这些相关性则不可忽视，这个问题也被称为可观测性^[2]。

测量噪声的协方差矩阵 $R$ （Measurement noise covariance matrix）代表着测量误差的平方的期望值，是一个对称矩阵。矩阵对角线上的元素代表了每个观测量的误差的方差，非对角线上的元素则代表了不同观测量的误差之间的相关性。在大部分应用中，观测量之间是相互独立的，因此测量误差 $R$ 的协方差矩阵通常为一个对角矩阵^[2]。

过程噪声的协方差矩阵 $Q$ 也被称为系统噪声协方差矩阵（System noise covariance matrix），其同样为对称矩阵^[2]。可以将其理解为，在上一状态与下一状态的变化期间，一些随机的噪声被引入所导致的。例如在推测轨道上匀速运动的小车位置时，车上发动机原本恒定输出的力矩因为进气或者燃油泵喷嘴堵塞而造成的一些变化，将导致我们的小车产生短促的加减速，进而引入过程噪声。

值得注意的是，这里描述误差与噪声所使用的字母是对应前面贴出的卡尔曼滤波计算过程图的。在别的一些文献或者介绍中，可能使用不同的字母来进行表示。概要的来说，预测误差的协方差矩阵 $P$ 反映了你对于通过你的预测模型得到的值有多不确定，测量噪声的协方差矩阵 $R$ 反映的是测量中使用的传感器的误差，过程噪声的协方差矩阵 $Q$ 则涵盖的东西较多，例如使用计算机进行计算时的舍入误差、模型线性化程度等等 $^{[6]}$ 。这三个误差在进行卡尔曼滤波时都需要进行初始化，正如前文所述，虽然我们没有办法得知误差的确切范围，但只要实际的误差范围在我们估计的误差范围之内即可。

如果P $_0$ 、Q、R无法精确获得，只知道可能的取值范围，则采用可能的较大值（保守）。如果不确切知道Q、R、P $_0$ 的准确先验信息，应适当增大Q的取值，以增大对实时量测值的利用权重，俗称调谐。但是调谐存在盲目性，无法知道Q要调到多大才行。

——《卡尔曼滤波与组合导航原理》秦永元^{[6] （？待查证）}

1.2 - 师傅别念了来点例子吧

下面的例子来源于 AdamShan的博客：“无人驾驶汽车系统入门（一）——卡尔曼滤波与目标追踪 https://blog.csdn.net/AdamShan/article/details/78248421” ，此处对其中的内容做了部分修改与调整

1.2.1 - 建模

以估计行人运动状态为例，首先需要建立起待估计的行人状态表示，也即前文中卡尔曼滤波计算过程图中的状态矩阵 $x$ 。在这里， $x_{k+1}$ 代表的是当前时刻的状态， $x_k$ 代表的是前一时刻的状态。若仅考虑匀速运动的行人在二维平面中状态，大致可以用x、y方向上的位置 $p$ 以及速度分量 $v$ 进行表示，即：
$x=(p_x,p_y,v_x,v_y)^T$
构建好状态表示后，还需要构建一个用于参考的行人运动模型。通过这个模型可以推断下一时刻行人的状态，还可依此得到用于卡尔曼滤波之中的“预测模型”，也就是卡尔曼滤波计算过程图中的左半部分。对于在二维平面中的匀速运动，可以使用一个恒定速度模型：

$x_{k+1}=Ax_k+ε$
式中的 $A$ 为矩阵，也即是描述从上一个状态 $x_k$ 如何转移到下一个状态 $x_{k+1}$ 的状态转移矩阵； $ε$ 为过程噪声所引入的误差。在这个简单的匀速运动例子中，限定过程噪声仅来源于行人在x方向上以 $a_x$ 为加速度值，与y方向上以 $a_y$ 为加速度值的加减速。将上式的行人模型写为矩阵形式可能更便于理解，其中 $\Delta t$ 为滤波步骤，或者说状态与状态之间计算的间隔时间：
$\begin{bmatrix} p_x\\p_y\\v_x\\v_y \end{bmatrix}_{k+1}=\begin{bmatrix} 1&0&\Delta t&0\\0&1&0&\Delta t\\0&0&1&0\\0&0&0&1 \end{bmatrix}.\begin{bmatrix} p_x\\p_y\\v_x\\v_y \end{bmatrix}_k+\begin{bmatrix} {1\over2}a_x\Delta t^2\\{1\over2}a_y\Delta t^2\\a_x\Delta t \\a_y\Delta t \end{bmatrix}$

展开，便可以得到：
$\left \{ \begin{matrix} p_x^{k+1} = p_x+v_x^k\Delta t+{1\over2}a_x\Delta t^2\\p_y^{k+1} = p_y+v_y^k\Delta t+{1\over2}a_y\Delta t^2\\v_x^{k+1}=v_x+a_x\Delta t \\v_y^{k+1}=v_y+a_y\Delta t\end{matrix}\right.$
显然，含有 $a_x$ 与 $a_y$ 的项便是默认的行人匀速运动中不定的加速度所引入的误差。在实际应用中，过程噪声不大可能仅来源于某一可以量化的干扰项，更常见的是多种不可测的因素的组合结果。因此，在设计过程噪声协方差矩阵 $Q$ 时，可以遵循前文所述的建议，尽可能让实际的误差落在模型所描述的误差范围之中。

既然行人的大致建模已经完成，下一步便可以开始着手卡尔曼滤波的相关构建与实现。

1.2.2 - Python实现

在卡尔曼滤波计算过程图的状态预测步骤（Project the state ahead）中， $B$ 是作用在控制向量 $u$ 上的输入控制模型矩阵，它描述了控制向量 $u$ 实际能作用到状态矩阵 $x$ 的哪些状态以及作用的方式。在这个例子中，行人没有踩着平衡车，背后也没有安装火箭喷射器，只是单纯的在匀速行走。但行人内部的肌肉控制是无法透析的，这导致无法对 $B u$ 进行一个测量。因此在这个例子中， $B u$ 被设置为0，图中状态预测步骤的公式也就变成了 $x_{k+1}=Ax_k$ ，将其写为矩阵形式就是这样：

$\begin{bmatrix} p_x\\p_y\\v_x\\v_y \end{bmatrix}_{k+1}=\begin{bmatrix} 1&0&\Delta t&0\\0&1&0&\Delta t\\0&0&1&0\\0&0&0&1 \end{bmatrix}.\begin{bmatrix} p_x\\p_y\\v_x\\v_y \end{bmatrix}_k$

x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T                 # x状态矩阵：x位置，y位置，x速度，y速度
print("the X is\n", x, x.shape)

dt = 0.1                                                # Time step between filter steps
A = np.matrix([[1.0, 0.0, dt, 0.0],                     # 状态变换矩阵（处理矩阵）A
               [0.0, 1.0, 0.0, dt],
               [0.0, 0.0, 1.0, 0.0],
               [0.0, 0.0, 0.0, 1.0]])
print("------\nthe A is\n", A, A.shape)

第二步，需要给定预测误差的协方差矩阵 $P$ 、过程噪声的协方差矩阵 $Q$ ，以及测量噪声的协方差矩阵 $R$ 。对于预测误差协方差矩阵 $P$ ，只需要给出初始值即可，在后续的误差协方差更新（Project the error covariance ahead）中，会对其自动的更新。

对于过程噪声协方差矩阵 $Q$ ，由于例子中限定了过程噪声仅考虑为行人在x、y方向上随机的加减速所带来的影响，且x、y方向上的加速度限定为同一标量数值，其符合均值为0，标准差为 $σ_a$ 的正态分布。故过程噪声协方差矩阵 $Q$ ，即公式2中的 $ε$ 的协方差矩阵可以展开如下^[3,4]：
$Q=cov(ε)=cov(Ga)=E[(Ga)(Ga)^T]=GE[a^2]G^T=G[σ_a^2]G^T=[σ_a^2]GG^T$
其中 $G=\begin{bmatrix} {1\over2}\Delta t^2&{1\over2}\Delta t^2&\Delta t&\Delta t \end{bmatrix}^T$ 。而对于 $σ_a$ ，可以取经验值 $0.5m/s^2$ 。

而对于测量噪声的协方差矩阵 $R$ ，其与测量向量相关。在这个例子中，限定可测量的量仅为行人在x、y方向上的速度，即测量向量 $z$ 可表示为 $z=\begin{bmatrix} v_x&v_y\end{bmatrix}^T$ 。测量噪声的协方差矩阵 $R$ 对角线上的元素反映了每个观测量的误差的方差，非对角线上的元素则反映了不同观测量的误差之间的相关性。对于速度测量，两个方向上的测量量是相互独立的，故矩阵 $R$ 的非对角线上的元素因为0，对角线上的元素可以分别用 $σ_{v_x}$ 与 $σ_{v_y}$ 来描述传感器在x、y方向上的测量结果与实际值之间的偏差，这属于传感器的固有性质，可以通过查阅数据手册或者进行试验测得。那么，测量噪声协方差矩阵 $R$ 便可以写为以下矩阵形式：
$R=\begin{bmatrix}σ_{v_x}^2&0\\0&σ_{v_y}^2\end{bmatrix}$
借着讨论测量向量，我们不妨也讨论一下测量矩阵 $H$ 。在测量方程 $z_k=H_kx_k+ν_k$ 中， $ν$ 是测量误差， $z_k$ 是 $m \times 1$ 维的测量向量， $x_k$ 为 $n \times 1$ 维的状态向量。由此可知，测量矩阵 $H$ 为 $m \times n$ 维的矩阵。即 $H$ 的维数与测量向量 $z$ 、状态向量 $x$ 都有关。

在例子中，由于只能测得x与y方向上的速度，简便起见，将 $z$ 写为了 $\begin{bmatrix} v_x&v_y\end{bmatrix}^T$ 的一个2×1维的向量，由于状态向量 $x$ 为4×1维，那么对应的， $H$ 即为 $\begin{bmatrix}0&0&1&0\\0&0&0&1\end{bmatrix}$ 的2×4维矩阵。同理，若将 $z$ 写为 $\begin{bmatrix}0&0&v_x&v_y\end{bmatrix}^T$ ，则 $H$ 应写为 $\begin{bmatrix}0&0&0&0\\0&0&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix}$ 。关于更多卡尔曼滤波过程中矩阵维数的讨论，可以参考^[7]。

P = np.diag([1000.0, 1000.0, 1000.0, 1000.0])           # P:预测误差协方差矩阵，在不确定的时候对角线取一个合适的较大的数值、、
print("------\nthe P is\n", P, P.shape)

H = np.matrix([[0.0, 0.0, 1.0, 0.0],                    # H:测量矩阵，只可以直接测量到速度
               [0.0, 0.0, 0.0, 1.0]])
print("------\nthe H is\n", H, H.shape)

ra = 0.09                                               # 测量噪声，随便取的
R = np.matrix([[ra, 0.0],                               # R:测量噪声协方差矩阵
               [0.0, ra]])
print("------\nthe R is\n", R, R.shape)

sv = 0.5                                                # 作为过程噪声的行人行走加速度值，这个是取的经验值
G = np.matrix([[0.5*dt**2],
               [0.5*dt**2],
               [dt],
               [dt]])
Q = G*G.T*sv**2                                         # Q:过程噪声协方差矩阵
print("------\nthe Q is\n", Q, Q.shape)

而后，我们需要随机的产生一些测量数据以供滤波算法使用。

# 随机产生测量数据
measureNum = 200                                        # 共计多少组测量数据
vx = 20                                                 # x方向速度
vy = 10                                                 # y方向速度

mx = np.array(vx + np.random.randn(measureNum))         # 生成x方向的随机测量量
my = np.array(vy + np.random.randn(measureNum))         # 生成y方向的随机测量量
measureData = np.vstack((mx, my))

print("measureData shape: ", measureData.shape)
print('Standard Deviation of Acceleration Measurements=%.2f \n You assumed %.2f in R' % (np.std(mx), R[0, 0]))

我们还可以将生成的随机数据绘制出来进行查看

fig = plt.figure(figsize=(16, 5))
plt.step(range(measureNum), mx, label= '$\dot x$')
plt.step(range(measureNum), my, label= '$\dot y$')
plt.ylabel(r'Velocity $m/s$')
plt.title('Measurements')
plt.legend(loc='best', prop={'size':18})
plt.show()

最后，我们使用前面定义好的变量，实现一下卡尔曼滤波的计算过程，循环会不断的利用新的测量值与当前状态，对状态向量 $x$ 进行更新。

for n in range(len(measureData[0])):
    # Time update (Prediction)
    # ======
    # Project the state ahead
    x = A*x

    # Project the error covariance ahead
    P = A*P*A.T + Q

    # Measurement Update (Correction)
    # ======
    # Compute the Kalman Gain
    _tempKpart = H*P*H.T + R
    K = (P*H.T) * np.linalg.pinv(_tempKpart)

    # Update the estimate via z
    Z = measureData[:, n].reshape(2, 1)
    _tempZpart = Z - (H * x)
    x = x + (K * _tempZpart)

    # Update the error covariance
    P = (I - (K * H)) * P

1.2.3 - 代码

此处与前节的代码主要源自AdamShan^[3]的博客，此处仅做了部分修改，去除了部分容易引起歧义的代码，并补全了注释，以便阅读。

import numpy as np
import matplotlib.pyplot as plt

# 卡尔曼滤波参数初始化
x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T                 # x状态矩阵：x位置，y位置，x速度，y速度
print("the X is\n", x, x.shape)
P = np.diag([1000.0, 1000.0, 1000.0, 1000.0])           # P:预测误差协方差矩阵，在不确定的时候对角线取一个合适的较大的数值、、
print("------\nthe P is\n", P, P.shape)

dt = 0.1                                                # Time step between filter steps
A = np.matrix([[1.0, 0.0, dt, 0.0],                     # 状态变换矩阵（处理矩阵）A
               [0.0, 1.0, 0.0, dt],
               [0.0, 0.0, 1.0, 0.0],
               [0.0, 0.0, 0.0, 1.0]])
print("------\nthe A is\n", A, A.shape)

H = np.matrix([[0.0, 0.0, 1.0, 0.0],                    # H:测量矩阵，只可以直接测量到速度
               [0.0, 0.0, 0.0, 1.0]])
print("------\nthe H is\n", H, H.shape)

ra = 0.09                                               # 测量噪声
R = np.matrix([[ra, 0.0],                               # R:测量噪声协方差矩阵
               [0.0, ra]])
print("------\nthe R is\n", R, R.shape)

sv = 0.5                                                # 作为过程噪声的行人行走加速度值，这个是取的经验值
G = np.matrix([[0.5*dt**2],
               [0.5*dt**2],
               [dt],
               [dt]])
Q = G*G.T*sv**2                                         # Q:过程噪声协方差矩阵
print("------\nthe Q is\n", Q, Q.shape)

I = np.eye(4)
print("------\nthe I is\n", I, I.shape)

# 随机产生测量数据
measureNum = 200                                        # 共计多少组测量数据
vx = 20                                                 # x方向速度
vy = 10                                                 # y方向速度

mx = np.array(vx + np.random.randn(measureNum))         # 生成x方向的随机测量量
my = np.array(vy + np.random.randn(measureNum))         # 生成y方向的随机测量量
measureData = np.vstack((mx, my))

print("measureData shape: ", measureData.shape)
print('Standard Deviation of Acceleration Measurements=%.2f \n You assumed %.2f in R' % (np.std(mx), R[0, 0]))

fig = plt.figure(figsize=(16, 5))
plt.step(range(measureNum), mx, label= '$\dot x$')
plt.step(range(measureNum), my, label= '$\dot y$')
plt.ylabel(r'Velocity $m/s$')
plt.title('Measurements')
plt.legend(loc='best', prop={'size':18})
plt.show()

# 过程值保存
xt = []
yt = []
dxt= []
dyt= []
Zx = []
Zy = []
Px = []
Py = []
Pdx= []
Pdy= []
Rdx= []
Rdy= []
Kx = []
Ky = []
Kdx= []
Kdy= []


def SaveStates(x, Z, P, R, K):                          # x:状态向量，Z：测量值，P：预测误差协方差矩阵，R：测量误差协方差矩阵，K：卡尔曼增益
    xt.append(float(x[0]))
    yt.append(float(x[1]))
    dxt.append(float(x[2]))
    dyt.append(float(x[3]))
    Zx.append(float(Z[0]))
    Zy.append(float(Z[1]))
    Px.append(float(P[0, 0]))
    Py.append(float(P[1, 1]))
    Pdx.append(float(P[2, 2]))
    Pdy.append(float(P[3, 3]))
    Rdx.append(float(R[0, 0]))
    Rdy.append(float(R[1, 1]))
    Kx.append(float(K[0, 0]))
    Ky.append(float(K[1, 0]))
    Kdx.append(float(K[2, 0]))
    Kdy.append(float(K[3, 0]))


for n in range(len(measureData[0])):
    # Time update (Prediction)
    # ======
    # Project the state ahead
    x = A*x

    # Project the error covariance ahead
    P = A*P*A.T + Q

    # Measurement Update (Correction)
    # ======
    # Compute the Kalman Gain
    _tempKpart = H*P*H.T + R
    K = (P*H.T) * np.linalg.pinv(_tempKpart)

    # Update the estimate via z
    Z = measureData[:, n].reshape(2, 1)
    _tempZpart = Z - (H * x)
    x = x + (K * _tempZpart)

    # Update the error covariance
    P = (I - (K * H)) * P

    # Save states (for plotting)
    SaveStates(x, Z, P, R, K)


# 绘制结果
def PlotFilterResult():
    _fig = plt.figure(figsize=(16, 9))
    plt.step(range(len(measureData[0])), dxt, label='$filter Vx$')
    plt.step(range(len(measureData[0])), dyt, label='$filter Vy$')

    plt.step(range(len(measureData[0])), measureData[0], label='$measurement Vx$')
    plt.step(range(len(measureData[0])), measureData[1], label='$measurement Vy$')

    plt.axhline(vx, color='#999999', label='$true Vx$')
    plt.axhline(vy, color='#999999', label='$true Vy$')

    plt.xlabel('Filter Step')
    plt.ylabel('Velocity')
    plt.title("Estimate (Elements from State Vector $x$)")
    plt.legend(loc='best', prop={'size': 11})
    plt.ylim([0, 30])


def PlotEstimatePossition():
    _fig = plt.figure(figsize=(16, 9))
    plt.scatter(xt, yt, s=20, label='State', c='k')         # 路径线段
    plt.scatter(xt[0], yt[0], s=100, label='Start', c='g')  # 起点
    plt.scatter(xt[-1], yt[-1], s=100, label='End', c='r')  # 终点

    plt.xlabel('X')
    plt.ylabel('Y')
    plt.title('Position')
    plt.legend(loc='best')
    plt.axis('equal')


PlotEstimatePossition()
PlotFilterResult()
plt.show()

代码运行后绘制效果如下：
在这里插入图片描述

里先森

关注

1
点赞
踩
6

收藏

觉得还不错? 一键收藏
0
评论
卡尔曼滤波学习笔记

本文是在学习卡尔曼滤波过程中所作的个人笔记，由于个人数学功底较差，文中的一些观点与理解仅供读者参考，如有疏漏还请诸位不吝赐教。文中的部分参考文献来源如下：《Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation》《GNSS与惯性及多传感器组合导航系统原理》无人驾驶汽车系统入门（一）——卡尔曼滤波与目标追踪 https://blog.csdn.net/AdamShan/article/det.
复制链接

扫一扫

专栏目录