卡尔曼滤波进阶学习

卡尔曼滤波进阶学习

概率知识：

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

这里只记录比较重要的几个公式:

全概率公式：

贝叶斯公式：

具有条件的全概率公式:

具有条件的贝叶斯公式：

推导过程如下：

多元高斯分布：

一维高斯分布的概率密度函数：

图形就是中间高两边低的bell形状。

二维高斯分布的概率密度函数：

图形如下：

二维高斯分布从ｘ方向和ｙ方向看（向ｘ方向和ｙ方向分别作投影），可以得到两个一维高斯分布：

个人经验分享：

高斯分布看着很吓人，那个PDF公式长成那个样，不好记忆。但其实很简单，一维高斯分布只有两个参数决定：均值和方差。

只要记住这三点，一维高斯分布就可以熟练使用了。熟悉了一维高斯分布之后，二维高斯分布慢慢也就清晰明了了。

多元高斯分布，就是将维度扩展到多维。公式可以写成：

给出几个例子，感受一下不同的均值与协方差矩阵对多元高斯分布的影响：

贝叶斯滤波：

值得注意的一点是：贝叶斯滤波是递归的算法，即下一时刻的结果，要建立在前一时刻结果的基础上。

扩展卡尔曼滤波(EKF)：

EKF是用于处理转移方程和观测方程都是非线性变换时的方法。

核心思想是：线性化。用泰勒展开的一阶导来近似转移方程和观测方程。

线性化：

首先介绍一个概念：泰勒展开与线性化。

学过高等数学（微积分）的同学，可能都知道泰勒展开。

就是任意一个函数都可以用它的导数（一阶导/二阶导/三阶导…）来表示出来。

将函数进行泰勒展开，变成级数的形式，会发现一个有趣的现象：等号右边的级数，越往后，级数项越小。后面的级数项，可以当作一种误差。那么“取前三项代替原函数”产生的误差，要比“取前五项代替原函数”产生的误差大的多。取前五项代替原函数，产生的误差虽然小，但是其计算量却要比”取前三项“的计算量大很多。

泰勒公式的用途：任何一个函数，都可以用其导数的级数来代替。现实生活中，原函数分析起来比较困难。将其进行泰勒展开，取其泰勒展开的前几项来代替原函数进行分析。

例如：ln(1+x)这个函数分析起来比较困难，将其进行泰勒展开，取其前三项，就变成了ｘ－(1/2)ｘ^2　+　(1/3)x^3，这时进行分析就变得容易些。这种方法在科研工程中非常常见。

线性化：就是将原函数用泰勒展开，保留一阶导，舍弃高阶导（二阶导/三阶导/…）

雅可比矩阵：

要进行线性化处理，就要求一阶导。多元函数的一阶导，可以用雅可比矩阵表示。

EKF原理：

在EKF中，考虑的是转移方程和观测方程都是非线性的情况。

因此要对转移变换和观测变换进行线性化处理，即对这两个变换分别进行泰勒展开，保留一阶导，舍弃高阶导。

当然这会造成一定的误差，因此只有在目标函数的曲率较小的地方（或者较小的一段邻域内），进行线性化处理，才会有好的效果。

无迹卡尔曼滤波UKF:

EKF和UKF都是用于处理非线性变换的方法，但二者的思想完全不同。

EKF是将转移方程和观测方程进行线性化处理。 而UKF是用采样的方式，来近似概率分布，并不对转移方程和观测方程进行处理。

UKF提出无迹变换，来进行采样，并计算每个采样点的权重。转移方程和观测方程依旧是非线性变换。

无迹变换：

听着听玄乎，其实也很简单。

就是在非线性变换前进行采样，得到采样点X（称为Sigma点），并计算每个采样点的权重W。然后将采样点进行非线性变换F(X), 得到结果Y，然后再对结果Y进行加权处理，得到最终的结果WY

这里面的难点，就在于：如何采样，如何计算Sigma点的权重。

根据采样规则和权重计算上的不同，无迹变换可分为两种：一般形式的无迹变换，比例无迹变换。后者是前者的改进。

一般形式的无迹变换：

采样方式：在均值mean附近，再对称采样2n个点，构成2n+1个采样点。

引入一个外部参数K，用于表示 sigma 点相对均值的散布程度 。K越大，非均值处的 sigma 点距离均值越远，且所占权重越小, 而均值处 sigma 点所占权重则相对越 大。

权重的计算，采用以下公式：

比例无迹变换：

