卡尔曼滤波

本文参考：从放弃到精通！卡尔曼滤波从理论到实践~_哔哩哔哩_bilibili

1.卡尔曼滤波入门

卡尔曼滤波的引入：

滤波就是将测量得到的波形中的的噪声过滤掉，使得到的数据更趋于真实情况，也更加平滑，方便使用。如下图所示（红色曲线是测量直接得到的波形，紫色曲线是滤波后得到的平滑曲线）

卡尔曼滤波适用的系统：

卡尔曼滤波适用线性高斯系统

1.线性系统：满足叠加性和齐次性

叠加性：

齐次性：

2.高斯系统

高斯：噪声满足正态分布

宏观意义：

滤波即加权

理想状态：信号 $\times$ 1+噪声 $\times$ 0

卡尔曼滤波：估计值 $\times$ 估计值权重+观测值 $\times$ 观测值权重

2.学卡尔曼滤波的必备知识

2.1.状态空间表达式

$x_{k}$ ：当前状态的值；

$x_{k-1}$ ：上一个时刻该状态的值；

$u_{k}$ ：输入，即给到 $x_{k}$ 的一个输入；

$w_{k}$ ：过程噪声

A：状态转移矩阵，代表当前状态上一时刻的值乘上A这种关系，作用在该系统

B：控制矩阵，输入乘上B这种关系，作用在该系统

举例：一辆小车以速度v在路上行驶，迎面而来的风速v1

那么状态方程与该情形对应关系如下

$x_{k}$ ：当前状态的值；

C： $x_{k}$ 乘上某种关系得到要观测的值

$V_{k}$ ：观测噪声，和观测器的误差有关

举例：火炉对水加温，蓝色点是水温的状态值，红色的点是温度计，温度计是一个传感器得到观测值

那么状态方程和观测方程与该情形对应关系如下

方框图表示如下所示

2.2.高斯分布

高斯分布直观图解：

二维中，所有值投影到x轴或y轴，都是正态分布

高斯分布参数分析：

$w_{k}$ 是过程噪声，符合正太分布，均值为0，方差为 $Q_{k}$

$V_{k}$ 是观测噪声，符合正太分布，均值为0，方差为 $R_{k}$

统称 $w_{k}$ 和 $V_{k}$ 为高斯白噪声

举例1：GPS检测到小车的位置，检测到小车开了1000米，但是GPS有精度误差，因此要加上 $\delta$ 米的噪声，该噪声就是观测误差， $V_{k}$ = $\delta$ 。因为 $\delta$ 是服从正态分布的，假设其方差为1，则此时 $R_{k}$ =1

举例2：有一个滑板，滑板前进的速度是5m/s，由于风的作用，因此滑板的速度有一个噪声 $\delta$ ，此时滑板的速度为v+ $\delta$ ，那么 $w_{k}$ = $\delta$ ， $\delta$ 是符合正太分布的，假设 $\delta \in N(0,1)$ ，则 $Q_{k}$ =1

2.3.方差

一维方差：

（1）噪声的方差：前面的 $Q_{k}$ 和 $R_{k}$

（2）状态的方差：后面会提到状态估计值 $\hat{x}_{t}^{-}$ 的方差,

举例：小车实际跑了10米，但是由于噪声的影响，小车跑的距离是一个范围，服从正太分布，此时小车跑的距离就是10+ $w_{k}$ （噪声），该距离符合正态分布，因此小车跑的距离本身也会有一个方差。

二维协方差：cov(x,y)

一维的状态估计值 $\hat{x}_{t}^{-}$ 为一个值；

二维的 $\hat{x}_{t}^{-}$ 就是一个矢量，有两个不同的状态，如下图所示。这两个不同的状态同时对应着不同的噪声。

对于二维的 $\hat{x}_{t}^{-}$ ，其协方差就为

多维协方差矩阵C：

2.4.超参数

实验中，需要自己调来提高系统性能的参数：

卡尔曼滤波器中：Q（过程噪声的方差） R（观测噪声的方差）

PID控制器中：P I D

2.5.卡尔曼直观图解

横轴为车的位置，竖轴为概率密度。

$\hat{x}_{k}$ ：状态量旁边没有横杠就是最优估计值（修正值、后验估计值）

