卡尔曼滤波(F)

2020-09-15 本文已影响0人 zidea

图

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

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

因为在动态方程我们有两个状态变量，所以也就有两个误差分别表示 $e_1$ 和 $e_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)

猜你喜欢

热点阅读