-
Notifications
You must be signed in to change notification settings - Fork 1
/
motion_model.py
38 lines (31 loc) · 986 Bytes
/
motion_model.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
import numpy as np
from prob_utils import sample
from state import State
from data_utils import Control
# noise constants
SIGMA_V = 0.1
SIGMA_W = 2.0
def motion_model(state, control, dt):
"""Function that represents a motion model.
Input args:
:state: current state
:control: current control
:dt: time for which the control is applied (dt)
Output:
Predicted state at time t+dt
"""
# just for ease of understanding explicitly declare the variables
v = control[Control.V]
w = control[Control.W]
x0 = state[State.X]
y0 = state[State.Y]
theta0 = state[State.HEADING]
# add gaussian noise to the commands
v = v + sample(SIGMA_V)
w = w + sample(SIGMA_W)
# the actual motion model
x = x0 + v * np.cos(theta0) * dt
y = y0 + v * np.sin(theta0) * dt
# this can cause issues because theta is unbounded
theta = theta0 + w * dt
return np.array([x, y, theta])