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)