美文网首页深度学习
卡尔曼滤波(F)

卡尔曼滤波(F)

作者: zidea | 来源:发表于2020-09-15 22:02 被阅读0次

在开始之前我们简单回顾以下上一次分享的内容。我们目标是就是寻找一个K_k 使得估计值\hat{X}_k越趋近X_k越好。那么问题是我们如何通过数学的形式将这个表达出来呢。这里用e_k = \hat{X}_k - X_k 来表示两者之间关系,其中e_k 表示\hat{X}_kX_k之间的距离,也就是我们希望e_k越小越好。因为e_k 是误差所以我们认为其服从正态分布(0,P),其中 P 表示方差。今天分享依然是枯燥的公式推导为主,我在学习这部分时候也是听一段然后停下来消化一下再继续学习下一段内容。这里说一说有关一些枯燥内容,我们可能看了一段时间,还是没有什么进展,这个时候不要急躁,停下来做的别,看点简单内容然后再回来学,这是个人学习一些枯燥难懂知识点的一点感受。

e_k = X_k - \hat{X}_{k}
p(e_k) (0,P)

因为在动态方程我们有两个状态变量,所以也就有两个误差分别表示e_1e_2
P = E \left[ e e^T \right] = \begin{bmatrix} \sigma e_1^2 & \sigma e_1 \sigma e_2\\ \sigma e_2 \sigma e_1 & \sigma e_2^2 \end{bmatrix}

当我们的估计值 \hat{X}_k 距离真实 X_k 越小,也就是说明整个方差值最小。方差越小也就是 e 误差的期望为 0,也就是希望tr(P) = \sigma e_1^2 + \sigma e_2^2 越小。也就是等价于其方差越小。

\begin{aligned} P = E\left[ee^T\right] = E \left[ (X_k - \hat{X}_k)(X_k - \hat{X}_k)^T \right] \end{aligned}

因为这里我们要求 K_k 所以将\hat{X_k} = \hat{X}^- + K_k(Z_k - H\hat{X_k}^-) 带入上面公式

\begin{aligned} X_k - \hat{X_k} = X_k - \hat{X}^- + K_k(Z_k - H\hat{X_k}^-)\\ = X_k - \hat{X}^- + K_kZ_k - K_kH\hat{X_k}^- \end{aligned}

那么因为 Z_k = HX_k + V_k 将带入上面式子替换掉 Z_k

\begin{aligned} X_k - \hat{X_k} = X_k - \hat{X}_k^- + K_k(Z_k - H\hat{X}_k^-)\\ = X_k - \hat{X}_k^- + K_kHX_k - K_kV_k - K_kH\hat{X}_k^-\\ = (X_k - \hat{X}_k^-) + K_kH(X_k - \hat{X}_k^-) - K_kV_k\\ = (I - K_kH)(X_k - \hat{X}_k^-) - K_kV_k \end{aligned}

现在将推导出公式带入到上面 P = E[ee^T] 公式中得到下面公式

\begin{aligned} P = E[ee^T] \\ = E \left[ (X_k - \hat{X}_k)(X_k - \hat{X}_k)^T \right]\\ = E\left[[(I - K_kH)(X_k - \hat{X}_k^-) - K_kV_k][(I - K_kH)(X_k - \hat{X}_k^-) - K_kV_k]^T\right] \end{aligned}

在继续开始推导之前我们,还需要简单复习以下有关矩阵转置的性质。
(AB)^T = B^TA^T
(A+B)^T = A^T + B^T

\begin{aligned} = E\left[[(I - K_kH)(X_k - \hat{X}_k^-) - K_kV_k][(I - K_kH)(X_k - \hat{X}_k^-) - K_kV_k]^T\right]\\ = E\left[[(I - K_kH)e_k^- - K_kV_k][(I - K_kH)e_k^- - K_kV_k]^T\right]\\ = E\left[[(I - K_kH)e_k^- - K_kV_k][(e_k^-)^T(I - K_kH)^T - V_k^TK_k^T]\right] \end{aligned}

接下来就是耐心活了,我们需要将上面公式进行展开

\begin{aligned} E\left[ (I - K_kH)e_k^-(e_k^-)^T(I - K_kH)^T - (I - K_kH)e_k^-V_k^TK_k^T - K_kV_k(e_k^-)^T(I - K_kH)^T + K_kV_kV_k^TK_k^T ] \right] \end{aligned}

