【转载】Kalman滤波详细推导

原文作者：学海拾贝
原文标题：卡尔曼滤波器、扩展卡尔曼滤波器、无向卡尔曼滤波器的详细推导
原文链接：https://blog.csdn.net/u013102281/article/details/59109566?utm_source=blogxgwz3
（转载仅为了个人学习方便，并无他意）

这段时间做轴承故障诊断和预测的时候，需要一个针对已经获取了特征向量的工具来对轴承故障状态进行估计和预测。卡尔曼滤波器可以实现对过去、当前和未来目标位置的估计，所以想通过卡尔曼滤波器的设计思路找到一些灵感。虽然最后发现：卡尔曼滤波器中的状态量是有具体的物理含义的物理量，而表征轴承故障状态的量只是一种表征量。这两者之间存在着本质的差别，因为轴承的退化过程目前为止还不能建模。虽然如此，我还是想将卡尔曼滤波器详细的推导过程分享给大家。
学习过程中，参考了白巧克力亦唯心的文章：卡尔曼滤波–从推导到应用（一），卡尔曼滤波–从推导到应用（二），在此给出直达连接。他/她的文章有故事，有推导，有例子，有代码，很优秀，向大家推荐。我在这篇文章里希望把很多文章中语焉不详的推导，以及符号的定义阐释清楚，并加入一些自己的理解，希望和大家分享。第一次写博客，markdown编辑器的用法还不熟悉，公式编辑会花去很多时间。所以直接用图片代替了，希望大家能够理解，以后我会慢慢按照标准的格式来编辑公式的。
一、基本卡尔曼滤波器（BKF）
卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。关于这种滤波器的论文最早由Swerling(1958)发表了这种想法。
1、基本动态模型
假设1：k时刻的真实状态是从k-1时刻的真实状态演化而来；
假设2：演化与测量的过程由线性算子来描述。
2、两个基本的方程
四个状态值的定义：

状态转移方程：

状态测量方程：

基于k-1时刻状态对k时刻状态的估计值与真实值之间的差称为估计误差，该估计误差的协方差矩阵的定义为后验估计误差协方差矩阵，用下式表示：

其模型拓扑结构用隐马尔科夫链可以表示成图1（维基百科:卡尔曼滤波）这样：

图1 卡尔曼滤波的隐马尔可夫链式模型
3、两个基本的过程
3.1 预测过程


从公式（3）*推导公式（2）：

3.2 更新过程
我们基于k-1时刻对k时刻状态的估计是否正确，需要用与实际测量值之间的误差来衡量，并且考虑用这个误差来补偿。所以在更新之前，我们应该计 算实际测量值与估计输出值之间的差值及其协方差矩阵。

（3）式的协方差矩阵表示为：

下面推导公式（4）：

然后我们再进行更新的步骤。更新是指：由基于k-1时刻对k时刻状态的估计值应当如何得到k时刻的估计值。卡尔曼的思想就是：用基于k-1时刻对k时刻状态的估计值与预测输出值和实际输出值之间的差进行线性组合得到k时刻的估计值，连接这两者的就是卡尔曼增益。这里体现的就是反馈的思想，更新过程的第一步用下式表示：

那么现在问题来了，如何求取这个卡尔曼增益呢？
这时候我们应该回到我们的出发点，我们希望的是滤除干扰真实状态的噪声，是滤波器的估计状态与真实状态最为接近。最为接近可以理解为k时刻的真实状态与k时刻的估计状态之间的误差二范平方和最小，也就等价于协方差矩阵的迹最小。可以表示为：

在这种情况下求取的卡尔曼增益称为最优卡尔曼增益。求取的过程就是直接上式对卡尔曼增益求一阶导数。
更新过程的第二步，就是计算卡尔曼增益：

下面给出推导过程：


最优卡尔曼增益计算出来之后，我们发现在最优卡尔曼增益情况下可以对后验误差协方差矩阵进行简化。第三步就是计算在最优卡尔曼增益下的后验误差协方差矩阵：

推导过程：

4、总结
卡尔曼在滤波器的推导过程就已经完成了，下面我们再将它们整合到一起，有个更清晰的认识：

将这些方程做成框图：

从中可以看到，只要对初始状态进行设定，卡尔曼滤波器就可以完成迭代了。
下面的图是我用白巧克力亦唯心提到的匀加速的例子做了三张GIF来动态展示这个过程，以示其效果。

图2 理论值

图3 测量值

