def step(self, action):
"""Propagate dynamics."""
pos = self.state[0:3]
vel = self.state[3:6]
quat = self.state[6:10]
ang_vel = self.state[10:13]
moments = action[0:3]
thrust = action[4]
goal_state = np.array([5, 5, 5, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0])
cost = np.transpose(self.state - goal_state) @ \
np.eye(self.state.shape[0]) @ (self.state - goal_state)
quat = Quaternion(quat)
rotation_matrix = quat.rotation_matrix
pos_dot = vel
vel_dot = (thrust / self.mass) * \
np.array([0, 0, 1]) @ rotation_matrix - \
np.array([0, 0, 1]) * self.gravity
quat_dot = quat.derivative(ang_vel)
ang_vel_dot = np.inverse(self.inertia) @ \
(moments - np.cross(ang_vel, self.inertia @ ang_vel))
pos = pos + self.dt * pos_dot
vel = vel + self.dt * vel_dot
quat = quat + self.dt * quat_dot
quat = quat.normalize
ang_vel = ang_vel + self.dt * ang_vel_dot
self.time += self.dt
self.state = np.hstack((pos, vel, quat, ang_vel))
return self.state, -cost, False, {'time': self.time}
def advance(self, external_force_world_frame, external_torque_world_frame,
dt):
'''
external_force is in world frame, relative to model origin
external_torque is in world frame, relative to model origin
'''
# Get external force & torque in model frame
world_to_model = self.rotation.inverse
external_force_model_frame = world_to_model.rotate(
external_force_world_frame)
external_torque_model_frame = world_to_model.rotate(
external_torque_world_frame)
# Get external force & torque in cog frame
external_force_cog_frame = external_force_model_frame
external_torque_cog_frame = external_torque_model_frame + np.cross(
self.cog, external_force_model_frame)
# Get acceleration in cog frame
acceleration_translational_cog_frame = 1 / self.mass * external_force_cog_frame
acceleration_rotational_cog_frame = np.inverse(
self.moment_of_inertia) * external_torque_cog_frame
# Get acceleration in world frame, relative to cog
acceleration_translational_world_frame = self.rotation.rotate(
acceleration_translational_cog_frame)
acceleration_rotational_world_frame = self.rotation.rotate(
acceleration_rotational_cog_frame) + np.cross(
self.rotational_velocity, self.velocity)
# Integrate acceleration to get velocity
velocity_translational_world_frame = self.velocity + acceleration_translational_world_frame * dt
velocity_rotational_world_frame = self.rotational_velocity + acceleration_rotational_world_frame * dt
def advance(self, external_force_world_frame, external_torque_world_frame, dt):
'''
external_force is in world frame, relative to model origin
external_torque is in world frame, relative to model origin
'''
# Get external force & torque in model frame
world_to_model = self.rotation.inverse
external_force_model_frame = world_to_model.rotate(external_force_world_frame)
external_torque_model_frame = world_to_model.rotate(external_torque_world_frame)
# Get external force & torque in cog frame
external_force_cog_frame = external_force_model_frame
external_torque_cog_frame = external_torque_model_frame + np.cross(self.cog, external_force_model_frame)
# Get acceleration in cog frame
acceleration_translational_cog_frame = 1/ self.mass * external_force_cog_frame
acceleration_rotational_cog_frame = np.inverse(self.moment_of_inertia) * external_torque_cog_frame
# Get acceleration in world frame, relative to cog
acceleration_translational_world_frame = self.rotation.rotate(acceleration_translational_cog_frame)
acceleration_rotational_world_frame = self.rotation.rotate(acceleration_rotational_cog_frame) + np.cross(self.rotational_velocity, self.velocity)
# Integrate acceleration to get velocity
velocity_translational_world_frame = self.velocity + acceleration_translational_world_frame * dt
velocity_rotational_world_frame = self.rotational_velocity + acceleration_rotational_world_frame * dt
# Integrate velocity to get position and rotation
position_cog = self.position + self.rotation.rotate(self.center_of_gravity) + velocity_translational_world_frame * dt
velocity_rotational_cog_frame = world_to_model.rotate(velocity_rotational_world_frame)
self.rotation = quaternion.Quaternion.sym_exp_map(self.rotation, velocity_rotational_cog_frame * dt) # questionable
self.position = position_cog - self.rotation.rotate(self.center_of_gravity)
def riccati(c_yy, x, y,t):
'''
Description:
Solves the Riccati equation, specifically AOA^T - O + Q = 0
Parameters:
c_yy: a whitened covariance matrix, used in calculation of Q
x,y: matrices that are used to calculate koopman estimators
t: timelaga
Returns:
The matrix, O, solved in the above equation.
'''
# c_xx, c_yy = whitening(x,y)
K_xx, K_xy = koopman_est(x, y ,t)
evals_xx,evect_xx = np.linalg.eigh(K_xx) #hermetian
# evals_xy,evect_xy = np.linalg.eigh(K_xy)
print ('\n', evals_xx, '\n \n', evect_xx)
v_x = evect_xx[:,list(evals_xx).index(1)]
try:
v_y = np.linalg.solve(K_xy, v_x) # I think this v_y is extraneous, there isn't v_y used anywhere
except:
raise Error('cannot find eigenvalue = 1')
chi_bar = np.mean(chi(x).T, axis =1)
gamma_bar = np.mean(gamma(y).T, axis =1)
A = K_xx - np.outer(v_x*chi_bar)
B = K_xy - np.outer(v_x*gamma_bar)
Q = B.dot(np.inverse(c_yy)).dot(B.T)
return solve_discrete_lyapunov(A, Q)
def get_S(X, Xtest):
cov = np.inverse(np.cov(X.transpose()))
xt_c_x = np.matmul(Xtest, np.matmul(cov, X.transpose()))
Xnorms = get_norms(X, cov)
Xtnorms = get_norms(Xtest, cov)
denominator = np.matmul(Xtnorms, Xnorms.transpose())
return np.divide(xt_c_x, denominator)
def advance(self,
dt,
external_force_world_frame,
external_torque_world_frame,
added_mass_matrix_6x6=np.zeros((6, 6))):
'''
external_force is in world frame, relative to model origin
external_torque is in world frame, relative to model origin
'''
# Get external force & torque in model frame
world_to_model = self.rotation.inverse
external_force_model_frame = world_to_model.rotate(
external_force_world_frame)
external_torque_model_frame = world_to_model.rotate(
external_torque_world_frame)
# Set up 6x6 inertia matrix in model frame
inertia_matrix_6x6 = np.zeros((6, 6))
inertia_matrix_6x6[0:3, 0:3] = np.identity(3) * self.mass
inertia_matrix_6x6[3:6, 3:6] = self.get_paralleled_moment_of_inertia(
-self.center_of_gravity, self.moment_of_inertia)
inertia_matrix_6x6[0:3, 3:6] = self.get_smtrx(
self.center_of_gravity) * self.mass
inertia_matrix_6x6[3:6, 0:3] = -inertia_matrix_6x6[0:3, 3:6]
acceleration_translational_and_rotational_model_frame = np.inverse(
inertia_matrix_6x6 + added_mass_matrix_6x6) * np.concatenate(
external_force_model_frame, external_torque_model_frame)
# # Get external force & torque in cog frame
# external_force_cog_frame = external_force_model_frame
# external_torque_cog_frame = external_torque_model_frame + np.cross(self.cog, external_force_model_frame)
# # Get acceleration in cog frame
# acceleration_translational_cog_frame = 1/ self.mass * external_force_cog_frame
# acceleration_rotational_cog_frame = np.inverse(self.moment_of_inertia) * external_torque_cog_frame
# # Get acceleration in world frame, relative to cog
# acceleration_translational_world_frame = self.rotation.rotate(acceleration_translational_cog_frame)
# acceleration_rotational_world_frame = self.rotation.rotate(acceleration_rotational_cog_frame) + np.cross(self.rotational_velocity, self.velocity)
# Get acceleration in world frame, relative to model origin
acceleration_translational_world_frame = self.rotation.rotate(
acceleration_translational_and_rotational_model_frame[0:3])
acceleration_rotational_world_frame = self.rotation.rotate(
acceleration_translational_and_rotational_model_frame[3:6]
) + np.cross(self.rotational_velocity, self.velocity)
# Integrate acceleration to get velocity in world frame, relative to model origin
velocity_translational_world_frame = self.velocity + acceleration_translational_world_frame * dt
velocity_rotational_world_frame = self.rotational_velocity + acceleration_rotational_world_frame * dt
# Integrate velocity to get position and rotation
self.position = self.position + +velocity_translational_world_frame * dt
velocity_rotational_model_frame = world_to_model.rotate(
velocity_rotational_world_frame)
self.rotation = quaternion.Quaternion.sym_exp_map(
self.rotation,
velocity_rotational_model_frame * dt) # questionable
def kl_approx(mu1, sig1, mu2, sig2, size = 1): # compute KL divergence between 2 Gaussians if size == 1: KL = np.log(sig2) - np.log(sig1) - size + sig1 / sig2 + (mu1 - mu2) ** 2 / sig2 else: is2 = np.inverse(sig2) KL = np.log(np.linalg.det(sig2)) - np.log(np.linalg.det(sig1)) - size + np.trace(np.dot(is2, sig1)) KL += np.dot(np.dot((mu1 - mu2), is2), (mu1 - mu2)) return KL / 2.0
def cramer_eq(self):
"""
Another optimization function that uses cramer rule.
"""
x_trans = np.transpose(
self.x
) #solving takes a lot of time as size of input matrix increase
deno = np.inverse(np.matmul(x_trans, self.x))
numer = np.matmul(x_trans, self.y)
self.t = np.matmul(deno, numer)
return self.t
def advance(self, external_force, external_torque, dt):
'''
external_force is in world frame, relative to model origin
external_torque is in world frame, relative to model origin
'''
# Get external force & torque in model frame
world_to_model = self.rotation.inverse
external_force_model_frame = world_to_model.rotate(external_force)
external_torque_model_frame = world_to_model.rotate(external_torque)
# Get external force & torque in cog frame
external_force_cog_frame = external_force_model_frame
external_torque_cog_frame = external_torque_model_frame + np.cross(
self.cog, external_force_model_frame)
# Get acceleration in cog frame
acceleration_translational_cog_frame = 1 / self.mass * external_force_cog_frame
acceleration_rotational_cog_frame = np.inverse(
self.moment_of_inertia) * external_torque_cog_frame
def GNSS_LS_position_velocity(GNSS_measurements,no_GNSS_meas,predicted_r_ea_e,predicted_v_ea_e):
"""
Purpose
----------
Calculates position, velocity, clock offset, and clock drift using
unweighted iterated least squares. Separate calculations are implemented
for position and clock offset and for velocity and clock drift
Parameters
----------
GNSS_measurements: GNSS measurement data:
Column 1 Pseudo-range measurements (m)
Column 2 Pseudo-range rate measurements (m/s)
Columns 3-5 Satellite ECEF position (m)
Columns 6-8 Satellite ECEF velocity (m/s)
no_GNSS_meas: Number of satellites for which measurements are supplied
predicted_r_ea_e: prior predicted ECEF user position (m)
predicted_v_ea_e: prior predicted ECEF user velocity (m/s)
Outputs
----------
est_r_ea_e: estimated ECEF user position (m)
est_v_ea_e: estimated ECEF user velocity (m/s)
est_clock: estimated receiver clock offset (m) and drift (m/s)
"""
x_pred = np.matrix(np.zeros((4,1))) # Initialize x_pred matrix
x_pred[0:3] = predicted_r_ea_e
x_est = np.matrix(np.zeros((4,1))) # Initialize x_est matrix
test_convergence = 1
est_clock = np.matrix(np.zeros((2,1))) # Initialize est_clock matrix
while test_convergence > 0.0001:
pred_meas = np.matrix(np.zeros((no_GNSS_meas,1))) # Initialize pred_meas matrix for use in for loop
H_matrix = np.matrix(np.zeros((no_GNSS_meas,4))) # Initialize H_matrix for use in for loop
delta_r = np.matrix(np.zeros((3,1))) # Initialize delta_r for use in for loop
for k in range(1,no_GNSS_meas+1):
# predict approx. range
delta_r = np.subtract(np.transpose(GNSS_measurements[k-1,2:5]),x_pred)
approx_range = math.sqrt(delta_r.T * delta_r)
# Calculate frame rotation during signal transit time
C_e_I = np.matrix('1,0,0;0,1,0;0,0,1') # Initialize matrix and change values in following lines
C_e_I[0,1] = omega_ie * approx_range / c
C_e_I[1,0] = -omega_ie * approx_range / c
# Predict pseudo-range
delta_r = np.subtract(np.cross(C_e_I, np.transpose(GNSS_measurements[k-1,2:5])),x_pred[0:3])
range = math.sqrt(np.dot(delta_r.T, delta_r))
pred_meas[k-1] = range + x_pred[3]
# Predict line of sight and deploy in measurement matrix
H_matrix[k-1,0:3] = - np.transpose(delta_r) / range
H_matrix[k-1,3] = 1
# Unweighted least-squares solution
x_est = x_pred + np.cross(np.cross(np.inverse(np.cross(H_matrix[0:no_GNSS_meas,:].T, \
H_matrix[0:no_GNSS_meas,:])),H_matrix[0:no_GNSS_meas,:].T), np.subtract(\
GNSS_measurements[0:no_GNSS_meas,0], pred_meas[0:no_GNSS_meas]))
# Test convergence
test_convergence = math.sqrt(np.dot(np.subtract(x_est, x_pred).T, np.subtract(x_est, x_pred)))
# Set predictions to estimate
x_pred = x_est
# Set outputs to estimates
est_r_ea_e = np.matrix(np.zeros((4,1))) # Initialize est_r_ea_a matrix to output
est_r_ea_e[0:3] = x_est[0,3]
est_r_ea_e[3] = x_est[3]
est_clock[0] = x_est[3]
# VELOCITY AND CLOCK DRIFT
omega_ie_skewed = Skew_symmetric([0,0,omega_ie]) #REMEMBER TO IMPLEMENT Skew_symmetric from Skew_symmetric.m
# Setup predicted state
x_pred[0,3] = predicted_v_ea_e
x_pred[3] = 0
test_convergence = 1
while test_convergence > 0.0001:
pred_meas = np.matrix(np.zeros((no_GNSS_meas,1))) # Initialize pred_meas matrix for use in for loop
u_as_e = np.matrix(np.zeros((3,1))) # Initialize u_as_e matrix for use in for loop
delta_r = np.matrix(np.zeros((3,1))) # Initialize delta_r matrix for use in for loop
H_matrix = np.matrix(np.zeros((no_GNSS_meas,4))) # Initialize H_matrix for use in for loop
for k in range(1,no_GNSS_meas+1):
# predict approx. range
delta_r = np.subtract(np.transpose(GNSS_measurements[k-1,2:5]),est_r_ea_e)
approx_range = math.sqrt(np.dot(delta_r.T, delta_r))
# Calculate frame rotation during signal transit time
C_e_I = np.matrix('1,0,0;0,1,0;0,0,1') # Initialize matrix and change values in following lines
C_e_I[0,1] = omega_ie * approx_range / c
C_e_I[1,0] = -omega_ie * approx_range / c
# Calculate range
delta_r = np.subtract(np.cross(C_e_I, np.transpose(GNSS_measurements[k-1,2:5])),est_r_ea_e)
range = math.sqrt(np.dot(delta_r.T, delta_r))
# Calculate line of sight
u_as_e = delta_r / range
# Predict pseudo-range
range_rate = u_as_e.T * (C_e_I * (GNSS_measurements[k-1,5:8].T + \
omega_ie_skewed * GNSS_measurements[k-1,2:5].T) - \
(x_pred[0,3] + omega_ie_skewed * est_r_ea_e))
pred_meas[k-1] = range_rate + x_pred[3]
# Predict line of sight and deploy in measurement matrix
H_matrix[k-1, 0:3] = - u_as_e.T
H_matrix[k-1, 3] = 1
# Unweighted least-squares solution
x_est = x_pred + np.inverse(H_matrix[0:no_GNSS_meas,:].T * H_matrix) * \
H_matrix[0:no_GNSS_meas,:].T * (GNSS_measurements[0:no_GNSS_meas,1] \
- pred_meas[0:no_GNSS_meas])
# Test convergence
test_convergence = math.sqrt((x_est - x_pred).T * (x_est - x.pred))
# Set predictions to estimates for next iteration
x_pred = x_est
# Set outputs to estimates
est_v_ea_e = np.matrix(np.zeros((4,1)))
est_v_ea_e[0:3] = x_est[0,3]
est_clock[1] = x_est[3]
# RETURN THE OUTPUTS
return (est_r_ea_e, est_v_ea_e, est_clock)
def f(x, mu, Sigma, sigma_g):
g_in = np.matmul((x - mu).T, np.matmul(np.inverse(Sigma), (x - mu)))
return (g(g_in, sigma_g))
# In[99]: h = np.random.randint(10, size=(3, 3)) # In[100]: print(h) # In[101]: np.matinverse(h) # In[102]: np.inverse(h) # In[103]: np.inv(a) # In[104]: inv(a) # In[107]: from numpy.linalg import inv # In[110]:
def matrixInv(A):
AInv = np.inverse(A)
return AInv
def inv(self) -> None:
return LieGroupElement(np.inverse(self.representation))
def header_superpose_trk(target_path, origin_path, outpath=None):
if not isinstance(origin_path, str):
origin_trk = origin_path
else:
origin_trk = load_trk(origin_path, 'same')
target_data, target_affine, vox_size, target_header, target_ref_info = extract_nii_info(
target_path)
if outpath is None:
if isinstance(origin_path, str):
warnings.warn("Will copy over old trkfile, if this what you want?")
permission = input("enter yes or y if you are ok with this")
if permission.lower() == "yes" or permission.lower() == "y":
outpath = origin_trk
else:
raise Exception("Will not copy over old trk file")
else:
raise Exception("Need to specify a output path of some kind")
trk_header = origin_trk.space_attributes
trk_affine = origin_trk._affine
trkstreamlines = origin_trk.streamlines
if np.any(trk_header[1][0:3] != np.shape(target_data)[0:3]):
raise TypeError(
'Size of the originating matrix are difference, recalculation not implemented'
)
if np.any(trk_affine != target_affine):
test = 3
if test == 1:
trk_header = list(trk_header)
trk_header[0] = target_affine
trk_header = tuple(trk_header)
myheader = create_tractogram_header(outpath, *trk_header)
trk_sl = lambda: (s for s in trkstreamlines)
save_trk_heavy_duty(outpath,
streamlines=trk_sl,
affine=target_affine,
header=myheader)
elif test == 2:
transform_matrix = (
np.inverse(np.transpose(trk_affine) * trk_affine) *
np.transpose(trk_affine)) * target_affine
from dipy.tracking.streamline import transform_streamlines
myheader = create_tractogram_header(outpath, *trk_header)
new_streamlines = transform_streamlines(trkstreamlines,
transform_matrix)
trk_sl = lambda: (s for s in new_streamlines)
save_trk_heavy_duty(outpath,
streamlines=trkstreamlines,
affine=trk_affine,
header=myheader)
elif test == 3:
myheader = create_tractogram_header(outpath, *target_ref_info)
trk_sl = lambda: (s for s in trkstreamlines)
save_trk_heavy_duty(outpath,
streamlines=trk_sl,
affine=target_affine,
header=myheader)
else:
print("No need to change affine, bring to new path")
if isinstance(origin_path, str):
copyfile(origin_path, outpath)
else:
myheader = create_tractogram_header(outpath, *trk_header)
save_trk_heavy_duty(outpath,
streamlines=trkstreamlines,
affine=target_affine,
header=myheader)
