Velocity Model Motion in Matlab (Probabilistic Robotics)

Question

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?

Answer 1

There are several traps you might have stepped into, but it is difficult to tell without more information. The first issues that came to my mind:

• The equations you wrote down are for sampling from the velocity motion model. But then you write about the Kalman Gain approaching singularity, which only makes sense of you apply a Gaussian filter (EKF or UKF). There is no sampling in EKF or UKF.

• 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.