$\hat{x}_{k}^{-}$ ：状态量旁边有横杠就是预测值（先验估计值）

$y_{k}$ ：观测值，也就是传感器直接测量得到的值

图解：

（1） $\hat{x}_{k-1}$

$\hat{x}_{k-1}$ 是上一时刻的最优估计值，也就是上一时刻卡尔曼滤波最终输出的值

（2） $\hat{x}_{k}^{-}$ 和 $y_{k}$

$\hat{x}_{k}^{-}$ 是基于上一时刻最优估计值 $\hat{x}_{k-1}$ 估计出来的当前时刻的预测值

$y_{k}$ 是当前时刻的观测值

（3） $\hat{x}_{k}$

$\hat{x}_{k}$ 是当前时刻的最优估计值，通过当前时刻预测值 $\hat{x}_{k}^{-}$ 和当前时刻观测值 $y_{k}$ 推出，上图可以看出 $\hat{x}_{k}$ 方差比较小，是因为经过卡尔曼滤波后，最终推出的 $\hat{x}_{k}$ 效果较好

3.卡尔曼滤波

3.1.卡尔曼公式理解

实现过程：使用上一时刻的最优结果预测这一时刻的预测值，同时使用这一时刻观测值（传感器测得的数据）修正这一时刻预测值，得到这一时刻的最优结果

注：当状态值是一维的时候，H和I可以看作是1

预测：

1.上一时刻的最优估计值，推出这一时刻的预测值：

2.上一时刻最优估计值方差/协方差和超参数Q推出这一时刻预测值方差/协方差

深入理解，Q其实对应的是过程噪声的方差

更新：

1.这一时刻预测值方差/协方差和超参数R推出****卡尔曼增益 $K_{t}$

$K_{t}$ 为卡尔曼增益

因为 $P_{t}^{-}$ 是和Q有关的，所以将 $P_{t}^{-}$ 的公式带入可以推出卡尔曼增益 $K_{t}$ 是和Q和R都有关的

深入理解，R其实对应的是观测噪声的方差

2.这一时刻预测值、这一时刻观测值、卡尔曼增益推出这一时刻最优估计值

$Z_{t}$ 为这一时刻观测值

3.这一时刻预测值方差/协方差、卡尔曼增益推出这一时刻最优估计值方差/协方差

举例：

小车有两个状态值，p代表位置，v代表速度。小车处于匀加速直线运动

预测模型：

1.上一时刻的最优估计值，推出这一时刻的预测值：

2.上一时刻最优估计值方差/协方差和超参数Q推出这一时刻预测值方差/协方差

推导：

因为

所以

其实第一步中这一时刻预测值后面应该还有一个过程噪声Wt要加上（前面省略了），如下图所示，这里面Q是前面省略的过程噪声Wt的方差

测量模型：

GPS测量只能测量小车的位置，不能测量小车的速度，所以

Zp是小车此时的真实位置

Zv是小车此时的真实速度

Pt是测量得到的小车此时的位置

$\Delta$ Pt是GPS测量位置的误差

更新模型：

1.这一时刻预测值方差/协方差和超参数R推出卡尔曼增益 $K_{t}$

前面测量模型中，因为无法测速度，测量相当于是一维的，一维时H看作为1

2.这一时刻预测值、这一时刻观测值、卡尔曼增益推出这一时刻最优估计值

3.这一时刻预测值方差/协方差、卡尔曼增益推出这一时刻最优估计值方差/协方差

预测更新循环往复，就能得到每一时刻的最优估计值

3.2.调节超参数

3.2.1.Q和R的取值

当F=1且一维的情况H=1，那么

再结合这一时刻最优估计值的公式，如下：

可以得出：

当我们更信任观测值时，那么应该让卡尔曼增益K增大；从K的公式中可以看出，R越小K越大，Q越大K越大

当我们更信任模型估计值时，那么应该让卡尔曼增益K减小；从K的公式中可以看出，R越大K越小，Q越小K越小

结论：

当我们更信任模型估计值时（模型估计基本没有误差），那么应该让K小一点，我们应该将R取大一点，Q取小一点

当我们更信任观测值时（模型估计误差较大），那么应该让K大一点，我们应该将R取小一点，Q取大一点