卡尔曼滤波初探（一）

最新推荐文章于 2024-05-21 14:52:34 发布

dieju8330

最新推荐文章于 2024-05-21 14:52:34 发布

阅读量474

点赞数 1

分类专栏： python机器学习

本文链接：https://blog.csdn.net/dieju8330/article/details/84194307

版权

python机器学习专栏收录该内容

5 篇文章 0 订阅

订阅专栏

卡尔曼滤波初探

基于时域的线性模型预测

这里先给出几个概念（初看的时候很多博客都没有这方面说明，若你看到下面懵逼的时候，不妨上来再看看？）

预测：就是根据已有的①经验、②公式、③以及上一个时间（ $t_{k-1}$ ）下检测对象的状态的最优估计等信息，从而得到一个对下一个时间（ $t_{k}$ ）下状态的一个估计。【X(k|k-1)=AX(k-1|k-1)+Bu(k).....................(1)】
更新：指到达时间（ $t_{k}$ ）后，我们的传感器就获得了此刻时间（ $t_{k}$ ）下检测对象的状态信息，这个信息是测量信息，同样包含了不确定的成分（uncertainty），因此，我们需要结合我们上面所说的预测，将测量值和预测值进行融合，得到一个最优的估计。【X(k|k)=X(k|k-1)+Kg(k)*(Z(k)-Hx(k|k-1))....(3)　】
协方差矩阵（误差估计）：协方差矩阵来表示，简而言之，矩阵中的每个元素 $这里写图片描述$ 表示第 i 个和第 j 个状态变量之间的相关度。

基于案例分析：

（1）假设一辆小车在路上行驶，其在时间t下的状态如下： $x_{t}= \begin{bmatrix} p_{t} \\ v_{t} \end{bmatrix}$ , pt为t时刻下的位置，vt为t时间下的速度

（2）进一步的，如果我们有一个控制信号 $u_{t}$ 表示油门，这个ut在物理模型上可表示为加速度a

那么，根据经典物理学，我们可以写出t时刻下的状态：

即 $x_{t}= \begin{bmatrix} 1 &\Delta t \\ 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} p_{t-1} \\ v_{t-1} \end{bmatrix} +\begin{bmatrix} \frac{\Delta t^{2}}{2}\\\Delta t \end{bmatrix}\cdot u_{t}$ ，可简写为如下：

其中：

F为状态转移矩阵
B为控制矩阵

到这里，上面的公式就是这个例子中的“预测”公式

（3）既然有了预测，那么我们的最终目的是更新，接下来，就是推导"更新操作"的公式

1、首先要明确的是：在t时刻，我们得到了①根据预测公式计算的预测值②传感器的测量值。一般来说，这两个都是都是高斯分布，如图。

2、那么，问题就转化为，我们应该相信谁的问题，是相信红色部分、还是蓝色部分的分布？答案是，两个都相信，两个都用，那么很自然的想到：两个事件同时发生的可能性越大，我们越相信它！要想考察它们同时发生的可能性，就是将两个事件单独发生的概率相乘。

即下图的绿色部分（也是正态分布）：

这里引用一下其他博客关于这个问题的解释：

这里就涉及到了高斯分布相乘的问题：

这里再次感谢翻译博主以及原文博主对此作出的贡献，到了这里，其实已经得到了“更新操作”的公式：

我们用之前沿用的 $x_{t}$ 对上面的均值 $\mu$ 进行替换，从而得到：

$x_{t,opt}=x_{(t|t-1)}+K_{t}*(x_{t|sensor}-x_{(t|t-1)})$

【这就是我们的更新公式】

其中，这些都是变量或矩阵，（因为计算机对于矩阵的运算更有效）

$x_{t,opt}$ 为我们更新操作后得到的最优值
$x_{(t|t-1)}$ 为我们的预测值（与t-1时刻的最优值相关），即基于t-1时刻的最优状态推算出来t时刻的状态
$x_{t|sensor}$ 为t时刻下，传感器获得的状态
公式中有一个 $K_{t}$ ，我们称为卡尔曼增益，这里先给出它的计算公式（我们用P代替了上面的 $\sum$ ）： $K_{t}=P_{(t|t-1)}*(P_{(t|t-1)}+P_{(sensor)})^{-1}$ :

其中：
$P_{(t|t-1)}$ 是协方差矩阵的估计值，是基于 $P_{(t-1|t-1)}$ 进行估计得到的。
至于 $P_{(t-1|t-1)}$ 是怎么来的，这里也给出公式： $P_{(t-1|t-1)}=(I-k_{t-1})*P_{(t-1|t-2)}$ 【I为单位矩阵】
很显然， $P_{(t-1|t-1)}$ 是在t-1时刻，进行“更新操作”完成之后，再按照上面的公式计算而来，以便在t时刻进行迭代
实际上，这里也是卡尔曼滤波迭代自回归的奇妙之处？
关于协方差P的解释，请继续看下文

（4）上文，我们已经得到了“更新操作”的公式，在此之前，我们还需要获得“更新”操作过程所需的参数协方差矩阵

1、关于协方差，协方差是用来表示两个维度之间的相关性

a、当两个维度不相关：当横轴上的变量v变化时，并不影响纵轴的变量position，对于纵轴上的变量也同理

gauss_1

b、两个维度相关：如图中白色部分，可想象为一条斜线，假设横轴方向的变量v变化时，其对应的纵轴变量positon也改变，实际上，这也是现实中的大部分情况，一个例子就是：我们基于旧的位置来估计新位置position。如果速度v过高，我们可能已经移动很远了。如果缓慢移动，则距离不会很远。

对于这种关系，一般用协方差矩阵表示，即：

