关于卡尔曼滤波详细推导的理解

最新推荐文章于 2024-08-18 10:13:28 发布

MAUM

最新推荐文章于 2024-08-18 10:13:28 发布

阅读量2k

点赞数 4

分类专栏：最优化问题卡尔曼滤波文章标签：卡尔曼滤波理论推导符号详解

本文链接：https://blog.csdn.net/maum61/article/details/88971344

版权

最优化问题同时被 2 个专栏收录

5 篇文章 0 订阅

订阅专栏

卡尔曼滤波

1 篇文章 0 订阅

订阅专栏

相信大家到了关注卡尔曼滤波原理推导的阶段，已经对卡尔曼滤波有所了解了，大概知道，卡尔曼滤波是应用在含有噪声的、模型（预测）基本知道一些的、传感器测量含有噪声的这种场合，对真实的状态（或变量值）进行最优估计。

卡尔曼滤波的详细推导过程各大博主都有详细介绍，写本文的主要目的是查漏补缺，并结合进去一些自己的理解，帮助更多人去学习卡尔曼滤波。为了符号的一致性，在看了几篇关于卡尔曼滤波的博文，进行对比后，本文采用这篇博文的符号（该文章写的很规范）。

一、卡尔曼滤波的思想与最优化问题的关系

卡尔曼滤波可以用在一维数据上，也可以用在向量上，因此就牵扯到了很多矩阵的知识。既然是最优估计，更具体说是使卡尔曼估计值与真实值之间的均方误差最小，那么就需要引入均方误差的计算。再说说均方误差，在最优估计里面就是l2范数。误差的l2范数是凸函数，因此求其关于某一个参数的最小值问题就可以转化为：l2范数对参数求偏导并令偏导函数为零，此时的参数值即为零l2范数最小的值，这是最优化问题典型的解决思路，在卡尔曼滤波里面，最优问题是使（真实值-卡尔曼估计值）的均方误差最小。

二、卡尔曼滤波公式

这里我先附上卡尔曼滤波的5个公式。当你看到这些公式感觉头大的时候，说明基础知识太薄弱，建议先花一些时间学习矩阵运算、状态方程建模、概率论等相关知识，以免造成理解不够透彻，只是生硬地知道这么几个公式，看着例子能简单地改改程序。

${\hat{\mathbf{x}}}'_{k}=\mathbf{A}{\hat{\mathbf{x}}} _{k-1}+\mathbf{B}\mathbf{u}_{k-1}$ (2.1) //该式是根据上一时刻（k-1）的卡尔曼滤波结果和系统状态模型（经验）得到本次的预测值

${\hat{\mathbf{P}}}'_{k}=\mathbf{A}{\hat{\mathbf{P}}} _{k-1}\mathbf{A}^{T}+\mathbf{Q}$ (2.2) //计算用于更新卡尔曼增益K的先验估计误差的协方差矩阵

下面是三个测量更新方程：

${\mathbf{K}}_{k}={\hat{\mathbf{P}}}' _{k} \mathbf{H}^{T}(\mathbf{H}{\hat{\mathbf{P}}}' _{k}\mathbf{H}^{T}+\mathbf{R})^{-1}$ (2.3)//计算此次的卡尔曼增益

${\hat{\mathbf{x}}}_{k}={\hat{\mathbf{x}}}'_{k}+\mathbf{K}_{k}({\mathbf{z}}_{k}-\mathbf{H}{\hat{\mathbf{x}}}'_{k})$ (2.4)//计算测量余量与卡尔曼增益的乘积，以此来校正此次预测值，得到此次的卡尔曼滤波值

${\hat{\mathbf{P}}}_{k}=(\mathbf{I}-{\mathbf{K}_{k}\mathbf{H})\hat{\mathbf{P}}}'_{k}$ (2.5)//利用先验估计误差的协方差矩阵更新此次的后验估计误差协方差矩阵，以备下次迭代使用。

三、符号详解

刚开始接触的时候，最难理解的是公式里的符号，这些符号的意义是什么？哪些量是已知的，哪些是未知的，哪些是永远也不知道的。本文参考了之前资料，针对第二章中卡尔曼推导公式中的符号做详细说明。

3.1 模型说明

说明之前，先设定好对象：

以一个例子为对象（这里是直接截别人的图，如作者认为不合适，则立即撤销）：

这样，系统的状态方程就可以表示为：

$\mathbf{x}_{k}=\mathbf{A}\mathbf{x}_{k-1}+\mathbf{B}\mathbf{u}$ （3.1）

（3.1）式是我们自认为合理的状态预测模型，实际上是有误差的，误差我们定义为wk，是指系统状态方程的过程激励噪声，可以指代模型误差的那一部分，也就是说，系统模型可以不那么精确。假如模型十分精确，传感器也十分精确，意味着wk=0，那卡尔曼滤波就没必要了。。。因此，完整的状态预测方程可以表示为：

