卡尔曼算法思想理解 Kalman filter 第二篇

本文链接：https://blog.csdn.net/weixin_44638957/article/details/105486282

卡尔曼算法思想理解 Kalman filter 第二篇

接下来这篇，要从算法的角度出发了

还没上车的朋友们请从卡尔曼算法思想理解 Kalman filter 第一篇务必先读过， 没有公式没有公式没有公式

首先我们将上一篇太空船的例子借用一下，还记得我们用了一个K控制器放在测量与观察之间吗？也记得我们说过K是要将y与yhat之间的误差降至最小对吧？

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Dlrt6MTC-1586753345895)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413112137131.png)]

这一整个浅蓝色的部位我们可以理解为一个状态观察器 state observer, 主要功能就是将x与xhat的误差降到最低

让xhat直逼x

x 表示实际要测量的燃烧室维度
x_hat表示预测出的燃烧室温度

在看到右式

第一行： e_obs 就表示误差值 $\hat{x}$
第二行：xpot = Ax+Bu , y = Cx 这表示整个测量过程的数学等式
第三行：xhat = Axhat + Bu + K(y-yhat) , yhat =Cxhat , 这表示预测的整个过程
- 这特别注意这里有多了一个K(y-yhat), 表达的就是在原先预测的基础上添加了K控制器来调控误差，看图上浅蓝色的部位，就很清楚明了，在下方y与yhat相减之后到K进行控制然后再回到预测model中，这就是我们的状态观察器(State Obsever)了
第四行：所以误差就是第二和第三方程式相减

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-tW4eYgzD-1586753345896)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413103704611.png)]

整个式子搭配左边的图对照起来就非常清楚了

最终可以简化成

$e_{obs} = (A - KC)^e_{obs}$ , $y-\hat{y} = Ce_{obs}$

好，讲这个状态观察器说的这么多做什么呢？主要是先得有观念

主要还是要为了Kalman铺陈一下的

现在回到自驾车比赛的例子，下面这张图还有印象的吧

没有的话就复习一下

上面属于测量measurement ：主要从自驾车起始点到终点线的过程，中间还有一些噪声影响速度比如风啊什么的
下面属于预测predicition ：也就是设计了一套算法预测出到终点线的位置

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-UNo8o2Aw-1586753345897)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413104845981.png)]

现在我们把自驾车的例子也写成公式摊开与**状态观察器(State Observer)**对照，是不是几乎是一样的？

实际上卡尔曼就算是一个状态观察器，只不过设计为多用途的

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PUz130RF-1586753345899)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413104516523.png)]

这个下面的公式可能会稍微想久一点，其实就把太空船状态观察器的式子套用到自驾车比赛上就是了

现在我们单独把Kalman filter 拉出

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-OBl2UMMg-1586753345900)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413113309829.png)]

可以看到圈起来的两个 $\hat{x_k}$ 并不是同一个预测值

等式左边的 $\hat{x_k}$ 表示的是当前的

右手边的 $\hat{x_k}$ 表示前一个，是将当前测量值添加进来之前就计算出的，就叫做Prior Estimate

我们将 $（A\hat{x}_{k-1}+Bu_k）$ 统一成 $\hat{x_k}$ 加上一横竖，式子如下

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-qZaibGhV-1586753345903)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413113810985.png)]

现在式子变成三个部分了

后预测值 Poserior Estimate
前预测值 Predict
更新条件 Update

好，到了这边

Kalman filter的形式就是如公式那样，容易看懂吧？当前值 = 前值 + 更新，不就类似梯度下降更新的过程吗？

接下来深究一下更新的过程

我们讲过Kalman可以将预测与测量相乘后得到方差更小的高斯分布

这个方差值我们记做P，看下图

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-5nQ0rLMv-1586753345904)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413114635303.png)]

看到一堆公式别慌，这只是要告诉我们是如何利用Kalman filter更新预测的结果的

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-v3i6T1Eh-1586753345905)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413114951313.png)]

先看左边predition， P就是高斯分布内（预测）的方差，注意到最一开始的预测肯定是没办法带入到Update 来调整的，为啥？因为一开始肯定还没有修正值啊，所以初始 $\hat{x_{k-1}}$ 及 $P_{k-1}$ 都是用初始值计算

怎么更新呢？看右边的Update, 将得到的 $P_{k}$ 带入更新式子，透过K 更新之后就能得到一个

新的 $\hat{x}_k$ （后预测值）及 $P_k$ （更新后方差）

那么如何调整K 控制器就很重要了， K是影响测量与误差的关键方程式

这里就引入新的概念 卡尔曼增益 就是 K

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ZdwApCOe-1586753345907)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413120449283.png)]

假设我们测量过程中的噪音很小，那就说明测量的值是相对更精确的，如下图 $y_k$ 对于计算的贡献是比较大的

如果用极端的例子，我们将测量的协方差R逼近0（说明测量结果非常精准），以数学新式表达就如下面

最终结果 $\hat{x}_k = y_k$ 也就是明显后预测的结果跟测量完全一样

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-TFBx76K5-1586753345908)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413122151413.png)]

下图则是相反情况，预测值更可靠的话

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-MNeg2VpB-1586753345909)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413122534934.png)]

看到这部应该可以有八成的理解了吧？maybe 九成？

Update 结束后就能得到修正后的值，这个值就继续用来预测新的预测值，不断迭代下面方程来获取最优的点

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-AHt5Uxfa-1586753345910)(/Users/stephenfang/Library/Application Support/typora-user-images/image-20200413124305934.png)]