卡尔曼滤波(2)

图

在今天将主要讨论 3 个方面

数据融合(Data Fusion)
协方差矩阵(Covariance Matrix)
状态空间表示(State Space Representation)

数据融合

假设我们手上有两个测量仪器，他们都存在误差而且测量值都服从正态分布，用协方差来表示他们精度。
$\begin{aligned} z_1 = 30g & \sigma_1 = 2g\\ z_2 = 32g & \sigma_2 = 4g\\ \end{aligned}$
都服从正态分布，在标准差之间覆盖率 $68.4\%$ ，现在有了分别用这两个测量仪器进行测量得到了两个测量结果，然后我们需要根据测量结果对真实值进行估计。首先我们看 $z_1$ 的测量精度要比 $z_2$ 高，所以我们估计值应该更靠近 $z_1$ 。这样就会用到上一次分享，

估算真实值 $\hat{z} = z_1 + K(z_2 - z_1) K \in [0,1]$
这里 K 取值在 0 到 1 之间，接下来看一看 K 在取 0 或 1 时估计值的取值情况。
$\begin{aligned} K = 0 & \hat{z} = z_1\\ K = 1 & \hat{z} = z_2\\ \end{aligned}$

我们目标就是求解 K 使得 $\sigma_{\hat{z}}$ (方差)最小，从而得到 $Var(\hat{z})$ 最小。
$\begin{aligned} \sigma_{\hat{z}} = Var(z_1 + K(z_2 - z_1))\\ = Var(z_1 - Kz_1 + Kz_2)\\ = Var((1-K)z_1 + Kz_2) \\ = Var((1 - K)z_1) + Var(Kz_2)\\ = (1 - K)^2 Var(z_1) + K^2Var(z_2)\\ = (1 - K)^2\sigma_1^2 + K^2\sigma_2^2 \end{aligned}$

上一系列公式是我们对方差求解过程的推导，这里来一行一行地给大家解释，首先我们写出 $\hat{z}$ 的方差，然后将 $K(z_2 - z_1)$ K 乘进去写成第二行形式，接下来我们将公式写成 $Var((1 - K)z_1) + Var(Kz_2)$ 这种形式后因为 $z_1$ 和 $z_2$ 是相互独立，根据方差的性质又可以写成 $Var((1 - K)z_1)$ 和 $Var(Kz_2)$ 取和的形式。最后将常数提取出来得到 $(1 - K)^2 Var(z_1) + K^2Var(z_2)$

然后大家都知道求极值问题就是求导的问题，我们通过求导数可以求极限问题。

$\frac{d \sigma_{\hat{z}}^2}{d K} = 0$
$\begin{aligned} -2(1 - K)\sigma_1^2 + 2 K \sigma_2^2 = 0\\ - \sigma_1^2 + K\sigma_1^2 + K\sigma_2^2 = 0\\ K = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2} = \frac{2^2}{2^2 + 4^2} = \frac{4}{4 + 16} = 0.2 \end{aligned}$

$\hat{z} = z_1 + K(z_2 - z_1) = 30 + 0.2 (32 - 30) = 30.4$
$\sigma_{\hat{z}}^2 = (1 -0.2)^22^2 + 0.2^24^2 = 3.2$
$\sigma_{\hat{z}}^2 = 1.79$

这就是数据融合的过程

协方差矩阵

协方差在一个矩阵中表现出来，变量间的联动关系

球员	身高	体重	年龄
瓦尔迪	179	74	33
奥巴梅扬	187	80	31
萨拉赫	175	71	28

$\sigma_x^2 = \frac{1}{3}((height - average \,height))$
$\sigma_x\sigma_y = \frac{1}{3}((height - average\,height)(weight - average \, height)) = \sigma_y \sigma_x$
如果得到正值表示这两个变量是正相关，也就是这两个变量变化是一致的，反之亦然。

