状态估计和KalmanFilter公式的推导与应用( 二 ) _生活百科

这里我们使用\(\hat{x}_{k}\)表示后验分布，使用\(\tilde{x}_k\)表示先验分布。

卡尔曼滤波第一步：通过运动方程确认\(x_k\)的先验分布。这一步是线性的，高斯分布的线性变换依然是高斯分布，所以可以得到如下公式：
\[P(x_k|x_{k-1},x_0, u_{1:k}, z_{1:k-1}) =N(A_k\hat{x}_{k-1} + u_k, A_k\hat{P}_{k-1}A^T_k + R)\tag{2.5}\]

这里协方差的推导可以参考《概率论与数理统计》的P112页，关于n维正态随机变量的协方差矩阵。

这一步称为预测。可以记作：
\[\tilde{x}_k = A_k\hat{x}_{k-1} + u_k, \quad\tilde{P}_k = A_k\hat{P}_{k-1}A^T_k + R\tag{2.6}\]卡尔曼滤波第二步：根据观测方程，我们可以计算莫格时刻应该产生怎样的观测数据：
\[P(z_k| x_k) = N(C_kx_k, Q)
\tag{2.7}\]
我们已经假设状态量符合高斯分布，根据贝叶斯公式，可以得到如下公式：
\[N(\hat{x}_k, \hat{P}_k) =\etaN(C_kx_k, Q) \cdotN(\tilde{x}_k, \tilde{P}_k)\tag{2.8}\]两侧都是高斯分布，我们带入高斯分布的公式，只需要保证指数部分相同，无需理会前面的因子部分。可以得到如下公式：
\[{(x_k - \hat{x}_k)}^T {\hat{P}^{-1}}_k (x_k - \hat{x}_k) ={(z_k -C_kx_k)}^T \hat{Q}^{-1}_k (z_k - C_kx_k) +{(x_k - \tilde{x}_k)^T} {\tilde{P}^{-1}_{k}} (x_k - \tilde{x}_k)\tag{2.9}\]我们需要根据上述这个公式推导出\(\hat{x}_k\)和\(\hat{P}_k\) 。
这里是通过系数相等进行了化简，我在这里简写一下：
\[\left\lbrace\begin{array}{l}\hat{P}^{-1}_k =C^T_kQ^{-1}C_k + \tilde{P}^{-1}_k\\ \\-2\hat{x}^T_k\hat{P}^{-1}_kx_k =-2z^T_k Q^{-1}C_kx_k - 2\tilde{x}^T_k\tilde{P}^{-1}_kx_k\end{array}\right.\tag{2.10}\]我们记作\(K = \hat{P}_kC^T_kQ^{-1}\)得到如下公式：
\[\left\lbrace\begin{array}{l}\hat{P}_k =(I - KC_k) \tilde{P}_k\\ \\\hat{x}_k = \tilde{x}_k + K(z_k -C_k \hat{x}_k)\end{array}\right.\tag{2.11}\]这个部称为更新。
总结kalmanFilter的用法

预测
\[\tilde{x}_k = A_k\hat{x}_{k-1} + u_k, \quad\tilde{P}_k = A_k\hat{P}_{k-1}A^T_k + R\tag{2.12}\]
更新：先计算K（卡尔曼增益），然后更新后验概率的分布。
\[\begin{array}{l}K = \hat{P}_kC^T_k(C_k\tilde{P}_kC^T_k + Q_k)^{-1}\\ \\\hat{P}_k =(I - KC_k) \tilde{P}_k\\ \\\hat{x}_k = \tilde{x}_k + K(z_k -C_k \hat{x}_k)\end{array}\tag{2.13}\]

【状态估计和KalmanFilter公式的推导与应用】