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

状态估计的概率解释

运动和观测方程：

(1.1){xk=f(xk1,uk)+wkzk=h(yj,xk)+vk,jk=1,…,N,j=1,…,M" role="presentation">{xk=f(xk1,uk)+wkzk=h(yj,xk)+vk,jk=1,…,N,j=1,…,M(1.1)$$\begin{array}{}\text{(1.1)}& \{\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}k=1,\dots ,N,j=1,\dots ,M\end{array}$$

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

• xk" role="presentation">xk$x_k$ ：机器人的位姿。

• uk" role="presentation">uk$u_k$ ：系统在k时刻的输入量。

• wk" role="presentation">wk$w_k$：位姿变化的随机噪声。

• zk" role="presentation">zk$z_k$ ：系统的观测值，传感器采集的观测数据。

• yj" role="presentation">yj$y_j$ ：路标，或者说是观测点。

• vk,j" role="presentation">vk,j$v_{k,j}$：观测过程中的随机噪声。

我们的目标则是利用系统在k时刻的输入量uk" role="presentation">uk$u_k$和系统的观测量zk" role="presentation">zk$z_k$，估计机器的位姿xk" role="presentation">xk$x_k$和路标点yj" role="presentation">yj$y_j$的概率分布。

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

由于位姿xk" role="presentation">xk$x_k$和路标点yj" role="presentation">yj$y_j$都是需要我们估计的变量，这里我们改变符号的意义。令xk" role="presentation">xk$x_k$为k时刻所有的未知量，记作：

(1.2)xk=def{xk,y1,…,ym}" role="presentation">xk=def{xk,y1,…,ym}(1.2)$$\begin{array}{}\text{(1.2)}& x_k\stackrel{\text{def}}{=}\{x_k,y_1,\dots ,y_m\}\end{array}$$

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

(1.3){xk=f(xk1,uk)+wkzk=h(xk)+vk,jk=1,…,N" role="presentation">{xk=f(xk1,uk)+wkzk=h(xk)+vk,jk=1,…,N(1.3)$$\begin{array}{}\text{(1.3)}& \{\begin{array}{l}{x}_k=f(x_{k_1},u_k)+w_k\\ z_k=h(x_k)+v_{k,j}\end{array}k=1,\dots ,N\end{array}$$

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

(1.4)P(xk|x0,u1:k,z1:k)" role="presentation">P(xk|x0,u1:k,z1:k)(1.4)$$\begin{array}{}\text{(1.4)}& P(x_k|x_0,u_{1:k},z_{1:k})\end{array}$$

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

(1.5)P(xk|x0,u1:k,z1:k)∝P(zk|xk)P(xk|x0,u1:k,z1:k−1)" role="presentation">P(xk|x0,u1:k,z1:k)∝P(zk|xk)P(xk|x0,u1:k,z1:k−1)(1.5)$$\begin{array}{}\text{(1.5)}& 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})\end{array}$$

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

(1.6)P(xk|x0,u1:k,z1:k−1)=∫P(xk|xk−1,x0,u1:k,z1:k−1)P(xk−1|x0,u1:k,z1:k−1)dxk−1" role="presentation">P(xk|x0,u1:k,z1:k−1)=∫P(xk|xk−1,x0,u1:k,z1:k−1)P(xk−1|x0,u1:k,z1:k−1)dxk−1(1.6)$$\begin{array}{}\text{(1.6)}& 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})d{x}_{k-1}\end{array}$$

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

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

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

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

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

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

线性系统和卡尔曼滤波

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

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

  (2.1)P(xk|xk−1,x0,u1:k,z1:k−1)=P(xk|xk−1,u1:k)" role="presentation">P(xk|xk−1,x0,u1:k,z1:k−1)=P(xk|xk−1,u1:k)(2.1)$$\begin{array}{}\text{(2.1)}& P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})=P(x_k|x_{k-1},u_{1:k})\end{array}$$

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

  (2.2)P(xk−1|x0,u1:k,z1:k−1)=P(xk−1|x0,u1:k−1,z1:k−1)" role="presentation">P(xk−1|x0,u1:k,z1:k−1)=P(xk−1|x0,u1:k−1,z1:k−1)(2.2)$$\begin{array}{}\text{(2.2)}& 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})\end{array}$$

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

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

(2.3){xk=Akxk−1+uk+wkzk=Ckxk+vk,jk=1,…,N" role="presentation">{xk=Akxk−1+uk+wkzk=Ckxk+vk,jk=1,…,N(2.3)$$\begin{array}{}\text{(2.3)}& \{\begin{array}{l}{x}_k=A_k{x}_{k-1}+u_k+w_k\\ z_k=C_k{x}_k+v_{k,j}\end{array}k=1,\dots ,N\end{array}$$

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

(2.4)wk∼N(0,R)vk∼N(0,Q)" role="presentation">wk∼N(0,R)vk∼N(0,Q)(2.4)$$\begin{array}{}\text{(2.4)}& w_k\sim N(0,R)v_k\sim N(0,Q)\end{array}$$

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

这里我们使用x^k" role="presentation">x^k${\hat{x}}_k$表示后验分布，使用x~k" role="presentation">x~k${\tilde{x}}_k$表示先验分布。

卡尔曼滤波第一步： 通过运动方程确认xk" role="presentation">xk$x_k$的先验分布。这一步是线性的，高斯分布的线性变换依然是高斯分布，所以可以得到如下公式：

(2.5)P(xk|xk−1,x0,u1:k,z1:k−1)=N(Akx^k−1+uk,AkP^k−1AkT+R)" role="presentation">P(xk|xk−1,x0,u1:k,z1:k−1)=N(Akx^k−1+uk,AkP^k−1ATk+R)(2.5)$$\begin{array}{}\text{(2.5)}& 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}_k^T+R)\end{array}$$

这里协方差的推导可以参考《概率论与数理统计》的P112页，关于n维正态随机变量的协方差矩阵。

这一步称为预测。可以记作：

(2.6)x~k=Akx^k−1+uk,P~k=AkP^k−1AkT+R" role="presentation">x~k=Akx^k−1+uk,P~k=AkP^k−1ATk+R(2.6)$$\begin{array}{}\text{(2.6)}& {\tilde{x}}_k=A_k{\hat{x}}_{k-1}+u_k,{\tilde{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+R\end{array}$$

卡尔曼滤波第二步： 根据观测方程，我们可以计算莫格时刻应该产生怎样的观测数据：

(2.7)P(zk|xk)=N(Ckxk,Q)" role="presentation">P(zk|xk)=N(Ckxk,Q)(2.7)$$\begin{array}{}\text{(2.7)}& P(z_k|x_k)=N(C_k{x}_k,Q)\end{array}$$

我们已经假设状态量符合高斯分布，根据贝叶斯公式，可以得到如下公式：

(2.8)N(x^k,P^k)=ηN(Ckxk,Q)⋅N(x~k,P~k)" role="presentation">N(x^k,P^k)=ηN(Ckxk,Q)⋅N(x~k,P~k)(2.8)$$\begin{array}{}\text{(2.8)}& N({\hat{x}}_k,{\hat{P}}_k)=\eta N(C_k{x}_k,Q)\cdot N({\tilde{x}}_k,{\tilde{P}}_k)\end{array}$$

两侧都是高斯分布，我们带入高斯分布的公式，只需要保证指数部分相同，无需理会前面的因子部分。可以得到如下公式：

(2.9)(xk−x^k)TP^−1k(xk−x^k)=(zk−Ckxk)TQ^k−1(zk−Ckxk)+(xk−x~k)TP~k−1(xk−x~k)" role="presentation">(xk−x^k)TP^−1k(xk−x^k)=(zk−Ckxk)TQ^−1k(zk−Ckxk)+(xk−x~k)TP~−1k(xk−x~k)(2.9)$$\begin{array}{}\text{(2.9)}& {(x_k-{\hat{x}}_k)}^T{{\hat{P}}^{-1}}_k(x_k-{\hat{x}}_k)={(z_k-C_k{x}_k)}^T{\hat{Q}}_k^{-1}(z_k-C_k{x}_k)+(x_k-{\tilde{x}}_k{)}^T{\tilde{P}}_k^{-1}(x_k-{\tilde{x}}_k)\end{array}$$

我们需要根据上述这个公式推导出x^k" role="presentation">x^k${\hat{x}}_k$和P^k" role="presentation">P^k${\hat{P}}_k$。

这里是通过系数相等进行了化简，我在这里简写一下：

(2.10){P^k−1=CkTQ−1Ck+P~k−1−2x^kTP^k−1xk=−2zkTQ−1Ckxk−2x~kTP~k−1xk" role="presentation">⎧⎩⎨⎪⎪⎪⎪⎪⎪P^−1k=CTkQ−1Ck+P~−1k−2x^TkP^−1kxk=−2zTkQ−1Ckxk−2x~TkP~−1kxk(2.10)$$\begin{array}{}\text{(2.10)}& \{\begin{array}{l}{\hat{P}}_k^{-1}=C_k^T{Q}^{-1}{C}_k+{\tilde{P}}_k^{-1}\\ \\ -2{\hat{x}}_k^T{\hat{P}}_k^{-1}{x}_k=-2z_k^T{Q}^{-1}{C}_k{x}_k-2{\tilde{x}}_k^T{\tilde{P}}_k^{-1}{x}_k\end{array}\end{array}$$

我们记作K=P^kCkTQ−1" role="presentation">K=P^kCTkQ−1$K={\hat{P}}_k{C}_k^T{Q}^{-1}$得到如下公式：

(2.11){P^k=(I−KCk)P~kx^k=x~k+K(zk−Ckx^k)" role="presentation">⎧⎩⎨⎪⎪P^k=(I−KCk)P~kx^k=x~k+K(zk−Ckx^k)(2.11)$$\begin{array}{}\text{(2.11)}& \{\begin{array}{l}{\hat{P}}_k=(I-K{C}_k){\tilde{P}}_k\\ \\ {\hat{x}}_k={\tilde{x}}_k+K(z_k-C_k{\hat{x}}_k)\end{array}\end{array}$$

这个部称为更新。

总结kalmanFilter的用法

1. 预测

   (2.12)x~k=Akx^k−1+uk,P~k=AkP^k−1AkT+R" role="presentation">x~k=Akx^k−1+uk,P~k=AkP^k−1ATk+R(2.12)$$\begin{array}{}\text{(2.12)}& {\tilde{x}}_k=A_k{\hat{x}}_{k-1}+u_k,{\tilde{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+R\end{array}$$

2. 更新：先计算K（卡尔曼增益）， 然后更新后验概率的分布。

   (2.13)K=P^kCkT(CkP~kCkT+Qk)−1P^k=(I−KCk)P~kx^k=x~k+K(zk−Ckx^k)" role="presentation">K=P^kCTk(CkP~kCTk+Qk)−1P^k=(I−KCk)P~kx^k=x~k+K(zk−Ckx^k)(2.13)$$\begin{array}{}\text{(2.13)}& \begin{array}{l}K={\hat{P}}_k{C}_k^T(C_k{\tilde{P}}_k{C}_k^T+Q_k{)}^{-1}\\ \\ {\hat{P}}_k=(I-K{C}_k){\tilde{P}}_k\\ \\ {\hat{x}}_k={\tilde{x}}_k+K(z_k-C_k{\hat{x}}_k)\end{array}\end{array}$$