图4 卡尔曼滤波结果
该卡尔曼滤波器是从第10个时间步才开始测量的，之前保持为0。
二、扩展卡尔曼滤波器（EKF）
EKF只是在KF的基础之上改变了状态转移函数和测量函数，从而将卡尔曼滤波器的线性算子变为非线性算子。下面只给出与KF不同的地方的公式推导。其他部分参考KF的推导。


可以看出，EKF依然是在KF的框架内进行的改进，所以思路与KF是完全一致的。只是与的计算方法不同。
下面给出推导过程。


到这一步，近似最优卡尔曼增益的计算就与KF的推导过程完全一致了，在此不再赘述。
只是需要注意一下的是：此处为什么是近似最优卡尔曼增益而不是最优卡尔曼增益。这是因为计算与的时候理论上应该计算函数f和h的雅可比矩阵。但是实际操作起来非常困难，特别是对于一些复杂的非线性系统。因此往往采用泰勒展开去一阶线性的部分。由于近似，得到的卡尔曼在增益也就不是最优卡尔曼增益，而是近似最优卡尔曼增益。这就直接导致了EKF在高度非线性系统下性能锐减的必然结果。而且系统初始状态估计错误或者说建模不正确，EKF也会迅速发散。所以在第三部分介绍的UKF则避免了求取函数的雅可比矩阵，从而提高了滤波器的性能和鲁棒性。
三、无向卡尔曼滤波器（UKF）
UKF依然没有脱离KF的框架。只不过对下一时刻状态的预测方法变成了sigma点集的扩充与非线性映射。这样做有两个优点：1、避免了复杂非线性函数雅可比矩阵的复杂运算；2、保证了非线性系统的普遍适应性。此外，由于高斯分布sigma点集的扩展，使高斯分布的噪声得到抑制。
预测过程：


更新过程：


在准确建模的前提下，KF，EKF和UKF都有不错的表现。但是对于很多复杂的系统而言，建模就是一个复杂的问题。如果模型参数没办法准确估计，那么卡尔曼滤波器的应用就会受到限制。在不知道模型参数的情况下，可以通过蒙特卡洛采样，特别是粒子滤波的方法来对参数进行估计。这也是笔者继续研究的方向。以上内容，仅供参考。限于水平，难免纰漏。如有不妥之处，还请告知。

# 欢迎使用Markdown编辑器

你好！ 这是你第一次使用 Markdown编辑器 所展示的欢迎页。如果你想学习如何使用Markdown编辑器, 可以仔细阅读这篇文章，了解一下Markdown的基本语法知识。

新的改变

我们对Markdown编辑器进行了一些功能拓展与语法支持，除了标准的Markdown编辑器功能，我们增加了如下几点新功能，帮助你用它写博客：

1. 全新的界面设计 ，将会带来全新的写作体验；
2. 在创作中心设置你喜爱的代码高亮样式，Markdown 将代码片显示选择的高亮样式 进行展示；
3. 增加了 图片拖拽 功能，你可以将本地的图片直接拖拽到编辑区域直接展示；
4. 全新的 KaTeX数学公式 语法；
5. 增加了支持甘特图的mermaid语法¹ 功能；
6. 增加了 多屏幕编辑 Markdown文章功能；
7. 增加了 焦点写作模式、预览模式、简洁写作模式、左右区域同步滚轮设置 等功能，功能按钮位于编辑区域与预览区域中间；
8. 增加了 检查列表 功能。

功能快捷键

撤销：Ctrl/Command + Z
重做：Ctrl/Command + Y
加粗：Ctrl/Command + B
斜体：Ctrl/Command + I
标题：Ctrl/Command + Shift + H
无序列表：Ctrl/Command + Shift + U
有序列表：Ctrl/Command + Shift + O
检查列表：Ctrl/Command + Shift + C
插入代码：Ctrl/Command + Shift + K
插入链接：Ctrl/Command + Shift + L
插入图片：Ctrl/Command + Shift + G

合理的创建标题，有助于目录的生成

直接输入1次#，并按下space后，将生成1级标题。
输入2次#，并按下space后，将生成2级标题。
以此类推，我们支持6级标题。有助于使用TOC语法后生成一个完美的目录。

如何改变文本的样式

强调文本 强调文本

加粗文本 加粗文本

标记文本

删除文本

引用文本

H₂O is是液体。

2¹⁰ 运算结果是 1024.

插入链接与图片

链接: link.

图片:

带尺寸的图片:

居中的图片:

居中并且带尺寸的图片:

