状态估计和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的用法

预测

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