机器人学中的状态估计（三）：线性高斯系统离散时间的批量估计问题

1. 定义

（1）首先，考虑离散时间线性时变系统，有如下的运动模型和观测模型：
$$\begin{gathered} 运动方程：x_k=A_{k-1}{x}_{k-1}+v_k+w_{k}k=1,\cdots ,K \\ 观测方程：y_k=C_k{x}_k+n_{k}k=0,\cdots ,K \end{gathered}$$
​ 各变量含义如下：

• 系统状态：$x_k\in R^N$（$k$时刻/当前时刻）
• 初始状态：$x_0\in R^N\sim \mathcal{N}({\check{x}}_0,{\check{p}}_0)$
• 输入：$v_k\in R^N$
• 过程噪声：$w_k\in R^N\sim \mathcal{N}(0,Q_k)$ 零均值高斯白噪声
• 测量：$y_k\in R^M$
• 测量噪声：$n_k\in R^M\sim \mathcal{N}(0,R_k)$

注意：

• 一般而言，系统是先经过运动模型，再经过观测模型。
• 只经过运动模型的系统状态称为先验（不含观测）；既经过运动模型，又经过观测模型的系统状态称为后验（包含观测）。
• 均值表示所估计的状态，方差表示该估计状态的不确定度。

（2）明确问题

​ 所谓的状态估计是指：在$k$个时间点上，基于初始的状态信息${\check{x}}_0$，一系列观测数据$y_{0:k,meas}$，一系列输入$v_{1:k}$，以及系统的运动模型和观测模型，来计算系统真实状态的估计值${\hat{x}}_k$。也就是说，已知：${\check{x}}_0，v_{1:k},y_{0:k,meas}$，求${\hat{x}}_k$。
（3）问题类别

• 批量式线性高斯系统状态估计：一次性使用所有时刻数据来推算所有时刻状态（实时性不强）
• 递归式线性高斯系统状态估计：使用上一时刻数据来推算下一时刻状态（可实时估计）

2. 估计方法

目的：结合初始信息、传入量、观测量，以及运动方程和观测方程，估计最优的系统状态${\hat{x}}_k$。那么在批量式情况下，并依据运动方程和观测方程可知，要求的是：第一，使概率密度函数$p(x|v,y)$取得最大值时的$x$（即为最优的系统状态$\hat{x}$）；第二，或者是求出概率密度函数$p(x|v,y)$的具体表达式（也就是要求出其均值和方差）。由于含有观测量$y$，因此所求得的均值就是后验均值，即系统的最优状态$\hat{x}$
 按照上述分析，批量式线性高斯系统状态估计可以有以下两种方法：

• 最大后验估计：求出使$p(x|v,y)$最大的$x$，即$\hat{x}=argmaxp(x|v,y)$。关键点：第一，以$x$为变量求出$p(x|v,y)$的形式（不一定是均值和协方差，也可以是其他式子的组合表示）；第二，依据优化理论求最值（导数为0）。
• 贝叶斯估计：求出$p(x|v,y)$的（均值、协方差）表达形式，求出的均值就是所估计的最优状态。

（1）最大后验估计（MAP）

目的：已知先验信息${\check{x}}_0$，以及所有时刻的输入$v$，观测$y$的条件下，推测出所有时刻的最优状态$\hat{x}$

​ 设：$x=x_{0:k}=(x_0,\cdots ,x_k)$，$v=({\check{x}}_0,v_{1:k})$，$y=y_{0:k}=(y_0,\cdots ,y_k)$，则问题转化为求：$\hat{x}=argmaxp(x|v,y)$

​ 由贝叶斯公式：
$$p(x|y)=\frac{p(y|x)p(x)}{p(y)}$$
​ 可类推出：
$$p(x|v,y)=\frac{p(y|x,v)p(x|v)}{p(y|v)}$$
​ 由于观测方程：$y_k=C_k{x}_k+n_k$中$y_k$与$v_k$无关，所以$p(y|x,v)=p(y|x)$。又由于分母中不含$x$，$p(y|v)$与$x$无关，则有：
$$\hat{x}=argmaxp(x|y,v)=argmaxp(y|x)\cdot p(x|v)$$
​ 因此，下面主要是求出$p(y|x)$与$p(x|v)$，进而通过优化理论求$argmax$

​ 假设：对于所有时刻$k=0,\cdots ,K$，所有的噪声项$w_k$和$n_k$之间是无关的，结合运动方程和观测方程，$p(y|x)$和$p(x|v)$可做如下分解：
$$\begin{gathered} p(y|x)=p(y_0,\cdots ,y_K|x_0,\cdots ,x_K)=\prod _{k=0}^{K}p(y_k|x_k) \\ p(x|v)=p(x_0,\cdots ,x_K|{\check{x}}_0,v_1,\cdots ,v_K)=p(x_0|{\check{x}}_0)\cdot \prod _{k=1}^{K}p(x_k|x_{k-1},v_k) \end{gathered}$$
对于分解后的$p(x|v)$、$p(y|x)$，使用具体的概率密度函数形式来表达，如下所示：

• 对于$p(x_0|{\check{x}}_0)$，由于已知的先验信息：$x_0\sim \mathcal{N}({\check{x}}_0,{\check{P}}_0)$，则：
  $$p(x_0|{\check{x}}_0)=\frac{1}{\sqrt{(2\pi {)}^{N}det({\check{P}}_0)}}exp(-\frac{1}{2}(x_0-{\check{x}}_0{)}^T{\check{P}}^{-1}(x_0-{\check{x}}_0))$$

• 对于$p(x_k|x_{k-1},v_k)$，由运动方程：$$x_k=A_{k-1}{x}_{k-1}+v_k+w_k(w_k\sim \mathcal{N}(0,Q_k))$$可知当$x_{k-1},v_k$已知时，有：
  $$\begin{gathered} E[x_k]=E[A_{k-1}{x}_{k-1}+v_k+w_k]=A_{k-1}{x}_{k-1}+v_k+0=A_{k-1}{x}_{k-1}+v_k \\ E[(x_k-E[x_k])(x_k-E[x_k]{)}^T]=E[w_k{w}_k^T]=Q_k \end{gathered}$$
  所以，$p(x_k|x_{k-1},v_k)$的表达式为：
  p ( x k ∣ x k − 1 , v k ) = 1 ( 2 π ) N d e t ( Q k ) e x p ( − 1 2 ( x k − A k − 1 x k − 1 − v k ) T Q k − 1 ( x k − A k − 1 x k − 1 − v k ) ) p(x_k|x_{k-1},v_k) = \frac{1}{\sqrt{(2\pi)^Ndet(Q_k)}}exp(-\frac{1}{2}(x_k-A_{k-1}x_{k-1}-v_k)^TQ_k^{-1}(x_k-A_{k-1}x_{k-1}-v_k)) p(xk​∣xk−1​,vk​)=(2π)Ndet(Qk​) ​1​exp(−21​(x