Probabilistic Motion Model

A robot’s motion model can be written by a state equation:

$${\xi }_{k+1}=f({\xi }_k,u_k)$$

where:

• ${\xi }_k$ is called a state vector, like ${\xi }_k=[x_k,y_k,{\theta }_k{]}^T$
• $u_k$ is an input vector (command that the robot tries to follow), such as $u_k=[v_k,{\omega }_k{]}^T$

The input $u_k$ and state ${\xi }_k$ are all probabilistic in general because of sensor/actuator noise.

We usually assume that the state equation follows the Markov property: The value of the state at the current time step is completely determined by that at the immediate prior time step, with all past values irrelevant.

Example

Recall that for a 2-wheel differential drive has continuous-time motion as”

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{cc}\cos (\theta )& 0\\ \sin (\theta )& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \omega \end{array}\right]$$

Multiplying it out, we can see that we have

$$\dot{x}=v_k\cos ({\theta }_k),\dot{y}=v_k\sin ({\theta }_k),\dot{\theta }={\omega }_k$$

Converting to discrete-time motion:

$$\left[\begin{array}{c}{x}_{k+1}\\ y_{k+1}\\ {\theta }_{k+1}\end{array}\right]=\left[\begin{array}{c}{x}_k\\ y_k\\ {\theta }_k\end{array}\right]+\left[\begin{array}{c}{v}_k\Delta t\cos \left({\theta }_k+\frac{{\omega }_k\Delta t}{2}\right)\\ v_k\Delta t\sin \left({\theta }_k+\frac{{\omega }_k\Delta t}{2}\right)\\ {\omega }_k\Delta t\end{array}\right]+\mathcal{N}(0,{\sigma }^2)$$

or

$$\left[\begin{array}{c}{x}_{k+1}\\ y_{k+1}\\ {\theta }_{k+1}\end{array}\right]=\left[\begin{array}{c}{x}_k\\ y_k\\ {\theta }_k\end{array}\right]+\left[\begin{array}{cc}\cos \left({\theta }_k+\frac{{\omega }_k\Delta t}{2}\right)& 0\\ \sin \left({\theta }_k+\frac{{\omega }_k\Delta t}{2}\right)& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{v}_k\Delta t+{ϵ}_k\\ {\omega }_k\Delta t+{\nu }_k\end{array}\right]$$

where ${ϵ}_k,{\nu }_k$ are random variables for noise/uncertainties.

Note that our model obeys the Markov property: the state at time $k+1$ only depends on $k$.

Probabilistic View

How do we make the motion model properly probabilistic?

$${\xi }_k=f({\xi }_{k-1},u_{k-1})⟶{\xi }_k\sim p({\xi }_k\mid {\xi }_{k-1},u_{k-1})$$

The naive way is to add noise to the deterministic motion model as we saw above.

More rigorous probabilistic motion models:

• Velocity Motion Model: Uses noisy $v$ and $\omega$
• Odometry Motion Model: Uses noisy wheel encoders

These motion models can be employed in two ways:

• Computing the posterior $p({\xi }_k|{\xi }_{k-1},u_{k-1})$
• Sampling (without directly computing the posterior)