状态估计和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}$$