# Velocity Model Motion in Matlab (Probabilistic Robotics)

I want to implement the velocity motion model in Matlab. According to Probabilistic Robotics page 124, the model is as following

\begin{align*} \hat{v} &= v + sample(\alpha_{1} v^{2} + \alpha_{2} w^{2}) \\ \hat{w} &= w + sample(\alpha_{3} v^{2} + \alpha_{4} w^{2}) \\ \hat{\gamma} &= sample(\alpha_{5} v^{2} + \alpha_{6} w^{2}) \\ x' &= x - \frac{\hat{v}}{\hat{w}} sin \theta + \frac{\hat{v}}{\hat{w}} sin(\theta + \hat{w} \Delta{t}) \\ y' &= y + \frac{\hat{v}}{\hat{w}} cos \theta - \frac{\hat{v}}{\hat{w}} cos(\theta + \hat{w} \Delta{t}) \\ \theta' &= \theta + \hat{w} \Delta{t} + \hat{\gamma} \Delta{t} \end{align*}

where $sample(b^{2}) \Leftrightarrow \mathcal{N}(0, b^{2})$. With this kind of variance $\alpha_{1} v^{2} + \alpha_{2} w^{2}$, the Kalman Gain is approaching singularity. Why?

• The model above is not defined for $\omega = 0$. You need to handle this special case by computing the limit for $\omega \to 0$. Hint: L'Hôpital's rule
• The model assumes perfect accuracy (no noise) if $\omega = v = 0$. This is a rather strong assumption and may or may not lead to problems.