I'm trying to understand the derivation of the velocity based motion model to implement an EKF: enter image description here

But the problem is, that I cannot find any derivation/explanation. Mostly there is only a visualization of the odometry based model. What are some useful references?


1 Answer 1


I did basically this for my last job. I also am not going to be able to cite any sources here on this, but if you consider a state feedback controller you have a state vector $\vec{x}$, state matrix $A$, an input $u$ and an input vector $\vec{B}$.

You can extend your state vector to include a bias term. For a rotational system you would expect to have the rotational equivalent of

$$ F = ma \\ $$

which is

$$ \tau = J\alpha \\ $$

If you were modeling this system such that you have torque as an input and angular position $\theta$ as an output then you would probably do something like:

$$ \left[\begin{matrix} \dot{\theta} \\ \ddot{\theta}\end{matrix}\right] = \left[\begin{matrix} 0 & 1 \\ 0 & 0\end{matrix}\right]\left[\begin{matrix} \theta \\ \dot{\theta} \end{matrix}\right] + \left[\begin{matrix} 0 \\ \frac{1}{J}\end{matrix}\right]u $$

I'll use the symbol gamm $\gamma$ for bias torque because that's what you have in your graphic. If you assume a constant bias, then what you're saying is that you're assuming $\dot{\gamma} = 0$, and so you can extend your equation:

$$ \left[\begin{matrix} \dot{\theta} \\ \ddot{\theta} \\ \dot{\gamma}\end{matrix}\right] = \left[\begin{matrix} 0 & 1 & 0\\ 0 & 0 & \frac{1}{J} \\ 0 & 0 & 0\end{matrix}\right]\left[\begin{matrix} \theta \\ \dot{\theta} \\ \gamma \end{matrix}\right] + \left[\begin{matrix} 0 \\ \frac{1}{J} \\ 0\end{matrix}\right]u $$

This may look like it's a trivial addition, but now your bias term $\gamma$ affects your angular acceleration:

$$ \ddot{\theta} = \frac{1}{J}\left(u + \gamma\right) \\ $$

and a Kalman filter will actual estimate this bias term for you. I used this kind of extension for a very challenging problem and was able to successfully estimate wind forces. No anemometer, I could estimate just by how the motion of the system deviated from what I expected.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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