这里看起来复杂其实没有什么好说的,看起来复杂就是公式展开。因为这是线性运算所以求期望可以表示为对每一项求期望。

\begin{aligned} E\left[ (I - K_kH)e_k^-(e_k^-)^T(I - K_kH)^T \right] - E \left(I - K_kH)e_k^-V_k^TK_k^T \right] - E \left[ K_kV_k(e_k^-)^T(I - K_kH)^T \right] + E \left[ K_kV_kV_k^TK_k^T ] \right] \end{aligned}

  • 我们来看E \left(I - K_kH)e_k^-V_k^TK_k^T \right] 这一项,因为(I - K_kH)K_k^T 都是常数所以提取出来的都 (I - K_kH) E\left [ e_k^-V_k^T \right] K_k^T 因为对于两个相互独立 A 和 B 随机变量他们就有E(AB) = E(A)E(B) 因为e_k^-V_k^T 是相互独立所以有 (I - K_kH) E\left [ e_k^-\right] E\left[V_k^T \right] K_k^T 而且E\left [ e_k^-\right]E\left[V_k^T \right]期望都是 0 所以 E \left(I - K_kH)e_k^-V_k^TK_k^T \right] 就是 0

P_k = (I - K_kH)E\left[ e_k^-(e_k^-)^T \right](I - K_kH)^T + K_kE \left[ V_kV_k^T ] \right]K_k^T

E\left[ e_k^-(e_k^-)^T \right] = P_k^- 也就是先验误差的协方差矩阵。那么 E \left[ V_kV_k^T ] \right] = R

P_k = (I - K_kH) P_k^-(I - K_kH)^T + K_kE \left[ V_kV_k^T ] \right]K_k^T

\begin{aligned} P_k = (P_k^- - K_kHP_k^-)(I - H^TK_k^T) + K_kRK_k^T\\ P_k^- - K_kHP_k^- - P_k^-H^TK_k^T + K_kHP_k^-H^TK_k^T + K_kRK_k^T \end{aligned}

相关文章

  • 卡尔曼滤波(F)

    在开始之前我们简单回顾以下上一次分享的内容。我们目标是就是寻找一个 使得估计值越趋近越好。那么问题是我们如何通过数...

  • 卡尔曼滤波及其无人驾驶应用

    无人驾驶汽车系统感知模块的重要技术——卡尔曼滤波,应用包括:卡尔曼滤波与行人状态估计扩展卡尔曼滤波(EKF)与传感...

  • 图文并茂,卡曼滤波

    ​卡尔曼滤波是如何工作? 看图说话! 我务必向大家介绍卡尔曼滤波器(Kalman Filter),因为它之所能, ...

  • iOS-卡尔曼滤波算法

    一:前言 滤波算法 用于过滤掉连续的数据中出现偏差较大的数据 二:卡尔曼滤波算法 <0>卡尔曼滤波的原理请自行百度...

  • 卡尔曼滤波

    把卡尔曼滤波写一下吧,思想很简单,不详细写了,就是根据方差实现的一种最优估计方法。 卡尔曼滤波五个基本的公式 1....

  • 卡尔曼滤波

    卡尔曼公式 先以状态协方差随时间的传播性,得到先验估计(时间更新),再利用测量值和最小二乘估计,得到后验估计(状态...

  • 卡尔曼滤波

    话不多说,我这里先给出我们的系统的模型方程,状态转移方程: 测量方程: 需要说明的是,这里的也可以是随变化的,但是...

  • 卡尔曼滤波

    概述   卡尔曼滤波(Kalman filter)是一种高效率的递归滤波器(自回归滤波器),它能够从一系列的不完全...

  • 卡尔曼滤波

    为什么看卡尔曼滤波 因为想要对变化/趋势进行预测,应用偏向于机械等故障类的预测。也看了不少卡尔曼滤波相关的东西,大...

  • 卡尔曼滤波

    本文是国外博主Bzarg在2015年写的一篇图解。虽然是几年前的文章,但是动态定位、自动导航、时间序列模型、卫星导...

网友评论

    本文标题:卡尔曼滤波(F)

    本文链接:https://www.haomeiwen.com/subject/jyjqyktx.html