当然，我们为了让用户更加便捷，我们增加了图片拖拽功能。

如何插入一段漂亮的代码片

去博客设置页面，选择一款你喜欢的代码片高亮样式，下面展示同样高亮的 代码片.

// An highlighted block
var foo = 'bar';

生成一个适合你的列表

• 项目
  • 项目
    • 项目

1. 项目1
2. 项目2
3. 项目3

• 计划任务
• 完成任务

创建一个表格

一个简单的表格是这么创建的：

项目	Value
电脑	$1600
手机	$12
导管	$1

设定内容居中、居左、居右

使用:---------:居中
使用:----------居左
使用----------:居右

第一列	第二列	第三列
第一列文本居中	第二列文本居右	第三列文本居左

SmartyPants

SmartyPants将ASCII标点字符转换为“智能”印刷标点HTML实体。例如：

TYPE	ASCII	HTML
Single backticks	'Isn't this fun?'	‘Isn’t this fun?’
Quotes	"Isn't this fun?"	“Isn’t this fun?”
Dashes	-- is en-dash, --- is em-dash	– is en-dash, — is em-dash

创建一个自定义列表

Markdown

Text-to- HTML conversion tool

Authors

John

Luke

如何创建一个注脚

一个具有注脚的文本。²

注释也是必不可少的

Markdown将文本转换为 HTML。

KaTeX数学公式

您可以使用渲染LaTeX数学表达式 KaTeX:

Gamma公式展示 $\Gamma(n) = (n-1)!\quad\forall n\in\mathbb N$ 是通过欧拉积分

$\Gamma(z) = \int_0^\infty t^{z-1}e^{-t}dt\,.$

你可以找到更多关于的信息 LaTeX 数学表达式here.

新的甘特图功能，丰富你的文章

• 关于 甘特图 语法，参考 这儿,

UML 图表

可以使用UML图表进行渲染。 Mermaid. 例如下面产生的一个序列图：:

这将产生一个流程图。:

• 关于 Mermaid 语法，参考 这儿,

FLowchart流程图

我们依旧会支持flowchart的流程图：

• 关于 Flowchart流程图 语法，参考 这儿.

导出与导入

导出

如果你想尝试使用此编辑器, 你可以在此篇文章任意编辑。当你完成了一篇文章的写作, 在上方工具栏找到 文章导出 ，生成一个.md文件或者.html文件进行本地保存。

导入

如果你想加载一篇你写过的.md文件或者.html文件，在上方工具栏可以选择导入功能进行对应扩展名的文件导入，
继续你的创作。

---

1. mermaid语法说明 ↩︎

2. 注脚的解释 ↩︎

这段时间做轴承故障诊断和预测的时候，需要一个针对已经获取了特征向量的工具来对轴承故障状态进行估计和预测。卡尔曼滤波器可以实现对过去、当前和未来目标位置的估计，所以想通过卡尔曼滤波器的设计思路找到一些灵感。虽然最后发现：卡尔曼滤波器中的状态量是有具体的物理含义的物理量，而表征轴承故障状态的量只是一种表征量。这两者之间存在着本质的差别，因为轴承的退化过程目前为止还不能建模。虽然如此，我还是想将卡尔曼滤波器详细的推导过程分享给大家。
学习过程中，参考了白巧克力亦唯心的文章：卡尔曼滤波–从推导到应用（一），卡尔曼滤波–从推导到应用（二），在此给出直达连接。他/她的文章有故事，有推导，有例子，有代码，很优秀，向大家推荐。我在这篇文章里希望把很多文章中语焉不详的推导，以及符号的定义阐释清楚，并加入一些自己的理解，希望和大家分享。第一次写博客，markdown编辑器的用法还不熟悉，公式编辑会花去很多时间。所以直接用图片代替了，希望大家能够理解，以后我会慢慢按照标准的格式来编辑公式的。
一、基本卡尔曼滤波器（BKF）
卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。关于这种滤波器的论文最早由Swerling(1958)发表了这种想法。
1、基本动态模型
假设1：k时刻的真实状态是从k-1时刻的真实状态演化而来；
假设2：演化与测量的过程由线性算子来描述。
2、两个基本的方程
四个状态值的定义：

状态转移方程：

状态测量方程：

基于k-1时刻状态对k时刻状态的估计值与真实值之间的差称为估计误差，该估计误差的协方差矩阵的定义为后验估计误差协方差矩阵，用下式表示：

其模型拓扑结构用隐马尔科夫链可以表示成图1（维基百科:卡尔曼滤波）这样：