比例无迹变换，同样采样2n+1个点。但却引入4个外部变量：α、β、κ 和 λ。

参数 α 和 κ 为确定 sigma 点分布在均值多远的范围内的比例参数。

α 满足 10−4≤α≤1，为避免强非线性系统中的非局部效应问题，α 通常取一个较小值；

κ满足 κ≥0，通常取 κ=3−n 或 κ=0。

下图呈现了当 α 取值分别为 0.30.3 和 11 时，sigma 点的分布情况，从图中可以发现，α 取值越大，非均值处的 sigma 点距离均值越远。

参数 β 用于引入随机变量概率分布的高阶矩信息，如果分布是精确的高斯分布，则 β=2 是最优选择。

参数 λ满足：

sigma 点同样位于均值处及对称分布于主轴的协方差处。

采样规则如下：

权重计算如下：

我们可以看到：比例无迹变换，对权重进行了精细化处理，分别计算了均值的权重和方差的权重。因此也引入了更多的参数。但核心思想还是一样的。

UKF原理：

就是将无迹变换插入到卡尔曼滤波的公式中。

粒子滤波PF：

蒙特卡洛：

用采样的方法，求某个参数。

求出pi

下面一张图中，存在一个半半径为1的一个圆。该圆的面积为

我们想要知道pi的具体值，就可以用蒙特卡洛采样的方法。

向图中撒豆子，统计掉入圆内的豆子数。

可以利用以下公式，来求出pi：

当豆子数足够多时，pi的值也就越加准确。

求定积分

下图中，图中的曲线为f(x). 我们想要求解定积分：

我们知道定积分表示曲线下的面积。因此我们同样可以蒙特卡洛的方法来求解。

在ab区间内画一个矩形。矩形的高为c。因此，该矩形的面积为(b-a)*c.

向该区域内撒豆子，统计落入ab区间内，f(x)曲线下的豆子数。

最终，我们就可以得到定积分的值：

马尔可夫链：

马尔可夫链（马尔可夫过程），其实就一个思想：

马尔可夫：

隐马尔可夫：E是Evidence的缩写，表示可以直接观测的量。X表示隐藏的量，不是直接得到。

重要性采样IS：

蒙特卡洛积分，是向图中均匀的撒豆子，当豆子数（样本数T）足够大时，算出的面积（积分）也就越准确。

但是当样本数T变大是，计算量也变大了。

在样本总数保持一致时，有没有什么好的改进方法？这就引出了重要性采样。

越重要的地方，采样的次数就越多一些；越不重要的地方，采样的次数就少一些。

那么，我们也不知道原函数的概率分布p(x), 我们怎么知道哪里重要（哪里概率大），哪里不重要（哪里概率小）？

这就要引出另外一个人为选定的概率分布q(x). 我们用这个已知的概率分布q(x), 来估计未知的p(x) 。

例如，x~p(x), 我们想要知道f(x)的均值，那么可以按照以下的公式进行：

如果我们知道x的分布p(x)，那么，就可以按照x的分布p(x)，采样出多个样本x_1, x_2,…

此时，上面的式子可以改变为：

但是我们不知道实际的分布p(x), 因此我们引入一种已知的分布q(x), 此时f的期望可以写成：

那么问题的关键在于：分布q(x)如何选取+重要性权重w来如何得到。这就引出了下一节SIS的内容。

序列式重要性采样SIS：

由于分布p(x)未知，所以重要性权重w也就未知。

我们想要找到一个递推公式，求得w(t)与w(t-1)的关系。所以叫做序列式重要性采样。

最终，我们找到了这样的递推公式（推导过程暂略）：

此时，我们就解决了前一时刻的权重递推后一时刻权重的问题。

但我们依旧不知道分布q(x).

Condensation Filter：

这一小节，给出了分布q(x):

此时，权重递推公式就可以改写成：

具体过程略，有空再写。

重采样Resampling：

由于存在“粒子退化”问题，因此就需要进行重采样。

粒子退化:实际上就是在用粒子的位置和权重表示一个分布时，有几个粒子的权重接近于1,而剩余粒子的权重非常接近于0.这样仅有少数粒子是有效的，那么这种有效粒子减少的现象，就是粒子退化现象

如上图，原本有3个样本，权重分别为：0.1，0.1，0.8.

根据权重，进行重采样后，得到了10个样本。

PF整体流程：

其实核心就是下面这张图片，只不过具体实现上会涉及到：IS，SIS，Condensation Filter，Resampling等等