$\mathbf{x}_{k}=\mathbf{A}\mathbf{x}_{k-1}+\mathbf{B}\mathbf{u}+\mathbf{w}_{k}$ （3.2）

3.2 上述公式和模型变量详解

$\mathbf{A}$ ：状态转移矩阵（已知，系统建模中已经给出。）

$\mathbf{B}$ ：输入增益矩阵（已知，表示控制输入如何影响状态。比如给小车施加外力f，该输入f反应在小车位置上的影响。）

$\mathbf{w}_{k}$ ：过程激励噪声（未知，随机的，但是模型已知，wk服从N(0,Q)。假如状态只有一维，那么Q就是方差 $\mathbf{\sigma }^{2}$ 。）

$\mathbf{H}$ ：状态到观测的转换矩阵（已知，解释见 ${\mathbf{z}}_{k}$ 中）

${\mathbf{z}}_{k}$ ：观测值（已知，观测到的状态，不准， ${\mathbf{z}}_{k}=\mathbf{H}{\mathbf{x}}_{k}+{\mathbf{v}}_{k}$ 。观测值可以理解为，真实值xk加入一些噪声vk，如果观测到的数据不是xk的直接表示，那么经过一个转换矩阵H，把xk转换为观测的数据，再组合进来一些噪声。）

${\mathbf{x}}_{k}$ ：真实的状态值（不知道，可能也永远都不知道。但是它包含在了 ${\mathbf{z}}_{k}$ 里面，一般传感器都有噪声，因此真实值，可望，而不可及。）

$\hat{{\mathbf{x}}}_{k}$ ：卡尔曼估计值（求解得到，真相 ${\mathbf{x}}_{k}$ 永远不得而知，退而求其次，找到最佳的估计，也是很好的选择。这个也是卡尔曼滤波最终的输出结果。）

${\hat{\mathbf{x}}}'_{k}$ ：状态值的先验估计（求解得到，这个就是根据状态模型（3.1）式和上一次的卡尔曼估计值 $\hat{{\mathbf{x}}}_{k}$ 得到的估计值，此处不考虑误差，因为误差谁都不知道。）

$\hat{{\mathbf{x}}}_{k-1}$ ：（已知，上一次迭代卡尔曼滤波器的输出值，也即k-1时刻的卡尔曼估计值。）

${{\mathbf{u}}}_{k-1}$ ：（已知，上一次状态预测模型的输入。）

${\hat{\mathbf{P}}}'_{k}$ ：先验估计误差的协方差矩阵（求解得到，先验估计误差指的是什么？先验估计可以理解为，我们利用预测模型(3.1）式估计得到的状态 ${\hat{\mathbf{x}}}'_{k}$ 与真实的状态 ${\mathbf{x}}_{k}$ 之间的误差e'的自协方差。而后面只用到了迹，那么可以简单的理解为， ${\hat{\mathbf{P}}}'_{k}$ 是我们利用先验模型预测误差的平方)。

$\mathbf{Q}$ ：过程激励噪声协方差矩阵（已知，卡尔曼滤波器假定过程激励噪声符合标准正态分布，方差为Q，也即是高斯白噪声。这个也是需要我们去确定的，属于先验知识。也就是 $\mathbf{w}_{k}$ 服从的正态分布的方差）

$\mathbf{R}$ ：观测噪声协方差矩阵（已知，卡尔曼滤波器假定观测噪声符合标准正态分布，方差为R，也即是高斯白噪声，也就是。这个也是需要我们去确定的，属于先验知识。）