图1 卡尔曼滤波的隐马尔可夫链式模型
3、两个基本的过程
3.1 预测过程


从公式（3）*推导公式（2）：

3.2 更新过程
我们基于k-1时刻对k时刻状态的估计是否正确，需要用与实际测量值之间的误差来衡量，并且考虑用这个误差来补偿。所以在更新之前，我们应该计 算实际测量值与估计输出值之间的差值及其协方差矩阵。

（3）式的协方差矩阵表示为：

下面推导公式（4）：

然后我们再进行更新的步骤。更新是指：由基于k-1时刻对k时刻状态的估计值应当如何得到k时刻的估计值。卡尔曼的思想就是：用基于k-1时刻对k时刻状态的估计值与预测输出值和实际输出值之间的差进行线性组合得到k时刻的估计值，连接这两者的就是卡尔曼增益。这里体现的就是反馈的思想，更新过程的第一步用下式表示：

那么现在问题来了，如何求取这个卡尔曼增益呢？
这时候我们应该回到我们的出发点，我们希望的是滤除干扰真实状态的噪声，是滤波器的估计状态与真实状态最为接近。最为接近可以理解为k时刻的真实状态与k时刻的估计状态之间的误差二范平方和最小，也就等价于协方差矩阵的迹最小。可以表示为：

在这种情况下求取的卡尔曼增益称为最优卡尔曼增益。求取的过程就是直接上式对卡尔曼增益求一阶导数。
更新过程的第二步，就是计算卡尔曼增益：

下面给出推导过程：


最优卡尔曼增益计算出来之后，我们发现在最优卡尔曼增益情况下可以对后验误差协方差矩阵进行简化。第三步就是计算在最优卡尔曼增益下的后验误差协方差矩阵：

推导过程：

4、总结
卡尔曼在滤波器的推导过程就已经完成了，下面我们再将它们整合到一起，有个更清晰的认识：

将这些方程做成框图：

从中可以看到，只要对初始状态进行设定，卡尔曼滤波器就可以完成迭代了。
下面的图是我用白巧克力亦唯心提到的匀加速的例子做了三张GIF来动态展示这个过程，以示其效果。

图2 理论值

图3 测量值

图4 卡尔曼滤波结果
该卡尔曼滤波器是从第10个时间步才开始测量的，之前保持为0。
二、扩展卡尔曼滤波器（EKF）
EKF只是在KF的基础之上改变了状态转移函数和测量函数，从而将卡尔曼滤波器的线性算子变为非线性算子。下面只给出与KF不同的地方的公式推导。其他部分参考KF的推导。


可以看出，EKF依然是在KF的框架内进行的改进，所以思路与KF是完全一致的。只是与的计算方法不同。
下面给出推导过程。


到这一步，近似最优卡尔曼增益的计算就与KF的推导过程完全一致了，在此不再赘述。
只是需要注意一下的是：此处为什么是近似最优卡尔曼增益而不是最优卡尔曼增益。这是因为计算与的时候理论上应该计算函数f和h的雅可比矩阵。但是实际操作起来非常困难，特别是对于一些复杂的非线性系统。因此往往采用泰勒展开去一阶线性的部分。由于近似，得到的卡尔曼在增益也就不是最优卡尔曼增益，而是近似最优卡尔曼增益。这就直接导致了EKF在高度非线性系统下性能锐减的必然结果。而且系统初始状态估计错误或者说建模不正确，EKF也会迅速发散。所以在第三部分介绍的UKF则避免了求取函数的雅可比矩阵，从而提高了滤波器的性能和鲁棒性。
三、无向卡尔曼滤波器（UKF）
UKF依然没有脱离KF的框架。只不过对下一时刻状态的预测方法变成了sigma点集的扩充与非线性映射。这样做有两个优点：1、避免了复杂非线性函数雅可比矩阵的复杂运算；2、保证了非线性系统的普遍适应性。此外，由于高斯分布sigma点集的扩展，使高斯分布的噪声得到抑制。
预测过程：


更新过程：


在准确建模的前提下，KF，EKF和UKF都有不错的表现。但是对于很多复杂的系统而言，建模就是一个复杂的问题。如果模型参数没办法准确估计，那么卡尔曼滤波器的应用就会受到限制。在不知道模型参数的情况下，可以通过蒙特卡洛采样，特别是粒子滤波的方法来对参数进行估计。这也是笔者继续研究的方向。以上内容，仅供参考。限于水平，难免纰漏。如有不妥之处，还请告知。