卡尔曼滤波入门学习

卡尔曼滤波入门学习

本文记录卡尔曼滤波的相关原理和个人心得，有些地方未作详细展开，纯个人技术笔记积累.

首先说一下个人的理解，卡尔曼滤波是干什么的？这个算法的提出原因是什么？

卡尔曼滤波，是用来评估“估计值和测量值哪个更可信”这个问题的。

程序推导出来的值，和实际测量的值，这两个都存在一定误差的情况下，如何获得一个更精确的数据？

卡尔曼滤波，正是用来解决这一问题的，所以在实际工程中，尤其是对精度要求非常高的项目（比如航天的项目，自动驾驶的项目等等），非常常见且适用。

以前一个同事问过：为什么叫卡尔曼滤波，怎么就滤波了呢？

个人理解是：滤波往往是指过滤噪点，或者将某个频率的信息过滤掉。而这个算法处理的是测量工具不准确，含有噪声存在的案例，通过这个算法，使得我们得到了更精确的值，所以叫滤波。然后以提出者（卡尔曼）这个人来命名，所以就叫做“卡尔曼滤波”了。

附赠自动驾驶最全的学习资料和量产经验：链接

问题的引入：

任何一个算法或知识点，肯定是有其适用场景，搞清其目的很重要。

看一个身边的例子：

比如地上有一筐苹果，你想知道一共有多少斤。你会怎么做？找一个秤来称一下。

称一次，你觉得不太准确，又称了10次，做个平均，就把这筐苹果的重量得到了。

这是现实生活中，非常常见的事情，每天都有类似的事情发生。

那么现在有人告诉你，你用的那个秤之前被人摔坏了，不是很准，但你只有这一个秤，你该怎么办？

这就是卡尔曼滤波这个算法的适用场景：测量值存在误差，我们该怎么处理。

想要理解卡尔曼滤波，就一定要区分“估计值和测量值（或者叫预测值和观测值）”, “估计误差与测量误差（或者叫预测误差和观测误差）”这几个概念。

协方差矩阵：

首先，介绍一个前置知识点：协方差矩阵（包含方差，协方差）

方差，代表着数据的波动情况。数据越稳定，方差越小。

在科研和工程中，当我们想要知道某一个真实的值时，我们会用某个工具反复测量，测量值通常会在真实值附近。我们希望测量值的方差越小越好。（方差越小，说明数值越稳定）

比如说上面的这张图片，真实值是图中的黑点（0，0）处，我们用两个工具来测量这个值。

第一个工具的测量结果是红点的分布，第二个工具的测量结果是绿点的分布。

哪个工具更好一点？当然是第一个工具，因为每次测量，红点都在黑点附近，偏离黑点小（数学语言描述是：红点的方差小）。

而卡尔曼滤波的最终优化思想，就是希望误差的方差达到最小。

再说一个数学技巧：用矩阵求协方差矩阵

具体原因见知乎文章：协方差矩阵与相关性分析

测量一个数据：

a. 用多次测试值来估计真实值：取平均数

至此，我们得到了一个很重要公式（我再来写一遍）：

我们要明确自己的需求：x表示估计值（这是我们一直想要知道的），z表示测量值。上面的公式，给出了一种本次结果与上次结果的约束关系：中间相差一个系数乘法式子。

之所以要设立这样的数学指标，是因为要：比较估计误差和测量误差。

逻辑就是：哪个误差小，我们就相信哪个。

至此可以总结一下计算步骤：三步

第一步：计算卡尔曼增益（Kalman Gain）

第二步: 计算当前k时刻估计值

第三步：更新此时（k时刻）的估计误差

看一个例子（数据融合）：

用两个秤来称一个东西，两个秤都有一定的误差。

最终我们看到，利用的思想是使得方差达到最小（方差越小，代表着数据越稳定，即数据越精确）。

在数学中求极值，就是令导数为0（这是很常见的数学技巧）。

这里面涉及一个问题：为什么是对k求导，令其为0?

原因是：k可以看成一个变量。我们就是想要知道k取何值时，方差能够达到最小，所以才对k求导。

状态空间方程：

这个方程组如何理解呢？

是这样的：

以上两个方程，除了w和v这两个噪声外，其他矩阵变量都是已知的，在实际工程和科研中，会明确给出。（状态之间如何变换以及人为控制，肯定是要明确给出的）

至此，我们得到了两种x值：模型推导出来的先验值，和测量出来的测量值。

利用前面介绍的公式：

将现在得到的两种x值，代入其中，可以得到：

我们看一下这个系数G有什么影响：

而一些教材上，取G=KH, 卡尔曼滤波的公式，就变成了

目标：寻找合适的K, 使得上面公式中得到x最接近于真实值。

（真实值，我们并不知道，最终的优化目标转化成了：使得误差的方差达到最小）

首先定义出误差：

我们最终是要使误差的方差最小，即上面的协方差矩阵P的迹最小.

首先，要记住一些常用的矩阵公式（4个）：

拿其中的一个矩阵公式，进行验证：第三个

误差的协方差矩阵P，化简过程为

至此，卡尔曼增益的所有式子就给出来了。

总结：

卡尔曼滤波一共有5个公式，梳理一下思路：

这５个公式可以分为２部分：预测部分＋矫正部分

对比一下前面的状态空间方程的内容，加深对上面５个公式的理解：

一定要清楚上面５个公式中每个变量的含义，以及过程噪声的协方差Q和测量噪声的协方差R都用在了哪里。

总的思路：

后续在此基础上又出现了扩展卡尔曼滤波，无迹卡尔曼滤波等。以后再研究吧 。

个人体会：

搞清卡尔曼滤波的引入问题很重要，哪里会用它。

卡尔曼滤波一共有5个公式，理解公式中每个变量的物理含义很重要。

有一些数学技巧在里面：

1. 真实值我们并不知道。为了求真实值，转化问题为：使得方差最小。

2. 为了求极值（或最值），使得导数为0。

3. 将协方差矩阵中的方差最小这一目的，转化为求矩阵的迹trace最小。（这点在图像领域中角点检测里面，也有一样的应用）

4. 本算法中时希望方差越小越好，而在某些算法中，是希望方差越大越好。比如PCA算法做主成分分析。

5. 前面引出的卡尔曼增益的计算公式（估计误差与测量误差的比较），与非线性优化算法中的Levenberg-Marquardt算法中的Trust Region有点相似。这可能就是一些数学大佬设计公式时，描述事物发展所培养出来的科研直觉。

本文还有一些知识点：

1. 扩展卡尔曼滤波，无迹卡尔曼滤波等等

2. 卡尔曼滤波与贝叶斯滤波，粒子滤波等区别与联系。

3. 相关系数为1，代表着什么？