卡尔曼滤波
卡尔曼滤波法
卡尔曼滤波算法是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，是一种最优化自回归数据处理算法。
通俗地讲，对系统 \(k-1\) 时刻的状态，我们有两种途径来获得系统 \(k\) 时刻的状态。一种是根据常识或者系统以往的状态表现来预测 \(k\) 时刻的状态，这个量我们称之为预测量；另一种是通过传感器等进行系统 \(k\) 时刻我们所观测变量状态的测量，这个量我们称之为观测量。
显然，两种途径均有误差，而卡尔曼滤波要做的事，就是结合这两个结果，滤去两种结果中的“噪声”，得到一个更加准确的系统 \(k\) 时刻的状态的估计。下面就来看卡尔曼滤波算法具体是怎么做到的。
系统状态预测方程
系统状态观测方程
卡尔曼滤波的计算过程
卡尔曼滤波向前的推进包含预测和观测过程。
下面的计算式中用 \(\hat x_k^-\) 表示 \(k\) 时刻的预测值， \(z_k\) 表示 \(k\) 时刻的观测值，\(\hat x_k\) 表示 \(k\) 时刻的卡尔曼估计值, \(x_k\) 表示系统 \(k\) 时刻的真实状态值。
时间更新（预测）
-
根据上述的系统状态预测方程计算出 \(k\) 时刻的系统状态
\[\hat{x}_{k}^- = A\hat x_{k-1} + Bu_{k-1} \] -
计算 \(k\) 时刻的误差的协方差
\[P_k^- = AP_{k-1}A^T + Q \]
测量更新（校正）
-
计算卡尔曼增益
\[K_k = \frac{P_k^- H^T}{HP_k^-H^T + R} \]上面的 \(P_k^-\) 即为 \(k\) 时刻的预测误差, \(R\) 为 \(k\) 时刻的系统观测误差。
-
校正预测得到的 \(k\) 时刻的系统状态
\[\hat x_k = \hat x_k^- + K_k(z_k -H\hat x_{k}^-) \] -
更新测量误差的协方差
\[P_k = (I-K_kH)P_k^- \]\(I\) 为单位矩阵。
卡尔曼增益推导
显然，卡尔曼滤波作为一种数据融合算法，其核心在于观测量和预测量的比例取值，也就是卡尔曼增益。
由于推导的数学过程比较复杂， 留着以后推导。
我们的主要目的是尽量缩小卡尔曼估计值 \(\hat x_k\) 与真实值 \(x_k\) 的误差大小，可推导出两者的差值为：
令 \(e_k = x_k-\hat x_k\)，由于 \(x_k-\hat x_k^-\) 和 \(v_k\) 均可以视作服从高斯分布的噪声，所以 \(e_k\) 总体服从高斯分布， \(e_k ～ N(0, P_k)\)
易知，要使卡尔曼估计值尽可能接近真实值，\(e_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\) 表示预测值与真实值的误差，称为估计误差。
则有
对括号内每一项求期望得
令 \(P_k^- = E(\hat e_k\hat e_k^T)\)，称为系统的预测误差协方差矩阵，或者估计误差协方差矩阵；\(R = E(v_kv_k^T)\)，称为系统的测量误差协方差矩阵，其之所以没有 \(k\) 的下标，是因为基本可以认为测量的误差基本不随时间变化。
则上式转化为
要使 \(e_k\) 的方差最小，则使 \(P_k\) 的迹最小，即
引入两个对迹的求導公式
\[\frac{\mathrm d\ \mathrm{tr}(AB)}{\mathrm d A}= B^T\\ \frac{\mathrm{d\ tr}(ABA^T)}{\mathrm{d} A} = 2AB \]
则执行求導：
由此，卡尔曼增益正式被推导出来。