$P = \begin{bmatrix} \sigma_x^2 & \sigma_x\sigma_y & \sigma_x\sigma_z \\ \sigma_y\sigma_x & \sigma_y^2 & \sigma_y\sigma_z \\ \sigma_z\sigma_x & \sigma_z\sigma_y & \sigma_z^2 \\ \end{bmatrix}$
求解过渡矩阵
$a = \begin{bmatrix} x_1 & y_1 & z_1\\ x_2 & y_2 & z_2\\ x_3 & y_3 & z_3\\ \end{bmatrix} - \frac{1}{3} \begin{bmatrix} 1 & 1 & 1\\ 1 & 1 & 1\\ 1 & 1 & 1 \end{bmatrix} \begin{bmatrix} x_1 & y_1 & z_1\\ x_2 & y_2 & z_2\\ x_3 & y_3 & z_3\\ \end{bmatrix}$

$P = \frac{1}{3}a^Ta$

状态空间

状态空间是一个完整的控制体系，现在控制理论就是以状态空间方程为基础。弹簧震动阻尼系统，质量 m 施加的力是 F 移动位移是 x 弹簧系数是 k ，B 是阻尼系数。
接下来写出弹簧阻尼系统的动态方程表达式
$m\ddot{x} + B\dot{x} + Kx = F(=u)$

其中 F 是系统输入，随后用小写 u 来表示

现在我们就要把这个方程化成状态空间的表达形式，先定义状态变量。
$\begin{aligned} x_1 = x\\ x_2 = \dot{x} \end{aligned}$
这样就得到下面，其中 $\dot{x_2}$ 可以通过上面动态方程进行求解表示
$\begin{aligned} \dot{x_1} = x_2\\ \dot{x_2} = \ddot{x} = \frac{1}{m}u - \frac{B}{m}\dot{x} - \frac{K}{m}x = \frac{1}{m}u - \frac{B}{m}x_2 - \frac{K}{m}x_1 \end{aligned}$

现在就用 2 个 1 阶微分方程表示了 $x_1$ 和 $x_2$
如果还有两个状态变量的测量量，可以用 $z_1$ 和 $z_2$ 分别表示 $x_1$ 和 $x_2$ 的测量量。
$\begin{aligned} z_1 = x = x_1 (position)\\ z_2 = \dot{x} = x_2 (speed)\\ \dot{x_1} = x_2\\ \dot{x_2} = \frac{1}{m}u - \frac{B}{m}x_2 - \frac{K}{m}x_1 \end{aligned}$

接下来我们用矩阵的形式将上面状态变量和测量变量表示出来就得到

$\begin{bmatrix} \dot{x_1}\\ \dot{x_2} \end{bmatrix} = \begin{bmatrix} 0 & 1\\ \frac{k}{m} & -\frac{B}{m} \end{bmatrix}\begin{bmatrix} x_1\\ x_2 \end{bmatrix} + \begin{bmatrix} 0\\ \frac{1}{m} \end{bmatrix} u$

$\begin{bmatrix} z_1\\ z_2 \end{bmatrix} = \begin{bmatrix} 1 & 0\\ 0 & 1 \end{bmatrix} \begin{bmatrix} x_1\\ x_2 \end{bmatrix}$
我们用矩阵形式表达出上面方程自然而然就联想到空间问题，这就是用状态空间来表示上面问题。

$\begin{aligned} X_k = AX_{k-1} + BU_k \\ Z_k = HX_k \end{aligned}$
这里的下标 k 表示状态变量对应的时刻，也就是表示采样时间。
真实情况充满了不缺定性，所以需要引入 $w_{k-1}$ 表示过程噪音，而 $v_k$ 表示测量噪音。

$\begin{aligned} X_k = AX_{k-1} + Bu_k + w_{k-1}\\ Z_k = HX_k + v_k \end{aligned}$

在模型也准确以及测量也不准确情况下如何估计出 $\hat{X_k}$ 这就是卡尔曼滤波器的用途，就是根据不准确的模型和不准确的测量值来估计出一个相对准确的估计值。