# Velocity based motion model (dead reckoning)

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

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?

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.