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

状态估计的概率解释

运动和观测方程：

\[\left\lbrace \begin{array}{l} x_k = f(x_{k_1}, u_k) + w_k \\ z_k = h(y_j, x_k) + v_{k,j} \end{array} \right. \qquad {k = 1,\dots,N, j = 1,\dots,M} \tag{1.1} \]

其中，各个参数的含义如下：

• \(x_k\) ：机器人的位姿。

• \(u_k\) ：系统在k时刻的输入量。

• \(w_k\)：位姿变化的随机噪声。

• \(z_k\) ：系统的观测值，传感器采集的观测数据。

• \(y_j\) ：路标，或者说是观测点。

• \(v_{k,j}\)：观测过程中的随机噪声。

我们的目标则是利用系统在k时刻的输入量\(u_k\)和系统的观测量\(z_k\)，估计机器的位姿\(x_k\)和路标点\(y_j\)的概率分布。

在比较常见且合理的情况下，我们可以假设状态量和噪声项服从高斯分布——这意味这我们在程序中只需要存储他们的均值和协方差矩阵即可。均值可以看作变量的最最优估计，协方差则可以度量变量的不确定性。

由于位姿\(x_k\)和路标点\(y_j\)都是需要我们估计的变量，这里我们改变符号的意义。令\(x_k\)为k时刻所有的未知量，记作：

\[x_k \overset{\text{def}}{=} \{x_k, y_1,\dots,y_m\} \tag{1.2} \]

根据上述(1.1)和(1.2)可以将运动方程和观测方程写成如下形式：

\[\left\lbrace \begin{array}{l} x_k = f(x_{k_1}, u_k) + w_k \\ z_k = h(x_k) + v_{k,j} \end{array} \right. \qquad {k = 1,\dots,N} \tag{1.3} \]

现在考虑第k时刻的情况，我们希望使用过去0到时刻的数据来估计现在的状态分布：

\[P(x_k|x_0,u_{1:k}, z_{1:k}) \tag{1.4} \]

根据贝叶斯公式，可以得到如下公式：

\[P(x_k|x_0,u_{1:k}, z_{1:k}) \propto P(z_k|x_k) P(x_k|x_0,u_{1:k}, z_{1:k-1}) \tag{1.5} \]

这里的第一项称为似然，第二项称为先验。似然由观测方程给定，而先验部分，\(x_k\)是基于过去所有状态估计而来的。至少，它会受到\(x_{k-1}\)的影响，于是我们以\(x_{k-1}\)时刻为条件概率展开：

\[P(x_k|x_0,u_{1:k}, z_{1:k-1})= \int P(x_k|x_{k-1},x_0, u_{1:k}, z_{1:k-1}) P(x_{k-1}|x_0,u_{1:k}, z_{1:k-1}) dx_{k-1} \tag{1.6} \]

对于后续的操作，有很多的方法。

• 其中一种方法就是假设马尔可夫性。

  一阶马尔可夫性： k时刻的状态只和k-1时刻的状态有关，而与再之前的无关。

  如果这样假设，我们得到的是以扩展卡尔曼滤波（EKF）为代表的滤波器方法。

• 另一种方法是依然考虑k时刻和之前所有状态的关系，此时得到的是非线性优化为主体的优化框架。

在这里，我们先了解卡尔曼滤波的原理和应用。

线性系统和卡尔曼滤波

根据上文，我们假设了这个系统符合马尔可夫性，我们可以对公式(1.6)做出一些简化。

• 公式右侧第一部分可以简化成如下形式：

  \[P(x_k|x_{k-1},x_0, u_{1:k}, z_{1:k-1}) = P(x_k|x_{k-1}, u_{1:k}) \tag{2.1} \]

• 公式右侧第二部分：(已知k时刻只和k-1时刻的状态相关)

  \[P(x_{k-1}|x_0,u_{1:k}, z_{1:k-1}) = P(x_{k-1}|x_0,u_{1:k-1}, z_{1:k-1}) \tag{2.2} \]

观察上述公式，我们可以知道，我们实际上在做“如何把k-1时刻的状态分布推导至k时刻”这一件事请。

我们假设状态量服从高斯分布，从最简单的线性高斯系统开始，得到如下公式：

\[\left\lbrace \begin{array}{l} x_k = A_kx_{k-1} + u_k + w_k \\ z_k = C_kx_k + v_{k,j} \end{array} \right. \qquad {k = 1,\dots,N} \tag{2.3} \]

假设所有的状态和噪声都符合高斯分布，这里的噪声可以记作：（这里省略了R和Q的下标）

\[w_k \sim N(0, R) \quad v_k \sim N(0, Q) \tag{2.4} \]

利用马尔可夫性，假设我们已知k-1时刻的状态，也就是k-1时刻的后验状态估计\(\hat{x}_{k-1}\)及其协方差\(\hat{P}_{k-1}\)，现在要根据k时刻的输入，确认\(x_k\)的后验。

这里我们使用\(\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) = \eta N(C_kx_k, Q) \cdot N(\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的用法

1. 预测

   \[\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} \]

2. 更新：先计算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} \]