机器人学中的状态估计批量形式

线性高斯系统的状态估计#

离散批量优化#

运动和观测方程#

在离散时间线性时变的条件下，定义运动和观测方程：

$$x_k=A_{k-1}x_{k-1}+v_k+w_k,k=1,\cdots,K \\ y_k=C_kx_k+n_k,k=0,\cdots,K$$

$v_k$ 是确定性变量，其他都是随机变量。噪声和初始状态一般假设为互不相关，并且在各个时刻与自己也互不相关。$A_k$ 为转移矩阵，$C_k$ 为观测矩阵。

最大后验估计#

最大后验估计的思想是使得在样本出现的条件下参数的后验概率最大化。也就是在已知输入 $v_k$ 和观测 $y_k$ 的条件下，系统状态 $x_k$ 概率最大化。即：

$$\hat{x}=arg \max_x \ p(x|v,y)$$

其中 $\hat{x}$ 表示后验估计，$x=x_{0:K}=(x_0,\cdots,x_K)$ ，$v=(\breve{x},v_{1:K})$ ，$y=y_{0:K}=(y_0,\cdots,y_K)$ 。

根据贝叶斯公式：

$$\hat{x}=arg \max_x \ p(x|v,y)=arg\max_x\frac{p(y|x)p(x|v)}{\int_xp(y|x)p(x|v)dx}=arg\max_x\ p(y|x)p(x|v)$$

假设所有时刻的噪声是无关的。上式中：

$$p(y|x)=\prod_{k=0}^KP(y_k|x_x) \\ p(x|v)=p(x_0|\breve{x_0})\prod_0^Kp(x_k|x_{k-1},v_k)$$

其中，

$$p(x_0|\breve{x_0})=\frac{1}{\sqrt{(2\pi)^Ndet\breve{P_0}}}exp(-\frac{1}{2}(x_0-\breve{x_0})^T\breve{P_0}^{-1}(x_0-\breve{x})) \\ p(x_k|x_{k-1},v_k)=\frac{1}{\sqrt{(2\pi)^NdetQ_k}}exp(-\frac{1}{2}(x_k-A_kx_{k-1}-v_k)^TQ_k^{-1}(x_k-A_kx_{k-1}-v_k)) \\ p(y_k|x_k)=\frac{1}{\sqrt{(2\pi)^MdetR_k}}exp(-\frac{1}{2}(y_k-C_kx_k)^TR^{-1}(y_k-C_kx_k))$$

上式中，$Q_k$ 是输入 $v_k$ 的噪声，$R_k$ 是观测的噪声。代入到最大后验估计可知：

$$arg \max_x p(y|x)p(x|v)=arg \max_x \prod_{k=0}^Kp(y_k|x_k)p(x_0|\breve{x_0})\prod_{k=1}^Kp(x_k|x_{k-1},v_k) \\ =arg \max_x \prod_{k=0}^K\frac{1}{\sqrt{(2\pi)^MdetR_k}}exp(-\frac{1}{2}(y_k-C_kx_k)^TR^{-1}(y_k-C_kx_k)) \\ \frac{1}{\sqrt{(2\pi)^Ndet\breve{P_0}}}exp(-\frac{1}{2}(x_0-\breve{x_0})^T\breve{P_0}^{-1}(x_0-\breve{x})) \\ \prod_{k=1}^K\frac{1}{\sqrt{(2\pi)^NdetQ_k}}exp(-\frac{1}{2}(x_k-A_kx_{k-1}-v_k)^TQ_k^{-1}(x_k-A_kx_{k-1}-v_k))$$

对最大后验估计取对数：

$$L(x)=\sum_{k=0}^K-\frac{1}{2}(y_k-C_kx_k)^TR^{-1}(y_k-C_kx_k)+ \\\sum_{k=1}^K(-\frac{1}{2}(x_k-A_kx_{k-1}-v_k)^TQ_k^{-1}(x_k-A_kx_{k-1}-v_k)) + (-\frac{1}{2}(x_0-\breve{x_0})^T\breve{P_0}^{-1}(x_0-\breve{x}))\\+(-\frac{K+1}{2}ln((2\pi)^MdetR_k)-\frac{K}{2}ln((2\pi)^NdetQ_k)-\frac{1}{2}ln((2\pi)^Ndet\breve{P_0}))$$

简化一下上式，上式共有四项，最后一项（最后用括号括起来的三个）与 $x$ 无关，可忽略，令第一项为 $\sum_{k=0}^KJ_{y,k}(x)$ ，第二项和第三项为 $\sum_{k=0}^KJ_{v,k}(x)$ 。所以整体的目标函数有：

$$J(x)=\sum_{k=0}^K(J_{v,k}(x)+J{y,k}(x))$$

由于取了对数的原因，所以最小化这个目标函数来求解 $x$ 的值。接下来我们可以把这个目标函数写成矩阵的形式。那么就有：

$$J(x)=\frac{1}{2}(z-Hx)^TW^{-1}(z-Hx)$$

对 $x^T$ 求导，并令其导数为 0:

$$\frac{\partial J(x)}{\partial x^T}=-HW^{-1}(z-Hx)=0$$