卡尔曼滤波

卡尔曼滤波法

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

通俗地讲，对系统 (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}^- = Ahat 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 -Hhat x_{k}^-) ]

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

   [P_k = (I-K_kH)P_k^- ]

   (I) 为单位矩阵。

卡尔曼增益推导

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

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

我们的主要目的是尽量缩小卡尔曼估计值 (hat x_k) 与真实值 (x_k) 的误差大小，可推导出两者的差值为：

[egin{aligned} x_k-hat x_{{k}} &= x_k - (hat x_{k}^- + K_k(z_k - Hhat x_{k}))\ &= x_k - hat x_k^- - K_k(Hx_k + v_k) + K_kHhat 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) 表示预测值与真实值的误差，称为估计误差。

则有

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

对括号内每一项求期望得

[P_k = (1- K_kH)E(hat e_khat e_k^T)(I-K_kH)^T + K_kE(v_kv_k^T)K_k^T ]

令 (P_k^- = E(hat e_khat 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 ]

则执行求導：

[egin{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} ]

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