算法笔记-卡尔曼滤波器简单解释

最新推荐文章于 2024-05-19 21:45:19 发布

raymond_kwan

最新推荐文章于 2024-05-19 21:45:19 发布

阅读量9.8k

点赞数 9

分类专栏： Algorithm 文章标签：算法

本文链接：https://blog.csdn.net/raym0ndkwan/article/details/50031729

版权

算法笔记-卡尔曼滤波器简单解释

算法笔记-卡尔曼滤波器简单解释

动态时间规整/规划(Kalman Filter, 又称linear quadratic estimation)也是一个比较老的算法，大概在1960年左右被提出来，算法的名字来自算法早期的一个提出者Rudolf E. Kálmán。卡尔曼滤波器至今还是十分常用的一种滤波算法，并且在此基础上发展出了很多种延伸的算法。卡尔曼滤波器算法的核心可由几道简洁的公式表示，但对于刚刚接触的人来说可能比较难以彻底理解这些公式，这篇笔记尝从一个实际的运动场景出发，具体解释卡尔曼滤波器的核心算法。

1.简介

卡尔曼滤波器在监控、导航和控制系统上有大量的运用。在这些系统里面，系统的观测量和这些被观测量的真实值往往是有细微差异的，基于我们对这些系统的认识（例如已有的描述系统的模型）和观测值，我们能够更好地推测系统的真实状态。这也是卡尔曼滤波器的核心思路，下面会先从公式出发，然后到一个简单的例子，进一步介绍这个思想是如何体现的。

2.公式

首先，假设一个系统在t时刻的状态是跟t-1时刻的状态是有关的：

x t = F t x t - 1 + B t u t + w t (1)

$\mathbf{x}_{t} = \mathbf{F}_{t}\mathbf{x}_{t-1}+\mathbf{B}_{t}\mathbf{u}_{t}+\mathbf{w}_{t}(1)$

其中各项的意义如下：
$\mathbf{x}_{t}$ 是一个代表系统在t时刻的真实状态向量，该向量可以包含着例如位置，速度等信息；
$\mathbf{F}_{t}$ 是t时刻的状态转换矩阵，代表着系统状态在t-1时刻对系统t时刻的影响，例如t-1时没有速度和加速度，那么在t时位置应该不变；
$\mathbf{u}_{t}$ 是t时刻的控制输入向量,例如是否有踩油门和转方向盘等；
$\mathbf{B}_{t}$ 是t时刻的输入转换矩阵，即量化了输入对状态的影响；
$\mathbf{w}_{t}$ 是t时刻的过程噪声，我们假设该噪声服从均值为0的多变量正态分布，其协方差矩阵我们标记为 $\mathbf{Q}_{t}$ 。

然后，我们要留意到，我们观察或测量的值往往不能直接反应我们关心的状态值，而且测量也存在测量误差，我们用下面这一式子反应这些：

z t = H t x t + v t (2)

$\mathbf{z}_{t} = \mathbf{H}_{t}\mathbf{x}_{t}+\mathbf{v}_{t}(2)$

其中各项的意义如下：
$\mathbf{z}_{t}$ 代表系统在t时刻的观察值向量，该向量包含着系统测量的结果；
$\mathbf{H}_{t}$ 是t时刻的测量转换矩阵，这个矩阵表示状态向量和观察值向量之间的关系，当观察值就是状态时，该矩阵为单位矩阵；
$\mathbf{v}_{t}$ 是t时刻的测量误差，类似过程噪声，我们假设该误差服从均值为0的多变量正态分布，其协方差矩阵我们标记为 $\mathbf{R}_{t}$ 。

卡尔曼滤波器要解决的问题，或者说将式子(1)和(2)简单概括一下，就是已知 $\mathbf{F}_{t}$ ， $\mathbf{u}_{t}$ ， $\mathbf{B}_{t}$ ， $\mathbf{z}_{t}$ ， $\mathbf{H}_{t}$ ， $\mathbf{x}_{t-1}$ 并且 $\mathbf{w}_{t}$ 与 $\mathbf{v}_{t}$ 服从已知的正态分布1，求解 $\mathbf{x}_{t}$ 。
我们首先假设所求状态服从一个多变量的正太分布,即 $\mathbf{x} \sim \mathcal{N}(\boldsymbol\mu,\,\boldsymbol\Sigma)$ 。然后卡尔曼滤波器是通过一个迭代的过程解决这个问题的，每一次迭代包含两步，分别是预测和观测两步。

第一步预测：

x ̂ t | t - 1 = F t x ̂ t - 1 | t - 1 + B t u t (3)

$\mathbf{\hat{x}}_{t|t-1} = \mathbf{F}_{t}\mathbf{\hat{x}}_{t-1|t-1}+\mathbf{B}_{t}\mathbf{u}_{t}(3)$

P t | t - 1 = F t P t - 1 | t - 1 F T t + Q t (4)

$\mathbf{P}_{t|t-1} = \mathbf{F}_{t}\mathbf{P}_{t-1|t-1}\mathbf{F}_{t}^{T}+\mathbf{Q}_{t}(4)$

此处出现的几个新的项：
首先是带帽的状态量 $\mathbf{\hat{x}}$ ，主要用于与真实值区分开，这个值是卡尔曼滤波器的运算结果，即我们的对状态量的估计值。从另一个角度讲，这个就是我们估测的 $\boldsymbol\mu$ 。
而 $\mathbf{P}$ 是一个协方差矩阵，也就是我们估计的 $\boldsymbol\Sigma$ 。
其中 $_{t|t-1}$ 指的是 $t$ 时刻的预测步的结果，而 $_{t-1|t-1}$ 指的是 $t-1$ 时刻的观测步的结果。