卡尔曼滤波(3)

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

图

状态空间方程
$\begin{aligned} X_k = AX_{k-1} + B u_{k-1} + W_{k-1}\\ Z_k = HX_k + V_k \end{aligned}$

用动态方程形式来描述系统动态响应，其中 $W_{k-1}$ 是过程噪声， $V_k$ 测量噪声。

$X_k$ 是状态变量
A 是状态矩阵
B 是控制矩阵
$u_k$ 是控制
$W_{k-1}$ 是过程噪音
$W_{k-1}$ 是过程噪音，是不可控制的，是过程的一种不确定性的表现。通常这些噪音都是符合正态分布。

$p(w)$ 服从 $(0,Q)$ 分布

Q 是方差
0 是期望
接下来我们来计算方差 Q，在开始计算方差之前，我们简单复习一个概率论关于方差计算公式

$VAR(X) = E(X^2) - E^2(X)$

$Q = E[w,w^T] = E \left[ \begin{bmatrix} w_1\\ w_2 \end{bmatrix} \begin{bmatrix} w_1 & w_2 \end{bmatrix} \right] = \begin{bmatrix} E(w_1)^2 & E(w_1w_2)\\ E(w_2w_1) & E(w_2)^2 \end{bmatrix}$

$Q = \begin{bmatrix} \sigma w_1^2 & \sigma w_1w_2\\ \sigma w_2w_1 & \sigma w_2^2 \end{bmatrix}$

中协方差矩阵可以表示出 w 过程噪声中 $w_1$ 和 $w_2$ 之间的关系。有关如何计算 $V_k$ 测量噪声，这里推导过程与 w 类似，就不再和大家一起再推导一次了。

回归头来我们在看这个两个动态方程，首先动态方程中 $X_k = AX_{k-1} + B u_{k-1}$ 和 $Z_k = HX_k$ 是已知是我们估计的或者测量出来，而 $W_{k-1}$ 和 $V_k$ 是未知的，我们无法对其进行建模。

$\hat{X_k}^- = A\hat{X}_{k-1} + B u_{k-1}$

其中 hat 符号表示估计
$\hat{X_k}^-$ 中的符号表示先验
$\hat{X}_{k-1}$ 也是估计出来的，所以也有一个 hat 符号
$Z_k = HX_k \rightarrow \hat{X}_{k_{MEA}} = H^{-}Z_k$
下面的 $\hat{X}_{k_{MEA}}$ 是根据测量值计算出出来的结果。现在我们就会用数据融合来进行处理问题。

$\hat{X_k} = \hat{X}^- G(H^-Z_k - \hat{X_k}^-)$

$\begin{cases} G = 0 & \hat{X}_k = \hat{X_k}^-\\ G = 1 & \hat{X}_k = GH^-Z_k \end{cases}$

G = 0 就更加相信计算出来的结果
G = 1 就等于更加相信测量出来的结果

如果我们让 $G = K_kH$ 然后将带入上面公式中就得到
$\hat{X_k} = \hat{X}^- + K_k(Z_k - H\hat{X_k}^-)$
这里 $K_k \in [0,H^-]$

接下来我们目标就是寻找 $K_k$ 来使得 $\hat{X}_k$ 趋近于其实际值 $X_k$
接下来我们引入误差，用 $e_k$ 来表示，误差也是服从正态分布，而且 P 为误差的协方差矩阵。
$e_k = X_k - \hat{X}_{k}$
$p(e_k) (0,P)$

$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}^-)$

卡尔曼滤波(3)

猜你喜欢

热点阅读