${\mathbf{K}}_{k}$ ：卡尔曼增益（求解得到，该参数被形象得称为最有估计组成的“调节器”。根据公式 ${\mathbf{K}}_{k}={\hat{\mathbf{P}}}' _{k} \mathbf{H}^{T}(\mathbf{H}{\hat{\mathbf{P}}}' _{k}\mathbf{H}^{T}+\mathbf{R})^{-1}$ ，可以看出该增益受到先验估计误差的协方差矩阵 ${\hat{\mathbf{P}}}'_{k}$ 和后验估计误差的协方差矩阵 $\mathbf{R}$ 调节。可以定性地理解：先验估计误差协方差矩阵 ${\hat{\mathbf{P}}}'_{k}$ 如果很小，趋近于0，那么意味着什么呢？我们的模型很准，预测值（先验值）即可准备描述真实值 ${\mathbf{x}}_{k}$ ，由公式可得 ${\mathbf{K}}_{k}$ 趋近于0。由（2.4） ${\hat{\mathbf{x}}}_{k}={\hat{\mathbf{x}}}'_{k}+\mathbf{K}_{k}({\mathbf{z}}_{k}-\mathbf{H}{\hat{\mathbf{x}}}'_{k})$ 得知，当 ${\mathbf{K}}_{k}$ 趋近于0时， ${\hat{\mathbf{x}}}_{k}\rightarrow {\hat{\mathbf{x}}}'_{k}$ 。这样就是符合我们的想法。因为我预测的误差本来就是很小的，我就应该听从预测值。所以卡尔曼的输出 $\hat{{\mathbf{x}}}_{k}$ 就近似等于 ${\hat{\mathbf{x}}}'_{k}$ 。相反，当我传感器很准时，也就是观测噪声很小，那么观测噪声协方差矩阵 $\mathbf{R}\rightarrow \mathbf{0}$ ，那么根据（2.4）， ${\mathbf{K}}_{k}\rightarrow {\hat{\mathbf{P}}}' _{k} \mathbf{H}^{T}(\mathbf{H}{\hat{\mathbf{P}}}' _{k}\mathbf{H}^{T})^{-1}$ ，也即是 $\mathbf{H}$ 的伪逆，简单理解为 $\frac{1}{\mathbf{H}}$ 。代入到（2.4），求得 ${\hat{\mathbf{x}}}_{k}\rightarrow {\hat{\mathbf{x}}}'_{k}+\mathbf{H}^{+}({\mathbf{z}}_{k}-\mathbf{H}{\hat{\mathbf{x}}}'_{k})\rightarrow \frac{{\mathbf{z}}_{k}}{\mathbf{H}}$ 。而 ${\mathbf{z}}_{k}=\mathbf{H}{\mathbf{x}}_{k}+{\mathbf{v}}_{k}$ ，我们已经假定传感器测量很准，因此测量噪声 ${\mathbf{v}}_{k}$ 是0，所以 ${\hat{\mathbf{x}}}_{k}\rightarrow {\hat{\mathbf{x}}}'_{k}+\mathbf{H}^{+}({\mathbf{z}}_{k}-\mathbf{H}{\hat{\mathbf{x}}}'_{k})\rightarrow \frac{{\mathbf{z}}_{k}}{\mathbf{H}}\rightarrow \frac{\mathbf{H}{\mathbf{x}}_{k}}{\mathbf{H}}\rightarrow \mathbf{x}_{k}$ 。也就是说，我们的卡尔曼滤波器输出的，就是真实的值 ${\mathbf{x}}_{k}$ 。当然如果你完全相信你的传感器，那就没必要滤波啦，直接使用传感器的值就好了。）

$({\mathbf{z}}_{k}-\mathbf{H}{\hat{\mathbf{x}}}'_{k})$ ：测量余量（求解得到，这里解释一下这一项， ${\mathbf{z}}_{k}$ 是观测值，也就是从传感器得到的数据。 $\mathbf{H}{\hat{\mathbf{x}}}'_{k}$ 是什么呢？可以这么理解，我们利用模型预测了本次的值 ${\hat{\mathbf{x}}}'_{k}$ ，通过 $\mathbf{H}$ 矩阵转化为测量值。这有啥意义呢？我们把预测的状态转化为观测值的形式，然后求观测值和预测观测值的差 $({\mathbf{z}}_{k}-\mathbf{H}{\hat{\mathbf{x}}}'_{k})$ ，这个差，我们要将其融合在预测值中。结合式（2.4） ${\hat{\mathbf{x}}}_{k}={\hat{\mathbf{x}}}'_{k}+\mathbf{K}_{k}({\mathbf{z}}_{k}-\mathbf{H}{\hat{\mathbf{x}}}'_{k})$ ，我们是取了 $({\mathbf{z}}_{k}-\mathbf{H}{\hat{\mathbf{x}}}'_{k})$ 的 ${\mathbf{K}}_{k}$ 倍，将测量余量的 ${\mathbf{K}}_{k}$ 加到预测值中，最后输出卡尔曼滤波结果。结合对 ${\mathbf{K}}_{k}$ 的理解， ${\mathbf{K}}_{k}$ 是系数。如下图所示。这一步也是卡尔曼融合算法的核心所在。）

${\hat{\mathbf{P}}}_{k}$ ：后验估计误差的协方差（求解得到， ${\hat{\mathbf{P}}}_{k}=E\begin{Bmatrix} \mathbf{e}_{k},\mathbf{e}_{k}^{T} \end{Bmatrix}=cov(\mathbf{e}_{k})$ ， $\mathbf{e}_{k}= \mathbf{x}_{k}-\hat{\mathbf{x}}_{k}$ ，也就是真实值与卡尔曼估计值之间的误差。又来了，真实值是不知道的啊，但是根据理论推导，特别是观测值里面含有真实值 $\mathbf{x}_{k}$ ，所以可以得到，具体过程还是参照这篇文章。）