到这里，我们获取到了一个我们更新所需的参数，协方差矩阵P（注意是大写的P，与状态x中的p--->positon区分）

PS：这里协方差矩阵有一个性质如下【下面的预测公式会用到】：

这里写图片描述

2、关于不确定性因素

a、我们先整理一下现在获得的公式：

预测公式：

$x_{(t|t-1)}=F_{t}\cdot x_{t-1,opt}+B_{t}\cdot u_{t}$

$P_{(t|t-1)}={\color{Red} F_{t}}\cdot P_{(t-1|t-1)}\cdot {\color{Red} F_{t}^{T}}$ （协方差矩阵的式子用到了上面提到的性质）

更新公式：

$x_{t,opt}=x_{(t|t-1)}+K_{t}*(x_{t|sensor}-x_{(t|t-1)})$

$P_{(t|t)}=(I-k_{t})*P_{(t|t-1)}$

$K_{t}=P_{(t|t-1)}\cdot *(P_{(t|t-1)}+P_{(sensor)})^{-1}$

【考虑到我们的传感器读取的数据可能会经过一些变换（缩放等），因此要加上一个传感器模型矩阵H，公式变为：】

$x_{t,opt}=x_{(t|t-1)}+K_{t}*(x_{t|sensor}-{\color{Red} H}\cdot x_{(t|t-1)})$

$P_{(t|t)}=(I-k_{t}\cdot {\color{Red} H^{T}})*P_{(t|t-1)}$

$K_{t}=(P_{(t|t-1)}\cdot {\color{Red} H^{T}} )\cdot ({\color{Red} H}\cdot P_{(t|t-1)}\cdot {\color{Red} H^{T} }+P_{(sensor)})^{-1}$

b、现在来加上我们的不确定因素

预测操作的不确定因素：

如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的，则不会出现什么问题。但是如果考虑外部干扰，例如，假设我们跟踪一个四旋翼飞行器，它可能会受到风的干扰，如果我们跟踪一个轮式机器人，轮子可能会打滑，或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪，如果没有把这些外部干扰考虑在内，我们的预测就会出现偏差。（引用自：https://blog.csdn.net/u010720661/article/details/63253509）

如何引入？

一种做法是：我们将这些没有被跟踪的干扰当作协方差为 ${\color{Blue} Q_{t}}$ 的噪声来处理

将上面总结的预测公式搬下来，并在协方差的式子上加上 ${\color{Blue} Q_{t}}$ 表示没有跟踪的外部干扰

新的预测式子：

${\color{Red} x_{(t|t-1)}=F_{t}\cdot x_{t-1,opt}+B_{t}\cdot u_{t}}$ ---------------------------->①

${\color{Red} P_{(t|t-1)}={\color{Red} F_{t}}\cdot P_{(t-1|t-1)}\cdot {\color{Red} F_{t}^{T}}}+{\color{Blue} Q_{t}}$ ---------------------->②

由上式可知，新的预测是根据上一最优估计预测得到的，并加上已知外部控制量的修正。
而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。

更新操作的不确定因素：

我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。【简单说，就是我们的传感器测得的不一定是真值】

如何引入？

一种做法是：将这种不确定性（例如：传感器噪声）用协方差 ${\color{Green} R_{t}}$ 表示，该分布的均值就是我们读取到的传感器数据，称之为 ${\color{DarkOrange} x_{t|sensor}}$ 。

将上面总结的更新公式搬下来，我们回过头来看卡尔曼增益 $K_{t}$ 【两个高斯分布相乘的部分提到的】：

${\color{Red} x_{t,opt}=x_{(t|t-1)}+K_{t}*({\color{DarkOrange} x_{t|sensor}}-{\color{Red} H}\cdot x_{(t|t-1)})}$ ---------------->③

${\color{Red} P_{(t|t)}=(I-k_{t}\cdot H)*P_{(t|t-1)}}$ ---------------------------------------->④

卡尔曼增益（前文提到的公式如下）：

$K_{t}=(P_{(t|t-1)}\cdot H^{T} )\cdot ( H\cdot P_{(t|t-1)}\cdot H^{T} +{\color{Magenta} P_{(sensor)}})^{-1}$

而由这里的不确定性因素（噪声）可知， ${\color{Magenta} P_{(sensor)}}={\color{Green} R_{t}}$ ，于是，卡尔曼增益公式变为：

${\color{Red} K_{t}=(P_{(t|t-1)}\cdot {\color{Red} H^{T}} )\cdot ({\color{Red} H}\cdot P_{(t|t-1)}\cdot {\color{Red} H^{T} }+{\color{Green} R_{t}})^{-1}}$ ------------------>⑤

以上 ①②③④⑤，5条红色标记的公式，就是卡尔曼滤波的5条公式【矩阵表示】。

【注意：本文没有进行公式类的推导，而是在某问题上给出了直接的公式，可能逻辑方面有欠缺】

最后，给出整一个卡尔曼滤波时间线操作过程

参考内容：

https://www.jianshu.com/p/d3b1c3d307e0

https://www.cnblogs.com/sillykog/p/6618587.html

https://blog.csdn.net/u010720661/article/details/63253509

http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

https://www.bilibili.com/video/av4356232?from=search&seid=13134061163091304662

dieju8330

关注

1
点赞
踩
0

收藏

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

卡尔曼滤波初探基于时域的线性模型预测这里先给出几个概念（初看的时候很多博客都没有这方面说明，若你看到下面懵逼的时候，不妨上来再看看？）预测：就是根据已有的①经验、②公式、③以及上一个时间（）下检测对象的状态的最优估计等信息，从而得到一个对下一个时间（）下状态的一个估计。【X(k|k-1)=AX(k-1|k-1)+Bu(k).....................(1)】更新...
复制链接

扫一扫

专栏目录