-
Notifications
You must be signed in to change notification settings - Fork 0
/
motion.py
39 lines (30 loc) · 1000 Bytes
/
motion.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
39
import numpy as np
class ConstantVelocityModel:
"""Constant velocity motion model."""
def __init__(self, q):
self.q = q
def getFQ(self, dT):
F = np.array([[1, 0, dT, 0],
[0, 1, 0, dT],
[0, 0, 1, 0],
[0, 0, 0, 1]])
Q = np.array([[dT ** 3 / 3, 0, dT ** 2 / 2, 0],
[0, dT ** 3 / 3, 0, dT ** 2 / 2],
[dT ** 2 / 2, 0, dT, 0],
[0, dT ** 2 / 2, 0, dT]]) * self.q
return F, Q
def step(self, xprev, Pprev, dT):
F, Q = self.getFQ(dT)
x = F * xprev
P = F * Pprev * F.T + Q
return x, P
def position_measurement(x):
"""Velocity measurement model."""
H = np.array([[1, 0, 0, 0],
[0, 1, 0, 0]])
return H * x, H
def velocity_measurement(x):
"""Velocity measurement model."""
H = np.array([[0, 0, 1, 0],
[0, 0, 0, 1]])
return H * x, H