-
Notifications
You must be signed in to change notification settings - Fork 3
/
ttc_depth_integrals_accel_bias.py
128 lines (98 loc) · 4.29 KB
/
ttc_depth_integrals_accel_bias.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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
###############################################################################
#
# File: ttc_depth_integrals_accel_bias.py
# Available under MIT license
#
# Estimate distance using the Tau constraint in the presence of
# a constant acceleration bias
#
# History:
# 09-26-22 - Levi Burner - Open source release
#
###############################################################################
import numpy as np
import numba as nb
import time
def cumulative_int(dt, x):
return dt * np.cumsum(x)
# Calculate Phi
def Phi_t_t_0(dt, dot_z_over_z):
int_dot_z_over_z = cumulative_int(dt, dot_z_over_z)
phi = np.exp(int_dot_z_over_z)
return phi
def accel_z_z_star_t0_no_inv(times, dot_z_over_z, accel_z):
# TODO all times are needed but they are assumed to be evenly spaced
#mid0 = time.time()
dt = times[1] - times[0]
int_accel = cumulative_int(dt, accel_z)
iint_accel = cumulative_int(dt, int_accel)
phi = Phi_t_t_0(dt, dot_z_over_z)
F_action = phi - (1 + ((times - times[0]) * dot_z_over_z[0]))
times_squared = np.square(times-times[0])
#total_time = (times[-1] - times[0])
#int_times_squared_squared = (1./5.)*(total_time * total_time * total_time * total_time * total_time)
a = dt * np.sum(np.square(F_action))
b = dt * np.sum(times_squared*F_action)
#c = (1./4.) * int_times_squared_squared
c = (1./4.) * dt * np.sum(np.square(times_squared))
d = -2.0 * dt * np.sum(iint_accel*F_action)
e = - dt * np.sum(iint_accel*times_squared)
P = np.array(((a, b/2),
(b/2, c)))
c = np.array((d, e))
return P, c
def accel_z_z_star_t0(times, dot_z_over_z, accel_z):
P, c = accel_z_z_star_t0_no_inv(times, dot_z_over_z, accel_z)
try:
z_star = np.linalg.solve(P, -0.5*c)
except np.linalg.LinAlgError:
z_star = (np.NAN, np.NAN)
return z_star
def accel_z_z_star_tf(times, dot_z_over_z, accel_z):
#start = time.time()
z_star = accel_z_z_star_t0(np.flip(times), np.flip(dot_z_over_z), np.flip(accel_z))
#end = time.time()
#print(end-start)
return z_star
def accel_x_z_star_t0_no_inv(times, dot_x_over_z, dot_z_over_z, accel_x):
# TODO all times are needed but they are assumed to be evenly spaced
#mid0 = time.time()
dt = times[1] - times[0]
int_accel = cumulative_int(dt, accel_x)
iint_accel = cumulative_int(dt, int_accel)
phi = Phi_t_t_0(dt, dot_z_over_z)
F_action = cumulative_int(dt, dot_x_over_z*phi) - ((times-times[0])*dot_x_over_z[0])
times_squared = np.square(times-times[0])
# TODO using the closed form makes the results worse...
#int_times_squared_squared = (1./5.)*((times[-1] - times[0])**5)
a = dt * np.sum(np.square(F_action))
b = dt * np.sum(times_squared*F_action)
#c = (1./4.) * int_times_squared_squared
c = (1./4.) * dt * np.sum(np.square(times_squared))
d = -2.0 * dt * np.sum(iint_accel*F_action)
e = - dt * np.sum(iint_accel*times_squared)
P = np.array(((a, b/2),
(b/2, c)))
c = np.array((d, e))
return P, c
def accel_x_z_star_t0(times, dot_x_over_z, dot_z_over_z, accel_x):
P, c = accel_x_z_star_t0_no_inv(times, dot_x_over_z, dot_z_over_z, accel_x)
try:
z_star = np.linalg.solve(P, -0.5*c)
except np.linalg.LinAlgError:
z_star = (np.NAN, np.NAN)
return z_star
def accel_x_z_star_tf(times, dot_x_over_z, dot_z_over_z, accel_x):
#start = time.time()
z_star = accel_x_z_star_t0(np.flip(times), np.flip(dot_x_over_z), np.flip(dot_z_over_z), np.flip(accel_x))
#end = time.time()
#print(end-start)
return z_star
# Setup numba
# No fast math because lots of the values are super tiny and might be sub-normal (not sure)
cumulative_int = nb.jit(nopython = True, cache = True, fastmath=False)(cumulative_int)
Phi_t_t_0 = nb.jit(nopython = True, cache = True, fastmath=False)(Phi_t_t_0)
accel_z_z_star_t0_no_inv = nb.jit(nopython = True, cache = True, fastmath=False)(accel_z_z_star_t0_no_inv)
# accel_z_z_star_tf = nb.jit(nopython = True, cache = True, fastmath=False)(accel_z_z_star_tf)
accel_x_z_star_t0_no_inv = nb.jit(nopython = True, cache = True, fastmath=False)(accel_x_z_star_t0_no_inv)
# accel_x_z_star_tf = nb.jit(nopython = True, cache = True, fastmath=False)(accel_x_z_star_tf)