卡尔曼滤波

卡尔曼滤波法

卡尔曼滤波算法是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，是一种最优化自回归数据处理算法。

通俗地讲，对系统 $k-1$ 时刻的状态，我们有两种途径来获得系统 $k$ 时刻的状态。一种是根据常识或者系统以往的状态表现来预测 $k$ 时刻的状态，这个量我们称之为预测量；另一种是通过传感器等进行系统 $k$ 时刻我们所观测变量状态的测量，这个量我们称之为观测量。

显然，两种途径均有误差，而卡尔曼滤波要做的事，就是结合这两个结果，滤去两种结果中的“噪声”，得到一个更加准确的系统 $k$ 时刻的状态的估计。下面就来看卡尔曼滤波算法具体是怎么做到的。

系统状态预测方程

$$X(k) = AX(k-1) + BU(k) + W(k-1)$$

系统状态观测方程

$$Z(k) = HX(k) + V(k)$$

卡尔曼滤波的计算过程

卡尔曼滤波向前的推进包含预测和观测过程。

下面的计算式中用 $\hat x_k^-$ 表示 $k$ 时刻的预测值， $z_k$ 表示 $k$ 时刻的观测值，$\hat x_k$ 表示 $k$ 时刻的卡尔曼估计值, $x_k$ 表示系统 $k$ 时刻的真实状态值。

时间更新（预测）

1. 根据上述的系统状态预测方程计算出 $k$ 时刻的系统状态

   $$\hat{x}_{k}^- = A\hat x_{k-1} + Bu_{k-1}$$

2. 计算 $k$ 时刻的误差的协方差

   $$P_k^- = AP_{k-1}A^T + Q$$

测量更新（校正）

3. 计算卡尔曼增益

   $$K_k = \frac{P_k^- H^T}{HP_k^-H^T + R}$$

   上面的 $P_k^-$ 即为 $k$ 时刻的预测误差, $R$ 为 $k$ 时刻的系统观测误差。

4. 校正预测得到的 $k$ 时刻的系统状态

   $$\hat x_k = \hat x_k^- + K_k(z_k -H\hat x_{k}^-)$$

5. 更新测量误差的协方差

   $$P_k = (I-K_kH)P_k^-$$

   $I$ 为单位矩阵。

卡尔曼增益推导

显然，卡尔曼滤波作为一种数据融合算法，其核心在于观测量和预测量的比例取值，也就是卡尔曼增益。

由于推导的数学过程比较复杂， 留着以后推导。

我们的主要目的是尽量缩小卡尔曼估计值 $\hat x_k$ 与真实值 $x_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_k(Hx_k + v_k) + K_kH\hat x_k\\ &= (1-K_kH)x_k - (I-K_kH)\hat x_k -K_kv_k\\ &= (I-K_kH)(x_k - \hat x_k^-) - K_k v_k \end{aligned}$$

令 $e_k = x_k-\hat x_k$，由于 $x_k-\hat x_k^-$ 和 $v_k$ 均可以视作服从高斯分布的噪声，所以 $e_k$ 总体服从高斯分布， $e_k ～ N(0, P_k)$

易知，要使卡尔曼估计值尽可能接近真实值，$e_k$ 的方差应该尽可能小。

因此求卡尔曼增益的问题转化成了求

$$P_k = f(K_k)$$

取最小值时 $K_k$ 的取值。

由于均值 $\mu = 0$，根据方差的定义， $P_k = E(e_ke_k^T)$。

接下来就是将 $e_k = (I - K_kH)(x_k-\hat x_k^-) - K_kv_k$ 带入上式展开，比较复杂，直接写结果：

我们用 $\hat e_k$ 表示预测值与真实值的误差，称为估计误差。

则有

$$\begin{aligned} P_k = E\{[(I&-K_kH)\hat e_k\hat e_k^T(1-K_kH)^T - (I-K_kH)\hat e_kv_k^TK_k^T \\&- K_kv_k\hat e_k^T(1-K_kH)^T + K_kv_kv_k^TK_k] \} \end{aligned}$$

对括号内每一项求期望得

$$P_k = (1- K_kH)E(\hat e_k\hat e_k^T)(I-K_kH)^T + K_kE(v_kv_k^T)K_k^T$$

令 $P_k^- = E(\hat e_k\hat e_k^T)$，称为系统的预测误差协方差矩阵，或者估计误差协方差矩阵；$R = E(v_kv_k^T)$，称为系统的测量误差协方差矩阵，其之所以没有 $k$ 的下标，是因为基本可以认为测量的误差基本不随时间变化。

则上式转化为

$$P_k = (P_k^- - K_kCP_k^-)(I-H^TK_k^T) + K_kRK_k^T$$

要使 $e_k$ 的方差最小，则使 $P_k$ 的迹最小，即

$$\frac{\mathrm d\ \mathrm{tr}(P_k)}{\mathrm d K_k} = 0$$

引入两个对迹的求導公式

$$\frac{\mathrm d\ \mathrm{tr}(AB)}{\mathrm d A}= B^T\\ \frac{\mathrm{d\ tr}(ABA^T)}{\mathrm{d} A} = 2AB$$

则执行求導：

$$\begin{aligned} \frac{\mathrm{d\ tr}(P_k)}{\mathrm d K_k} &= 0 - 2(HP_k^-)^T + 2K_kHP_k^-C^T + 2K_kR =0\\ K_k &= \frac{P_k^- H^T}{HP_k^-H^T + R} \end{aligned}$$

由此，卡尔曼增益正式被推导出来。