卡尔曼滤波

最新推荐文章于 2022-12-06 11:25:20 发布

tim514

最新推荐文章于 2022-12-06 11:25:20 发布

阅读量2k

点赞数

分类专栏：图像算法文章标签：算法人工智能

本文链接：https://blog.csdn.net/tim514/article/details/121502930

版权

图像算法专栏收录该内容

6 篇文章 2 订阅

订阅专栏

牛逼的算法往往来源于一个简单的思想 + 演化过程。

1. 卡尔曼滤波是什么?

卡尔曼滤波用于线性系统线性系统，对系统的输出进行最优估计（预测）的算法，因为系统的输入（观测量）存在噪声与干扰，会对整个输出结果带来累积误差与偏差，我们最终的结果也相当于对这些干扰进行滤波的过程。

根据此前的用观测值修正系统状态的思想，以最小化预测系统的均方误差为目标。

2. 卡尔曼滤波的用途？

应用：制导与导航系统，计算机视觉系统与信号处理，sensor的误差估算来得到实际测量值。

卡尔曼滤波一般用于优化估算（预测）一些无法直接测量，但是可以通过其他的量间接来估算的量。

3. 状态控制的知识准备。

举个栗子1：观测你弟弟的心情。

1. 你发现你弟弟面无表情，现在你想知道你弟弟的心情如何？

2.直接看不出来，所以你拿了一块饼干给了他，然后观察他的面部表情与动作。

状态观测器：帮助你估计你无法直接查看或者测量的内容。

：表示估计状态（estimated state）。

举个栗子2：估计发动机internal temperture。

状态观测器：我们可以选择控制器的增益K，来控制误差函数的衰减率。误差消减的越快，（估算状态）到（真实状态）速度就越快，模型收敛的也就越快。

增益K选择就是依靠（卡尔曼滤波）。

4. 卡尔曼滤波的应用。

在运动过程中会引人过程误差，系统会同时存在测量误差与过程误差，卡尔曼滤波就是

状态误差： V ~（0， R）（0：均值为0， R：协方差为R），V服从（0，R）的高斯分布。

其中：协方差R等于测量误差的方差

举个栗子3：估算汽车的position。

其中： $\widehat{x}_{k-1}$ : 上一时刻估计值，根据高斯分布，我们可以知道汽车大概率在avg附近。

$\widehat{x}_{k}$ ：当前时刻估计值，由于引入过程误差，所以当前时刻的估计值相比上一时刻不确定性更大，所以 $\widehat{x}_{k}$ 的方差更大。

$y_{k}$ ：当前时刻的测量值，因为测量过程也会存在误差，所以也用密度分布来表示。

最优估计：真正的位置（或者optimal的位置）处于 $\widehat{x}_{k}$ 与 $y_{k}$ 之间（将两个高斯斑相乘： $\widehat{x}_{k}$ 与 $y_{k}$ 概率函数相乘），最优估计的方差小于 $\widehat{x}_{k}$ 的方差，平均值为最优估算距离。

PS: 对两个均值方差都不相同的侧高斯斑进行相乘，你可以得到一个新的高斯斑。（定理）

高斯乘法：

一维高斯（均值u,方差 $\sigma ^{2}$ （sigma））：

两个高斯相乘：

preview

$N(x, u_{0}, \sigma_{0}) * N(x, u_{1}, \sigma_{1}) = N(x, u^{'}, \sigma^{'})$

多维矩阵形式，Σ表示协方差矩阵， $\overrightarrow{u}$ 表示平均向量：

K：卡尔曼增益

5. 卡尔曼滤波的使用场景。

任何含有不确定因素的动态系统，系统总是会受到一些未知的干扰，适合嵌入式实时操作系统（它仅需要保留系统上一个状态的数据，而不是一段跨度很长的历史数据）。

6. 卡尔曼滤波五兄弟。

大兄弟： $X_{k} = K_{k} * Z_{k} + (1 -K _{k}) * X_{k - 1}$

其中： $X_{k}$ ：本次的预测值， $K_{k}$ ：卡尔曼的增益值， $Z_{k}$ ：本次的预估值， $X_{k - 1}$ ：上次的测量值

可以看到我们的大兄弟中，需要我们考虑的值就是 $K_{k}$ ，而卡尔曼就是帮助我们不断修正大兄弟的 $K_{k}$ 。

