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

时间:2022-11-04 13:05:29

状态估计的概率解释

运动和观测方程:

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