状态估计和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})\proptoP(z_k|x_k) P(x_k|x_0,u_{1:k}, z_{1:k-1})\tag{1.5}\]【状态估计和KalmanFilter公式的推导与应用】这里的第一项称为似然，第二项称为先验。似然由观测方程给定，而先验部分，\(x_k\)是基于过去所有状态估计而来的。至少，它会受到\(x_{k-1}\)的影响，于是我们以\(x_{k-1}\)时刻为条件概率展开：
\[P(x_k|x_0,u_{1:k}, z_{1:k-1})=\intP(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) \quadv_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}\]