1: Algorithm motion_model_odometry $(x_{t}, u_{t}, x_{t-1})$:

 2: $\delta_{rot1} = atan2(\overline y'- \overline y, \overline x’ - \overline x) - \overline \theta$

 3: $\delta_{trans} = (\overline x - \overline x')^2 + (\overline y - \overline y')^2$

 4: $\delta_{rot2} = \overline \theta' - \overline \theta- \delta_{rot1}$ //里程計讀數，運動控制資訊$u_{t}$

 5: $\delta_{rot1} = atan2(y' - y, x' - x) - \theta$

 6: $\delta_{trans} = (x - x')^2 + (y - y')^2$

 7: $\delta_{rot2} = \theta' - \theta - \delta_{rot1}$

 8: $p1 = prob(\delta_{rot1} - \widehat {\delta}_{rot1}, α1\widehat {\delta}_{rot1}^2 + α2\widehat {\delta}_{rot1}^2 )$

 9: $p2 = prob(\delta_{trans} - \widehat {\delta}_{trans}, α3\widehat {\delta}_{trans}^2 + α4\widehat {\delta}_{rot1}^2 + α4\widehat {\delta}_{rot2}^2 )$

10: $p3 = prob(\delta_{rot2} - \widehat {\delta}_{rot2}, α1\widehat {\delta}_{rot2}^2 + α2\widehat {\delta}_{trans}^2 )$

11: return $p1 · p2 · p3 $

 1:  Algorithm Sample_motion_model_odometry $(u_{t}, x_{t-1})$:

 2:  $\delta_{rot1} = atan2(\overline y'- \overline y, \overline x’ - \overline x) - \overline \theta$

 3:  $\delta_{trans} = (\overline x - \overline x')^2 + (\overline y - \overline y')^2$ 

 4:  $\delta_{rot2} = \overline \theta' - \overline \theta- \delta_{rot1}$ //里程計讀數，運動控制資訊$u_{t}$

 5:  $\widehat {\delta}_{rot1}  =\delta_{rot1} - sample(α1\widehat {\delta}_{rot1}^2 + α2\widehat {\delta}_{rot1}^2)$

 6:  $\widehat {\delta}_{trans}=\delta_{trans}- sample(α3\widehat {\delta}_{trans}^2 + α4\widehat {\delta}_{rot1}^2 + α4\widehat {\delta}_{rot2}^2)$

 7:  $\widehat {\delta}_{rot2}  =\delta_{rot2} - sample(α1\widehat {\delta}_{rot2}^2 + α2\widehat {\delta}_{trans}^2)$ //為運動控制的變化量增加誤差項

 8:  $x'= x+\widehat {\delta}_{trans} cos(\theta+\widehat {\delta}_{rot1})$

 9:  $y'= y+\widehat {\delta}_{trans} cos(\theta+\widehat {\delta}_{rot1})$

10: $\theta'= \theta+\widehat {\delta}_{rot1}+\widehat {\delta}_{rot2}$ //加在$t-1$時刻的位姿上，計算出$t$時刻的位姿

11: return $x_{t}=(x',y',\theta')^T$