二师兄（系统状态方程）： $X_{k} = AX_{k - 1} + Bu_{k - 1} + w_{k - 1}$

其中： $X_{k}$ ：当前的状态量， $X_{k - 1}$ ：上一时刻的状态， $u_{k - 1}$ ：系统控制量，绝大数情况下该值为0， $w_{k - 1}$ ：满足高斯条件的过程激励噪声， A B：系统参数，矩阵形式。

然后二师兄变为了： $X_{k} = X_{k - 1}$ （系统状态无任何变化，前后相同）

三师弟（系统观测方程）： $Z_{k} = HX_{k} + v_{k - 1}$

其中： $Z_{k}$ ：测量值， $X_{k}$ ：当前的状态量，H：状态量对于测量值的增益， $v_{k-1}$ ：满足期望为0协方差为的高斯白噪声,也就是传感器的靠谱程度。

四师弟（增益更新）： $K_{k} = \frac{P_{\overline{k}}H^{T}}{HP_{\overline{k}}H^{T} + R}$

其中： $P_{\overline{k}}$ 预估误差协方差，也就是可信度，初始值为1就可以，之后会进行迭代更新。之所以用（列）向量而非一个数来表示状态值，是因为在实际的应用中很多状态并不是一个数就能表示的（比如导弹在空间中的位置，同时有x、y、z三个坐标）。

H 是状态量到观测量的转换矩阵，R是一个经验值, 说白点就是你自己看着办, 越小表示越信任预测值，为0的话表示完全相信预测值，越大表示越相信测量值

6. 卡尔曼滤波应用流程：

1. 首先把四师弟的H设置为0： $K_{k} = \frac{P_{\overline{k}}H^{T}}{HP_{\overline{k}}H^{T} + R}$ --> $K_{k} = \frac{P_{\overline{k}}}{P_{\overline{k}} + R}$

R：测量噪声协方差，传感器的靠谱程度，0.8？

2. 其次 $X_{k} = X_{\overline{k}} + K_{k}(z_{k} - HX_{\overline{k}})$ 当H为1时，也就成为了大兄弟： $X_{k} = K_{k} * Z_{k} + (1 -K _{k}) * X_{k - 1}$

$X_{k}$ ：状态估计值，也就是卡尔曼滤波后我们"猜"出来的温度值，它的初值你可以直接设为0。

3. $P_{k} = (1 - K_{k}H)P_{\overline{k}}$ ：预估误差协方差更新可信度 --> $P_{k} = (1 - K_{k})P_{\overline{k}}$ ，初始值为1就可以，之后会进行迭代更新。

实践：

kalman init：

可信度：R = 0.2， $P_{\overline{k}}$ = 1， $X_{\overline{k}}$ = 0

$Z_{k}$ ：每次的测量值

First time：

1. 卡尔曼增益： $K_{k} = \frac{1} {1 + 0.2} = 0.833$

2. $X_{k}$ = 0.8333 * 33.894 + 0.167 * 0 = 28.245

3. $P_{k}$ = （1 - 0.833） * 1 = 0.167

Second time：

1. $K_{k} = \frac{0.167} {0.167 + 0.2} = 0.455$

2. $X_{k}$ = 0.455 * 32.019 + 0.545* 28.245 = 29.962

3. $P_{k}$ = （1 - 0.455） * 0.167 = 0.091

Third time：

1. $K_{k} = \frac{0.091} {0.091 + 0.2} = 0.313$

2. $X_{k}$ = 0.313 * 35.270 + 0.687* 29.962 = 31.623

3. $P_{k}$ = （1 - 0.313） * 0.091 = 0.063

tim514

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
卡尔曼滤波

牛逼的算法往往来源于一个简单的思想 + 演化过程。1. 卡尔曼滤波是什么?卡尔曼滤波用于线性系统线性系统，对系统的输出进行最优估计（预测）的算法，因为系统的输入（观测量）存在噪声与干扰，会对整个输出结果带来累积误差与偏差，我们最终的结果也相当于对这些干扰进行滤波的过程。2. 卡尔曼滤波的用途？应用：制导与导航系统，计算机视觉系统与信号处理，sensor的误差估算来得到实际测量值。卡尔曼滤波一般用于优化估算（预测）一些无法直接测量，但是可以通过其他的量间接来估算的量。3. 状态控制的知
复制链接

扫一扫