概率机器人里程计运动模型.
为了保持状态空间比较小，通常把里程计数据认为是控制信号。在时间 $t$ , 机器人确切的位姿由随机变量 $x_t$ 建模。由于漂移和打滑，在机器人的内部里程计使用的坐标和物理世界坐标之间不存在固定的坐标变换。里程计模型使用相对运动信息 (relative motion information) , 该信息由机器人内部里程计测量。所以，运动信息 $u_t$ 由下式给定：
$$
u_t =
\left( \begin{array}{ccc}
\bar{x}_{t-1}\\
\bar{x}_t
\end{array} \right)
$$
上图给出了里程计运动模型的示例。该模型对机器人在二维平面内进行了如下的分解：初始旋转 $\delta_{rot1}$ 、平移 $\delta_{trans}$ 和第二次旋转 $\delta_{rot2}$ 。每对位置 $(\bar{s}, \bar{s}')$ 都具有唯一的参数向量 $(\delta_{rot1},\delta_{trans},\delta_{rot2})$ ，这些参数足以重现 $\bar{s}$ 和 $\bar{s}'$ 之间的相对运动。
闭式计算
下面根据上文说到的里程计运动模型，直接说明算法，后文再针对算法进行数学推导的说明：
$$
\begin{align}
\text{1:} \qquad \qquad & \text{Aligorithm motion_model_odometry}(x_t, u_t, x_{t-1}):\\
\text{2:} \qquad \qquad & \delta_{rot1} = atan2(\bar{y}' - \bar{y}, \bar{x}' - \bar{x}) - \bar{\theta}\\
\text{3:} \qquad \qquad & \delta_{trans} = \sqrt{(\bar{x} - \bar{x}')^2 + (\bar{y} - \bar{y}')^2}\\
\text{4:} \qquad \qquad & \delta_{rot2} = \bar{\theta}' - \bar{\theta} - \delta_{rot1}\\
\text{5:} \qquad \qquad & \hat{\delta}{rot1} = atan2(y' - y, x' - x) - \theta\\
\text{6:} \qquad \qquad & \hat{\delta} = \sqrt{(x - x')^2 + (y - y')^2}\\
\text{7:} \qquad \qquad & \hat{\delta}{rot2} = \theta' - \theta - \hat{\delta}\\
\text{8:} \qquad \qquad & p_1 = prob(\delta_{rot1} - \hat{\delta}{rot1}, \alpha_1 \hat{\delta}^2 + \alpha_2 \hat{\delta}{trans}^2)\\
\text{9:} \qquad \qquad & p_2 = prob(\delta - \hat{\delta}{trans}, \alpha_3 \hat{\delta}^2 + \alpha_4 \hat{\delta}{rot1}^2 + \alpha_4 \hat{\delta}^2)\\
\text{10:} \qquad \qquad & p_3 = prob(\delta_{rot2} - \hat{\delta}{rot2}, \alpha_1 \hat{\delta}^2 + \alpha_2 \hat{\delta}_{trans}^2)\\
\text{11:} \qquad \qquad & return \quad p_1 \cdot p_2 \cdot p_3
\end{align}
$$
采样算法
如果粒子滤波用于定位，也希望有一个 $p(x_t|u_t,x_{t-1})$ 的采样算法。
$$
\begin{align}
\text{1:} \qquad \qquad & \text{Aligorithm sample_motion_model_odometry}(u_t, x_{t-1}):\\
\text{2:} \qquad \qquad & \delta_{rot1} = atan2(\bar{y}' - \bar{y}, \bar{x}' - \bar{x}) - \bar{\theta}\\
\text{3:} \qquad \qquad & \delta_{trans} = \sqrt{(\bar{x} - \bar{x}')^2 + (\bar{y} - \bar{y}')^2}\\
\text{4:} \qquad \qquad & \delta_{rot2} = \bar{\theta}' - \bar{\theta} - \delta_{rot1}\\
\text{5:} \qquad \qquad & \hat{\delta}{rot1} = \delta - sample(\alpha_1 \delta_{rot1}^2 + \alpha_2 \delta_{trans}^2)\\
\text{6:} \qquad \qquad & \hat{\delta}{trans} = \delta - sample(\alpha_3 \delta_{trans}^2 + \alpha_4 \delta_{rot1}^2 + \alpha_4 \delta_{rot2}^2)\\
\text{7:} \qquad \qquad & \hat{\delta}{rot2} = \delta - sample(\alpha_1 \delta_{rot2}^2 + \alpha_2 \delta_{trans}^2)\\
\text{8:} \qquad \qquad & x' = x + \hat{\delta}{trans} \cos{(\theta + \hat{\delta})}\\
\text{9:} \qquad \qquad & y' = y + \hat{\delta}{trans} \sin{(\theta + \hat{\delta})}\\
\text{10:} \qquad \qquad & \theta' = \theta + \hat{\delta}{rot1} + \hat{\delta}\\
\text{11:} \qquad \qquad & return \quad x_t = (x', y', \theta')^T
\end{align}
$$
里程计运动模型的数学推导
此次为初次阅读，以后再来进行补充！
