马尔可夫定位

摘自《概率机器人》

1. 定位原理

\qquad初始置信度$bel(x_0)$反映机器人位姿的初始信息。初始位姿只是经常已知的近似值，使用位于中心${\overline{x}}_0$的窄高斯分布初始化。定义为：
bel(x0)=det(2πΣ)−12exp{−12(x0−x‾0)TΣ−1(x0−x‾0)} bel(x_{0})={det(2\pi\Sigma)^{-\frac{1}{2}}exp\{-\frac{1}{2}(x_{0}-\overline{x}_{0})^{T}\Sigma^{-1}(x_{0}-\overline{x}_{0})\}} bel(x0​)=det(2πΣ)−21​exp{−21​(x0​−x0​)TΣ−1(x0​−x0​)}式中，$\Sigma$为初始位置不确定性协方差。
\qquad当机器人向右移动时，置信度与运动模型$p(x_t|u_t,x_t)$做卷积运算，测量结果使用感知概率$p(z_t|x_t)$乘以当前的置信度。(还是贝叶斯滤波！！！)
$$\begin{gathered} \overline{bel}(x_t)=\int p(x_t|u_t,x_{t-1})bel(x_{t-1})d(x_{t-1}) \\ bel(x_t)=\eta \overline{bel}(x_t)p(z_t|x_t)d(x_t) \end{gathered}$$

2.使用扩展卡尔曼(EKF)滤波实现定位算法

\qquad实现定位算法的难点在于使用泰勒展开得到近似函数、整合控制和运动噪声、由控制空间到运动空间的转化(！！！)。

2.1 预测步骤

\qquad使用$x_{t-1}=(x,y,\theta {)}^T$和$x_t=(x^{{}^{\prime }},y^{{}^{\prime }},{\theta }^{{}^{\prime }}{)}^T$分别表示在时刻$t-1$和$t$的状态向量。真实的运动使用$(\hat{v},\hat{\omega }{)}^T$表示，分别为线速度和角速度。控制模型由运动控制$u_t=(v,\omega {)}^T$和高斯白噪声产生：
$$\left(\begin{array}{c}\hat{v}\\ \hat{\omega }\end{array}\right)=\left(\begin{array}{c}v\\ \omega \end{array}\right)+\left(\begin{array}{c}{\epsilon }_{{\alpha }_1{v}_t^2+{\alpha }_2{\omega }_t^2}\\ {\epsilon }_{{\alpha }_3{v}_t^2+{\alpha }_4{\omega }_t^2}\end{array}\right)=\left(\begin{array}{c}v\\ \omega \end{array}\right)+\mathcal{N}(0,{\mathbf{M}}_t)$$\qquad将控制模型分解为有噪声分量和无噪声分量。同样的方法分解运动模型：
$$\begin{gathered} \left(\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ {\theta }^{{}^{\prime }}\end{array}\right)=\underbrace{\left(\begin{array}{c}{x}^{}\\ y^{}\\ {\theta }^{}\end{array}\right)+\left(\begin{array}{c}-\frac{v_t}{{\omega }_t}sin\theta +\frac{v_t}{{\omega }_t}sin(\theta +{\omega }_t\Delta t)\\ \frac{v_t}{{\omega }_t}cos\theta -\frac{v_t}{{\omega }_t}cos(\theta +{\omega }_t\Delta t)\\ {\omega }_t\Delta t\end{array}\right)}+\mathcal{N}(0,{\mathbf{R}}_t) \\ g(u_t,x_{t-1}) \end{gathered}$$\qquad此时$x_t=g(u_t,x_{t-1})+\epsilon$对比$x_t=A{x}_{t-1}+B{u}_t+\epsilon$为非线性，使用泰勒展开线性近似函数$g$为：
$$g(u_t,x_{t-1})\approx g(u_t,{\mu }_{t-1})+G_t(x_{t-1}-{\mu }_{t-1})$$\qquad函数$g(u_t,{\mu }_{t-1})$通过用已知的期望${\mu }_{t-1}$替换位置的准确状态$x_{t-1}$，雅可比矩阵$G_t$是函数$g$在$u_t,{\mu }_{t-1}$关于$x_{t-1}$的导数：
$$G_t=\frac{\partial g(u_t,{\mu }_{t-1})}{\partial x_{t-1}}$$\qquad式中，使用${\mu }_{t-1}=({\mu }_{t-1,x}{\mu }_{t-1,y}{\mu }_{t-1,\theta }{)}^T$表示平均估计，带入$G_t$中。为了导出附加运动噪声$\mathcal{N}(0,{\mathbf{R}}_t)$，首先确定控制模型噪声协方差矩阵${\mathbf{M}}_t$，直接来自控制模型：
$${\mathbf{M}}_t=\left(\begin{array}{cc}{\alpha }_1{v}_t^2+{\alpha }_2{\omega }_t^2& 0\\ 0& {\alpha }_3{v}_t^2+{\alpha }_4{\omega }_t^2\end{array}\right)$$\qquad$求解运动模型噪声就是将控制噪声从控制空间映射到状态空间！！！！！！$。

