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?


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.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.