卡尔曼滤波

卡尔曼滤波法

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

通俗地讲，对系统 \(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} \]

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