$${\mathbf{V}}_t=\frac{\partial g(u_t,{\mu }_{t-1})}{\partial u_t}$$\qquad近似映射为${\mathbf{V}}_t{\mathbf{M}}_t{\mathbf{V}}_t^T$，协方差变化在下图第 7行。

2.2 修正步骤

\qquad测量模型也需要线性化
$$h({\mathbf{x}}_t,j,\mathbf{m})\approx h(\overline{\mathbf{\mu }},j,\mathbf{m})+{\mathbf{H}}_t^i({\mathbf{x}}_t-{\overline{\mathbf{\mu }}}_t)$$\qquad其中，$j$表示测量向量对应地标的身份，$\mathbf{m}$表示探测到第$i$个地标的坐标，${\mathbf{H}}_t^i$为$h$关于机器人位置的雅可比矩阵，使用预测的平均${\overline{\mathbf{\mu }}}_t$计算：
$${\mathbf{H}}_t^i=\frac{\partial h({\overline{\mathbf{\mu }}}_t,j,\mathbf{m})}{\partial x_t}$$\qquad测量噪声的协方差$Q_t$
$$Q_t=\left(\begin{array}{ccc}{\sigma }_r^2& 0& 0\\ 0& {\sigma }_{\varphi }^2& 0\\ 0& 0& {\sigma }_s^2\end{array}\right)$$\qquad第14-17行为测量更新。

2.3 测量似然

\qquad对扩展卡尔曼滤波并不必要，但对去除异常值或未知一致性的情况很有必要。测量$z_t$的似然为$p(z_t|c_{1:t},\mathbf{m},{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})$，由预测的置信度$\overline{bel}(x_t)=\mathcal{N}(x_t,{\overline{\mu }}_t,{\overline{\Sigma }}_t)$来计算似然。
$$p(z_t|c_{1:t},\mathbf{m},z_{1:t-1},{\mathbf{u}}_{1:t})=\int p(z_t^i|x_t,c_t^i,m)\overline{bel}(x_t)d{x}_t$$\qquad积分式中左项为位置信息$x_t$已知的测量似然。似然由高斯分布给出，测量值由测量函数$h$获得，协方差由测量噪声$Q_t$获得。
$$p(z_t^i|{\mathbf{x}}_t,c_t^i,\mathbf{m})\sim \mathcal{N}(z_t^i;h({\mathbf{x}}_i,c_t^i,\mathbf{m}),Q_t)\approx \mathcal{N}(z_t^i;h({\overline{\mathbf{\mu }}}_t,c_t^i,\mathbf{m})+{\mathbf{H}}_t({\mathbf{x}}_t-{\overline{\mathbf{\mu }}}_t),Q_t)$$\qquad使用高斯形式代替$\overline{bel}({\mathbf{x}}_t)$，得到测量似然：
$$p(z_t^i|{\mathbf{x}}_t,c_t^i,\mathbf{m})\approx \mathcal{N}(z_t^i;h({\mathbf{x}}_i,c_t^i,\mathbf{m}),Q_t)\approx \mathcal{N}(z_t^i;h({\overline{\mathbf{\mu }}}_t,c_t^i,\mathbf{m})+{\mathbf{H}}_t({\mathbf{x}}_t-{\overline{\mathbf{\mu }}}_t),Q_t)\otimes \mathcal{N}(x_t;{\overline{\mathbf{\mu }}}_t,{\overline{\mathbf{\Sigma }}}_t)$$\qquad$\otimes$为卷积符号，$表示似然函数为测量噪声和状态不确定性的高斯函数卷积？？？？$。因此上式的高斯分布的均值为$h({\overline{\mathbf{\mu }}}_t,c_t^i,m)$，协方差为${\mathbf{H}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{H}}_t^T+Q_t$测量似然函数为：
$$p(z_t|c_{1:t},\mathbf{m},{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\sim \mathcal{N}(z_t^i;h({\overline{\mathbf{\mu }}}_i,c_t^i,\mathbf{m}),{\mathbf{H}}_t{\overline{\mathbf{\Sigma }}}_t{\mathbf{H}}_t^T+Q_t)$$\qquad第 21行更新测量似然。