卡尔曼滤波(3)

作者: zidea | 来源:发表于2020-09-13 19:13 被阅读0次

图

状态空间方程
$\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}}$ 是根据测量值计算出出来的结果。现在我们就会用数据融合来进行处理问题。