forked from agnivsen/Py-M-SLAM
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathFeatureVector.py
66 lines (46 loc) · 2.7 KB
/
FeatureVector.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
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
# -*- coding: utf-8 -*-
#!/usr/bin/env python
import numpy as np
import math
import Param as param
import Transformations as trnsfrm
def getInitialStateVector():
state = np.array([ [param.INIT_X], #0
[param.INIT_Y], #1
[param.INIT_Z], #2
[param.QUAT_INIT_REAL], #3
[param.QUAT_INIT_I], #4
[param.QUAT_INIT_J], #5
[param.QUAT_INIT_K], #6
[param.INIT_V_X], #7
[param.INIT_V_Y], #8
[param.INIT_V_Z], #9
[param.INIT_OMEGA_X], #10
[param.INIT_OMEGA_Y], #11
[param.INIT_OMEGA_Z] ]) #12
return state
def getTransitionQuat(stateVec,del_t):
disp_norm = math.sqrt((stateVec[0]*del_t)**2 + (stateVec[1]*del_t)**2 + (stateVec[2]*del_t)**2)
angularAcc = np.array([[(stateVec[0]*del_t)/disp_norm],[(stateVec[1]*del_t)/disp_norm],[(stateVec[2]*del_t)/disp_norm]])
angularAcc_norm = math.sqrt((angularAcc[0]**2) + (angularAcc[1]**2) + (angularAcc[2]**2))
transition_quat = np.array([[math.cos(disp_norm/2)], #resulting in division by zero, when no angular velocity applied
[(angularAcc[0]/angularAcc_norm)*math.sin(disp_norm/2)],
[(angularAcc[1]/angularAcc_norm)*math.sin(disp_norm/2)],
[(angularAcc[2]/angularAcc_norm)*math.sin(disp_norm/2)]])
return transition_quat
def stateVectorUpdate(stateVec,del_t):
transition_quat = getTransitionQuat(stateVec[10:13],del_t)
result_quat = trnsfrm.quaternion_multiply(stateVec[3:7],transition_quat)
return np.array([ [stateVec[0] + (stateVec[7]*del_t)], #0
[stateVec[1] + (stateVec[8]*del_t)], #1
[stateVec[2] + (stateVec[9]*del_t)], #2
[result_quat[0]], #3
[result_quat[1]], #4
[result_quat[2]], #5
[result_quat[3]], #6
[stateVec[7]], #7
[stateVec[8]], #8
[stateVec[9]], #9
[stateVec[10]], #10
[stateVec[11]], #11
[stateVec[12]] ]) #12