```python
%matplotlib inline
from IPython.display import Markdown, display
import numpy as np
import matplotlib.dates as mdates
import matplotlib.pyplot as plt
import time
from scipy.stats import norm
from sympy import Symbol, symbols, Matrix, sin, cos, integrate, diff, Q, refine, simplify, factor, expand_trig, exp, latex, Integral
from sympy import init_printing
from sympy.utilities.codegen import codegen
init_printing(use_latex=True)
```

# Motion models
In tracking we can use different motion models. Here, we will derive a couple of those:

## CATR - constant acceleration and turn rate model
This model assumes the state $[x, y, \theta, v, \omega, a]^\top$, where $x$ and $y$ are coordinates of the object, $\theta$ is it CCW orientation, $v$ is its speed along the vector spawned by $\theta$, $\omega$ is the angular velocity (aka turn rate), and $a$ is the acceleration along the vector spawned by $\theta$. Furthermore, we assume that the object has constant acceleration $a$ and constant turn rate $\omega$. This is a useful model to model differential-drive systems.

### Derivation:


```python
speed, yaw, dt, x, y, acceleration, t, d_sin, d_cos = symbols('v \\theta \Delta{t} x y a t d_{sin} d_{cos}')
turn_rate_non_zero = Symbol('\hat{\omega}', nonzero=True, finite=True)
zero_turn_rate = Symbol('\omega_0', zero=True)
turn_rate = Symbol('\omega')

state_catr = Matrix([x, y, yaw, speed, turn_rate, acceleration])
print("CART state is:")
display(state_catr)

def get_next_state(turn_rate):
    # Specify the functions for yaw, speed, x, and y.
    yaw_func = yaw + turn_rate * t
    speed_func = speed + acceleration * t
    x_speed_func = speed_func * cos(yaw_func)
    y_speed_func = speed_func * sin(yaw_func)

    # Get next state by integrating the functions.
    next_speed = speed + integrate(acceleration, (t, 0, dt))
    next_yaw = yaw + integrate(turn_rate, (t, 0, dt))
    next_x = x + integrate(x_speed_func, (t, 0, dt))
    next_y = y + integrate(y_speed_func, (t, 0, dt))

    return Matrix([next_x, next_y, next_yaw, next_speed, turn_rate, acceleration])

# There is a difference in computation betwee the cases when the turn rate is allowed to be zero or not
print("Assuming a non-zero turn rate, the next state is:")
next_state = get_next_state(turn_rate_non_zero)
display(next_state)

substitutes = {x:42, y:23, yaw:0.5, speed:2, acceleration:2, dt:0.1, turn_rate_non_zero:2, zero_turn_rate:0}
display('Plugging in the numbers:', substitutes)
display(next_state.evalf(subs=substitutes))

state = Matrix([x,y,yaw,speed,turn_rate_non_zero,acceleration])
print("Jacobian of the next state with respect to the previous state:")
J = next_state.jacobian(state)
display(J)
display('Plugging in the numbers:', substitutes)
display(J.evalf(subs=substitutes))

print("Assuming a zero turn rate, state is:")
next_state = get_next_state(zero_turn_rate)
display(next_state)
display('Plugging in the numbers:', substitutes)
display(next_state.evalf(subs=substitutes))

state = Matrix([x,y,yaw,speed,zero_turn_rate,acceleration])
print("Jacobian of the next state with respect to the previous state with 0 turn rate:")
J = next_state.jacobian(state)
display(J)

display('Plugging in the numbers:', substitutes)
display(J.evalf(subs=substitutes))

```

## CVTR - constant velocity and turn rate model
This model assumes the state $[x, y, \theta, v, \omega]^\top$, where $x$ and $y$ are coordinates of the object, $\theta$ is it CCW orientation, $v$ is its speed along the vector spawned by $\theta$, $\omega$ is the angular velocity (aka turn rate). Furthermore, we assume that the object has constant speed $v$ and constant turn rate $\omega$. This is also a useful model to model differential-drive systems. It is a bit simpler than the CATR one.

### Derivation:


```python
speed, yaw, dt, x, y, t = symbols('v \\theta \Delta{t} x y t')
turn_rate_non_zero = Symbol('\omega', nonzero=True, finite=True)
zero_turn_rate = Symbol('\omega_0', zero=True)

state_cvtr = Matrix([x, y, yaw, speed, turn_rate_non_zero])
print("CVRT state is:")
display(state_cvtr)

def get_next_state(turn_rate):
    # Specify the functions for yaw, x, and y.
    yaw_func = yaw + turn_rate * t
    x_speed_func = speed * cos(yaw_func)
    y_speed_func = speed * sin(yaw_func)

    # Get next state by integrating the functions.
    next_yaw = yaw + integrate(turn_rate, (t, 0, dt))
    next_x = x + integrate(x_speed_func, (t, 0, dt))
    next_y = y + integrate(y_speed_func, (t, 0, dt))

    return Matrix([next_x, next_y, next_yaw, speed, turn_rate])

# There is a difference in computation betwee the cases when the turn rate is allowed to be zero or not
print("Assuming a non-zero turn rate, next state is:")
next_state = get_next_state(turn_rate_non_zero)
display(next_state)
substitutes = {x:42, y:23, yaw:0.5, speed:2, dt:0.1, turn_rate_non_zero:2, zero_turn_rate:0}
display(Markdown('Plugging in the numbers:'), substitutes)
display(next_state.evalf(subs=substitutes))

state = Matrix([x,y,yaw,speed,turn_rate_non_zero])
print("Jacobian of the next state with respect to the previous state:")
J = next_state.jacobian(state)
display(J)

display(Markdown('Plugging in the numbers:'), substitutes)
display(J.evalf(subs=substitutes))

print("Assuming a zero turn rate, next state is:")
next_state = get_next_state(zero_turn_rate)
display(next_state)
display(Markdown('Plugging in the numbers:'), substitutes)
display(next_state.evalf(subs=substitutes))

state = Matrix([x,y,yaw,speed,zero_turn_rate])
print("Jacobian of the next state with respect to the previous one with zero turn rate:")
J = next_state.jacobian(state)
display(J)

display(Markdown('Plugging in the numbers:'), substitutes)
display(J.evalf(subs=substitutes))
```

a9d1ecd12e2c9678ce358bcfe4e5c82c98c1ef0b
ipynb
Jupyter Notebook
src/common/motion_model/notebooks/motion_model.ipynb
ruvus/auto
25ae62d6e575cae40212356eed43ec3e76e9a13e
Apache-2.0
2021-05-28T06:14:21.000Z
2022-03-10T10:03:08.000Z
src/common/motion_model/notebooks/motion_model.ipynb
ruvus/auto
25ae62d6e575cae40212356eed43ec3e76e9a13e
Apache-2.0
2021-10-29T22:00:27.000Z
2022-03-29T20:56:34.000Z
src/common/motion_model/notebooks/motion_model.ipynb
ruvus/auto
25ae62d6e575cae40212356eed43ec3e76e9a13e
Apache-2.0
2021-05-29T14:59:17.000Z
2022-03-10T10:03:09.000Z
Qwen/Qwen-72B
1. YES
2. YES