def ekf_measurement_step(self, measurements_from_robot, particle): Qt = self.Qt position = particle[0] mean_beliefs = particle[1] covar_beliefs = particle[2] initialized = particle[3] w=1/1000 for measurement_set in measurements_from_robot: index = measurement_set[0] measurement = measurement_set[1] x = position[0] y = position[1] theta = position[2] if not initialized[index]: r1 = measurement[0] phi1 = measurement[1] mu_x1 = r1*cos(phi1 + theta) + x mu_y1 = r1*sin(phi1 + theta) + y mean_beliefs[index][0] = mu_x1 mean_beliefs[index][1] = mu_y1 initialized[index] = True delta_x1 = mu_x1 - x delta_y1 = mu_y1 - y q1 = (delta_x1)**2 + (delta_y1)**2 H1 = np.zeros((2,2)) H1[0,0] = (mu_x1-x)/sqrt(q1) H1[0,1] = (mu_y1-y)/sqrt(q1) H1[1,0] = -(mu_y1-y)/q1 H1[1,1] = (mu_x1-x)/q1 covar_beliefs[index] = inv(H1) @ Qt @ inv(H1.T) w = 1/1000 continue # Range and bearing from mean belief mean_belief = mean_beliefs[index] f_x = mean_belief[0] f_y = mean_belief[1] delta_x = f_x - x delta_y = f_y - y q = (delta_x)**2 + (delta_y)**2 zti = np.array([ [np.sqrt(q)], [wrap(np.arctan2((delta_y), (delta_x)) - theta)]]).reshape((2,1)) Ht = np.array([ [(f_x - x)/np.sqrt(q), (f_y - y)/np.sqrt(q)], [-(f_y - y)/q, (f_x - x)/q]]).reshape((2,2)) residual = wrap((measurement - zti), index=1) covariance_belief = covar_beliefs[index] Q = Ht @ covariance_belief @ Ht.T + Qt Kt = covariance_belief @ Ht.T @ np.linalg.inv(Q) mean_beliefs[index] = (mean_belief[:,np.newaxis] + Kt @ residual).reshape(2,) covar_beliefs[index] = (np.eye(len(Kt)) - Kt @ Ht) @ covariance_belief w = np.linalg.det(2*pi*Q)**(-1/2) * exp(-1/2*residual.T @ inv(Q) @ residual) # Update position return [position, mean_beliefs, covar_beliefs, initialized], w
def measurement_step(self, true_state): Qt = self.Qt for index, feature in enumerate(self.all_features): f_x = feature[0] f_y = feature[1] mean_x = self.mean_belief[0] mean_y = self.mean_belief[1] mean_theta = self.mean_belief[2] angle_to_check = wrap( np.arctan2((f_y - true_state[1]), (f_x - true_state[0])) - wrap(true_state[2])) if abs(angle_to_check) > self.vision_angle: continue measurement = simulate_measurement(true_state, f_x, f_y) if not self.initialized[index]: r = measurement[0] phi = measurement[1] self.mean_belief[2 * index + 3] = r * cos(phi + mean_theta) + mean_x self.mean_belief[2 * index + 4] = r * sin(phi + mean_theta) + mean_y self.initialized[index] = True # Range and bearing from mean belief delta_x = self.mean_belief[2 * index + 3] - mean_x delta_y = self.mean_belief[2 * index + 4] - mean_y delta = np.vstack((delta_x, delta_y)) q = (delta_x)**2 + (delta_y)**2 zti = np.array([[np.sqrt(q)], [np.arctan2((delta_y), (delta_x)) - mean_theta]]).reshape( (2, 1)) left = np.append(np.eye(3), np.zeros((2, 3)), axis=0) middle1 = np.zeros((5, 2 * (index + 1) - 2)) middle2 = np.append(np.zeros((3, 2)), np.eye(2), axis=0) right = np.zeros((5, 2 * self.n_features - 2 * (index + 1))) Fxj = np.concatenate((left, middle1, middle2, right), axis=1) sqrtq = np.sqrt(q) Ht = 1 / q * np.array([[ -sqrtq * delta_x, -sqrtq * delta_y, np.array([0]), sqrtq * delta_x, sqrtq * delta_y ], [delta_y, -delta_x, -q, -delta_y, delta_x]]).reshape( (2, 5)) @ Fxj covariance_belief = self.covariance_belief mean_belief = self.mean_belief St = Ht @ covariance_belief @ Ht.T + Qt Kt = covariance_belief @ Ht.T @ np.linalg.inv(St) self.mean_belief = mean_belief + Kt @ wrap( (measurement - zti), index=1) self.covariance_belief = (np.eye(len(Kt)) - Kt @ Ht) @ covariance_belief self.kt = Kt
def update(self, cmd, state): # lateral autopilot chi_c = wrap(cmd.course_command, state.chi) phi_c = cmd.phi_feedforward + self.course_from_roll.update( chi_c, state.chi) phi_c = self.saturate(phi_c, -np.radians(30), np.radians(30)) delta_a = self.roll_from_aileron.update(phi_c, state.phi, state.p) delta_r = self.yaw_damper.update(state.r) # longitudinal autopilot # saturate the altitude command alt_band = AP.altitude_zone altitude_c = self.saturate(cmd.altitude_command, state.altitude - alt_band, state.altitude + alt_band) theta_c = self.altitude_from_pitch.update(altitude_c, state.altitude) delta_e = self.pitch_from_elevator.update(theta_c, state.theta, state.q) delta_t = self.airspeed_from_throttle.update( cmd.airspeed_command - self.Va_trim, state.Va - self.Va_trim) delta_t = self.saturate(delta_t + u_trim.item(3), 0., 1.) # construct output and commanded states delta = MsgDelta(elevator=delta_e, aileron=delta_a, rudder=delta_r, throttle=delta_t) self.commanded_state.altitude = cmd.altitude_command self.commanded_state.Va = cmd.airspeed_command self.commanded_state.phi = phi_c self.commanded_state.theta = theta_c self.commanded_state.chi = cmd.course_command return delta, self.commanded_state
def measurement_update(self, state, measurement): # always update based on wind triangle pseudu measurement h_pseudo = self.h_pseudo(self.xhat, state) C = jacobian(self.h_pseudo, self.xhat, state) y = np.array([0, 0]).T # for i in range(0, 2): # Ci = S_inv = np.linalg.inv(self.R_psudo + C @ self.P @ C.T) L = self.P @ C.T @ S_inv I_LC = np.eye(7) - L @ C self.P = I_LC @ self.P @ I_LC.T + L @ self.R_psudo @ L.T self.xhat = self.xhat + L @ (y - h_pseudo) # only update GPS when one of the signals changes if (measurement.gps_n != self.gps_n_old) \ or (measurement.gps_e != self.gps_e_old) \ or (measurement.gps_Vg != self.gps_Vg_old) \ or (measurement.gps_course != self.gps_course_old): h_gps = self.h_gps(self.xhat, state) C = jacobian(self.h_gps, self.xhat, state) y = np.array([measurement.gps_n, measurement.gps_e, measurement.gps_Vg, measurement.gps_course]).T # for i in range(0, 4): # Ci = S_inv = np.linalg.inv(self.R_gps + C @ self.P @ C.T) L = self.P @ C.T @ S_inv I_LC = np.eye(7) - L @ C self.P = I_LC @ self.P @ I_LC.T + L @ self.R_gps @ L.T y[3] = wrap(y[3], h_gps[3]) self.xhat = self.xhat + L @ (y - h_gps) # update stored GPS signals self.gps_n_old = measurement.gps_n self.gps_e_old = measurement.gps_e self.gps_Vg_old = measurement.gps_Vg self.gps_course_old = measurement.gps_course
def update(self, cmd, state): # lateral autopilot chi_c = wrap(cmd.course_command, state.chi) #course command phi_c = self.course_from_roll.update(chi_c, state.chi) #roll command delta_a = self.roll_from_aileron.update(phi_c, state.phi, state.p) #aileron command delta_r = self.yaw_damper.update(state.r) #rudder command # longitudinal autopilot # saturate the altitude command h_c = self.saturate(cmd.altitude_command, AP.altitude_zone[0], AP.altitude_zone[1]) theta_c = self.altitude_from_pitch.update(h_c, state.h) delta_e = self.pitch_from_elevator.update(theta_c, state.theta, state.q) Va_command = cmd.airspeed_command delta_t = self.saturate( self.airspeed_from_throttle.update(Va_command, state.Va), 0, 1) # construct output and commanded states delta = np.array([[delta_e], [delta_a], [delta_r], [delta_t]]) self.commanded_state.h = cmd.altitude_command self.commanded_state.Va = cmd.airspeed_command self.commanded_state.phi = phi_c self.commanded_state.theta = theta_c self.commanded_state.chi = cmd.course_command return delta, self.commanded_state
def update(self, cmd, state): # Wrap chi to be within +- pi of the state chi_c = wrap(cmd.course_command, state.chi) # lateral autopilot phi_c_unsaturated = self.course_from_roll.update(chi_c, state.chi) phi_c_limit = np.pi / 4 phi_c = self.saturate(phi_c_unsaturated, -phi_c_limit, phi_c_limit) delta_a = self.roll_from_aileron.update(phi_c, state.phi, state.p) delta_r = self.yaw_damper.update(state.r) # longitudinal autopilot h_c = self.saturate(cmd.altitude_command, state.h - AP.altitude_zone, state.h + AP.altitude_zone) theta_c = self.altitude_from_pitch.update(h_c, state.h) delta_e = self.pitch_from_elevator.update(theta_c, state.theta, state.q) delta_t_unsat = self.airspeed_from_throttle.update( cmd.airspeed_command, state.Va) delta_t = self.saturate(delta_t_unsat, 0, 1.0) # construct output and commanded states delta = np.array([[delta_a], [delta_e], [delta_r], [delta_t]]) self.commanded_state.h = cmd.altitude_command self.commanded_state.Va = cmd.airspeed_command self.commanded_state.phi = phi_c self.commanded_state.theta = theta_c self.commanded_state.chi = cmd.course_command return delta, self.commanded_state
def update(self, cmd, state): # lateral autopilot chi_c = wrap(cmd.course_command, state.chi) phi_c = self.saturate( cmd.phi_feedforward + self.course_from_roll.update(chi_c, state.chi), -np.radians(30), np.radians(30)) delta_a = self.roll_from_aileron.update(phi_c, state.phi, state.p) delta_r = self.yaw_damper.update(state.r) # longitudinal autopilot # saturate the altitude command # h_c = self.saturate(cmd.altitude_command,state.h - AP.altitude_zone,state h_c = self.saturate(cmd.altitude_command, state.h - AP.altitude_zone, state.h + AP.altitude_zone) theta_c = self.altitude_from_pitch.update(h_c, state.h) delta_e = self.pitch_from_elevator.update(theta_c, state.theta, state.q) delta_t = self.airspeed_from_throttle.update(cmd.airspeed_command, state.Va) delta_t = self.saturate(delta_t, 0.0, 1.0) # construct output and commanded states delta = np.array([[delta_e], [delta_a], [delta_r], [delta_t]]) self.commanded_state.h = cmd.altitude_command self.commanded_state.Va = cmd.airspeed_command self.commanded_state.phi = phi_c self.commanded_state.theta = theta_c self.commanded_state.chi = cmd.course_command return delta, self.commanded_state
def measurement_update(self, state, measurement): # always update based on wind triangle pseudo measurement h = self.h_pseudo(self.xhat, state) C = jacobian(self.h_pseudo, self.xhat, state) y = np.array([[0, 0]]).T # for i in range(0, 2): S_inv = np.linalg.inv(self.R_pseudo + C @ self.P @ C.T) L = self.P @ C.T @ S_inv self.P = (np.eye(7) - L @ C) @ self.P @ (np.eye(7) - L @ C).T + L @ self.R_pseudo @ L.T self.xhat = self.xhat + L @ (y-h) # only update GPS when one of the signals changes if (measurement.gps_n != self.gps_n_old) \ or (measurement.gps_e != self.gps_e_old) \ or (measurement.gps_Vg != self.gps_Vg_old) \ or (measurement.gps_course != self.gps_course_old): h = self.h_gps(self.xhat, state) C = jacobian(self.h_gps, self.xhat, state) y = np.array([[measurement.gps_n, measurement.gps_e, measurement.gps_Vg, measurement.gps_course]]).T y[3, 0] = wrap(y[3, 0], h[3, 0]) # for i in range(0, 4): S_inv = np.linalg.inv(self.R + C @ self.P @ C.T) L = self.P @ C.T @ S_inv tmp = np.eye(7) - L @ C self.P = tmp @ self.P @ tmp.T + L @ self.R @ L.T self.xhat = self.xhat + L @ (y-h) # update stored GPS signals self.gps_n_old = measurement.gps_n self.gps_e_old = measurement.gps_e self.gps_Vg_old = measurement.gps_Vg self.gps_course_old = measurement.gps_course
def update(self, cmd, state, at_rest): # state regime: # 1 = at rest # 2 = take off (full throttle; regulate pitch to a fixed theta_c # 3 = climb zone (full throttle; regulate airspeed by commanding pitch attitude) # 4 = altitude hold zone (regulate altitude by commanding pitch attitude; regulate airspeed by commanding throttle) # 5 = descend zone (zero throttle; regulate airspeed by commanding pitch attitude) # TODO implement state machine h_c = cmd.altitude_command if self.state_regime == 1: # at rest theta_c = 0. if not at_rest: self.state_regime = 2 elif self.state_regime == 2: # going down the runway theta_c = 0. if state.h >= 1: self.state_regime = 3 elif self.state_regime == 3: # pitch up and take off theta_c = np.radians(10) if state.h > AP.h_takeoff: self.state_regime = 4 elif self.state_regime == 4: # climb zone up to commanded altitude and steady level flight and descent theta_c = self.altitude_from_pitch.update(h_c, state.h) if state.h <= 1: self.state_regime = 5 elif self.state_regime == 5: # flare theta_c = np.radians(5) if state.Va <= 15: self.state_regime = 6 elif self.state_regime == 6: # landed theta_c = self.altitude_from_pitch.update(h_c, state.h) # lateral autopilot chi_c = wrap(cmd.course_command, state.chi) phi_c = self.course_from_roll.update(chi_c, state.chi) delta_a = self.roll_from_aileron.update(phi_c, state.phi, state.p) delta_r = self.yaw_damper.update(state.r) # longitudinal autopilot # h_c = cmd.altitude_command # theta_c = self.altitude_from_pitch.update(h_c, state.h) delta_e = self.pitch_from_elevator.update(theta_c, state.theta, state.q) if at_rest: delta_t = 0. else: delta_t = MAV.delta_t_star + self.airspeed_from_throttle.update( cmd.airspeed_command, state.Va) # construct output and commanded states delta = np.array([[delta_e], [delta_a], [delta_r], [delta_t]]) self.commanded_state.h = cmd.altitude_command self.commanded_state.Va = cmd.airspeed_command self.commanded_state.phi = phi_c self.commanded_state.theta = theta_c self.commanded_state.chi = cmd.course_command return delta, self.commanded_state
def main(): data = loadmat('extended_information_filter/eif_data.mat') # Unpack data true_state = data['X_tr'] true_state = wrap(data['X_tr'], index=2) landmarks = data['m'] w_c = data['om_c'][0] w = data['om'][0] t = data['t'][0] v = data['v'][0] v_c = data['v_c'][0] true_bearing = data['bearing_tr'] true_range = data['range_tr'] eif = EIF(landmarks.T) # Initialize plots robot_plotter = RobotPlotter() robot_plotter.init_plot(true_state, eif.mean_belief, landmarks.T) # Initialize history for plotting all_mean_belief = [np.copy(eif.mean_belief)] all_covariance_belief = [np.copy(eif.covariance_belief)] all_information_vector = [np.copy(eif.info_vector)] # Go through data for time_step in range(1, len(t)): t_curr = t[time_step] change_t = t[time_step] - t[time_step-1] eif.prediction_step(v_c[time_step], w_c[time_step], change_t) eif.measurement_step(np.vstack((true_range[time_step], true_bearing[time_step]))) robot_plotter.update_plot(true_state[:, time_step], eif.mean_belief) all_mean_belief.append(np.copy(wrap(eif.mean_belief, index=2))) all_covariance_belief.append(np.copy(eif.covariance_belief)) all_information_vector.append(np.copy(eif.info_vector)) # Plot summary plot_summary(true_state, all_mean_belief, all_covariance_belief, t, all_information_vector)
def measurement_update(self, measurement): # always update pressure measurements, and fake sideslip u_ = np.array([]) h = self.h(self.xhat, u_) #C = self.jacobian(self.h, self.xhat, u_) C = self.h_jacobian(self.xhat) psuedo_vr = 0 #drive vr to zero so that beta = 0 y = np.array([[ measurement.static_pressure, measurement.diff_pressure, psuedo_vr, measurement.gps_n, measurement.gps_e, measurement.gps_Vg, measurement.gps_course ]]).T #wrap chi to be within pi of chi measured y[6] = wrap(y[6], h[6, 0]) for i in range(0, 3): Ci = C[i][None, :] if self.checkOutlier( self.R[i, i], Ci, self.P, y[i], h[i]) or self.checkOutlier( self.R[i, i], Ci, self.P, y[i], self.yave[i]): Li = np.dot(self.P, Ci.T) * 1 / ( self.R[i, i] + np.dot(np.dot(Ci, self.P), Ci.T)) temp = np.identity(14) - np.dot(Li, Ci) self.P = np.dot(np.dot(temp, self.P), (temp).T) + np.dot(Li, self.R[i, i] * Li.T) self.xhat = self.xhat + np.dot(Li, (y.item(i) - h.item(i))) # only update GPS when one of the signals changes if (measurement.gps_n != self.gps_n_old) \ or (measurement.gps_e != self.gps_e_old) \ or (measurement.gps_Vg != self.gps_Vg_old) \ or (measurement.gps_course != self.gps_course_old): for i in range(3, 7): Ci = C[i][None, :] if self.checkOutlier(self.R[i, i], Ci, self.P, y[i], h[i]) or self.checkOutlier( self.R[i, i], Ci, self.P, y[i], self.yave[i]): Li = np.dot(self.P, Ci.T) * 1 / ( self.R[i, i] + np.dot(np.dot(Ci, self.P), Ci.T)) temp = np.identity(14) - np.dot(Li, Ci) self.P = np.dot(np.dot(temp, self.P), temp.T) + np.dot( Li, self.R[i, i] * Li.T) self.xhat = self.xhat + np.dot(Li, (y.item(i) - h.item(i))) # update stored GPS signals self.gps_n_old = measurement.gps_n self.gps_e_old = measurement.gps_e self.gps_Vg_old = measurement.gps_Vg self.gps_course_old = measurement.gps_course #update average of measurements self.yave = (self.yave + y) / 2
def update(self, cmd, state): Va_c = cmd.airspeed_command chi_c = cmd.course_command h_c = cmd.altitude_command phi = state.phi theta = state.theta psi = state.psi # e0 = state._state.item(6) # FIX # e1 = state._state.item(7) # e2 = state._state.item(8) # e3 = state._state.item(9) # phi, theta, psi = Quaternion2Euler(e0, e1, e2, e3) h = state.h p = state.p q = state.q r = state.r # wrap the commanded course chi_c = wrap(chi_c, state.chi) # lateral autopilot phi_c = self.course_from_roll.update(chi_c, state.chi) delta_a = self.roll_from_aileron.update_with_rate(phi_c, phi, p) delta_r = self.sideslip_from_rudder.update(0, state.beta) # longitudinal autopilot theta_c = self.altitude_from_pitch.update(h_c, h) delta_e = self.pitch_from_elevator.update_with_rate(theta_c, theta, q) delta_t = self.airspeed_from_throttle.update(Va_c, state.Va) if delta_t < 0: delta_t = 0 # construct output and commanded states delta = np.array([[delta_a], [delta_e], [delta_t], [delta_r]]) self.commanded_state.h = cmd.altitude_command self.commanded_state.Va = cmd.airspeed_command self.commanded_state.phi = phi_c self.commanded_state.theta = theta_c self.commanded_state.chi = cmd.course_command return delta, self.commanded_state
def measurement_update(self, state, measurement): # always update based on wind triangle pseudo measurement h = self.h(self.xhat, state) C = jacobian(self.h, self.xhat, state) y = np.array([ measurement.gps_n, measurement.gps_e, measurement.gps_Vg, measurement.gps_course, 0, 0 ]) for i in range(4, 6): Ci = C[i][None, :] Li = np.dot(self.P, Ci.T) * 1 / (self.R[i, i] + np.dot(np.dot(Ci, self.P), Ci.T)) temp = np.identity(7) - np.dot(Li, Ci) self.P = np.dot(np.dot(temp, self.P), (temp).T) + np.dot(Li, self.R[i, i] * Li.T) self.xhat = self.xhat + np.dot(Li, (y[i] - h[i, 0])) # only update GPS when one of the signals changes if (measurement.gps_n != self.gps_n_old) \ or (measurement.gps_e != self.gps_e_old) \ or (measurement.gps_Vg != self.gps_Vg_old) \ or (measurement.gps_course != self.gps_course_old): h = self.h(self.xhat, state) C = jacobian(self.h, self.xhat, state) y = np.array([ measurement.gps_n, measurement.gps_e, measurement.gps_Vg, measurement.gps_course ]) for i in range(0, 4): Ci = C[i][None, :] Li = np.dot(self.P, Ci.T) * 1 / ( self.R[i, i] + np.dot(np.dot(Ci, self.P), Ci.T)) temp = np.identity(7) - np.dot(Li, Ci) if i == 4: y[i] = wrap(y[i], h[i, 0]) self.P = np.dot(np.dot(temp, self.P), (temp).T) + np.dot(Li, self.R[i, i] * Li.T) self.xhat = self.xhat + np.dot(Li, (y[i] - h[i, 0])) # update stored GPS signals self.gps_n_old = measurement.gps_n self.gps_e_old = measurement.gps_e self.gps_Vg_old = measurement.gps_Vg self.gps_course_old = measurement.gps_course
def particle_filter(self, robot, first_step): vc = robot.vc wc = robot.wc true_state = robot.actual_position updated_particles = [] # Simulate measurements measurements_from_robot = [] for index, feature in enumerate(self.all_features): f_x = feature[0] f_y = feature[1] if self.is_not_valid_measurement(f_x, f_y, true_state): continue measurement = simulate_measurement(robot.actual_position, f_x, f_y) measurements_from_robot.append((index, measurement)) # Update covariance and calculate weights all_weights = [] for particle in self.particles: v_perturbed = vc w_perturbed = wc v_perturbed = vc + robot.translational_noise(vc, wc) w_perturbed = wc + robot.rotational_noise(vc, wc) # sample pose position = particle[0] particle[0] = robot.next_position_from_state(position[0], position[1], position[2], v_perturbed, w_perturbed, self._change_t) particle[0] = wrap(particle[0], index=2) new_particle, w = self.ekf_measurement_step(measurements_from_robot, particle) updated_particles.append(new_particle) all_weights.append(w) all_weights = all_weights / np.sum(all_weights) # if np.count_nonzero(all_weights) < 100: self.particle_to_plot = self.particles[np.argmax(all_weights)] if first_step: self.particles = updated_particles else: self.resample_particles(updated_particles, all_weights) particles = np.array([particle[0] for particle in self.particles])[:,:2].reshape((-1,2)) self.mean_belief = np.mean(particles, axis=0) self.covariance_belief = np.var(particles, axis=0)
def measurement_update(self, state, measurement): # always update based on wind triangle pseudu measurement (pg. 145-146 supplement) h = self.h_pseudo(self.xhat, state) C = jacobian(self.h_pseudo, self.xhat, state) y = np.array([0, 0]).reshape(-1, 1) Ci = C[:, 4:6] L = self.P[4:6, 4:6] @ Ci.T @ np.linalg.inv(self.R_p + Ci @ self.P[4:6, 4:6] @ Ci.T) self.P[4:6, 4:6] = (np.eye(2) - L @ Ci) @ self.P[4:6, 4:6] @ ( np.eye(2) - L @ Ci).T + L @ self.R_p @ L.T self.xhat[4:6] += L @ (y - h) # only update GPS when one of the signals changes if (measurement.gps_n != self.gps_n_old) \ or (measurement.gps_e != self.gps_e_old) \ or (measurement.gps_Vg != self.gps_Vg_old) \ or (measurement.gps_course != self.gps_course_old): h = self.h_gps(self.xhat, state) C = jacobian(self.h_gps, self.xhat, state) y = np.array([ measurement.gps_n, measurement.gps_e, measurement.gps_Vg, measurement.gps_course ]).reshape(-1, 1) y[3, 0] = wrap(y[3, 0], h[3, 0]) Ci = C[:, 0:4] L = self.P[0:4, 0:4] @ Ci.T @ np.linalg.inv( self.R + Ci @ self.P[0:4, 0:4] @ Ci.T) self.P[0:4, 0:4] = (np.eye(4) - L @ Ci) @ self.P[0:4, 0:4] @ ( np.eye(4) - L @ Ci).T + L @ self.R @ L.T self.xhat[0:4] += L @ (y - h) # update stored GPS signals self.gps_n_old = measurement.gps_n self.gps_e_old = measurement.gps_e self.gps_Vg_old = measurement.gps_Vg self.gps_course_old = measurement.gps_course
def update(self, cmd, state): # lateral autopilot chi_c = wrap(cmd.course_command, state.chi) phi_c = self.course_from_roll.update(chi_c, state.chi) delta_a = self.roll_from_aileron.update(phi_c, state.phi, state.p) delta_r = self.yaw_damper.update(state.r) # longitudinal autopilot h_c = cmd.altitude_command theta_c = self.altitude_from_pitch.update(h_c, state.h) delta_e = self.pitch_from_elevator.update(theta_c, state.theta, state.q) delta_t = self.airspeed_from_throttle.update(cmd.airspeed_command, state.Va) # construct output and commanded states delta = np.array([[delta_e], [delta_a], [delta_r], [delta_t]]) self.commanded_state.h = cmd.altitude_command self.commanded_state.Va = cmd.airspeed_command self.commanded_state.phi = phi_c self.commanded_state.theta = theta_c self.commanded_state.chi = cmd.course_command return delta, self.commanded_state
def update(self, cmd, state, mavstate): """ mavstate is the self._state variable from the mav dynamics. We need to pick off u, v, w from here, since no other part of the simulator keeps track of that """ # cmd has four vars: airspeed_command, course_command, altitude_command, phi_feedforward # ignore phi_feedforward for now # lateral autopilot chi_c = wrap(cmd.course_command, state.chi) phi_c = cmd.phi_feedforward + self.course_from_roll.update( chi_c, state.chi) phi_c = self.saturate(phi_c, -np.radians(30), np.radians(30)) delta_a = self.roll_from_aileron.update(phi_c, state.phi, state.p) delta_r = self.yaw_damper.update(state.r) # longitudinal autopilot # saturate the altitude command alt_band = AP.altitude_zone altitude_c = self.saturate(cmd.altitude_command, state.altitude - alt_band, state.altitude + alt_band) theta_c = self.altitude_from_pitch.update(altitude_c, state.altitude) delta_e = self.pitch_from_elevator.update(theta_c, state.theta, state.q) delta_t = self.airspeed_from_throttle.update( cmd.airspeed_command - self.Va_trim, state.Va - self.Va_trim) delta_t = self.saturate(delta_t + u_trim.item(3), 0., 1.) # get x from state x = np.array([ state.north, state.east, -state.altitude, mavstate.item(3), mavstate.item(4), mavstate.item(5), state.phi, state.theta, state.chi, state.p, state.q, state.r ]) # set x_goal from cmd chi_c = wrap(cmd.course_command, state.chi) alt_band = AP.altitude_zone altitude_c = self.saturate(cmd.altitude_command, state.altitude - alt_band, state.altitude + alt_band) altitude_c = cmd.altitude_command self.xVaGoal[2] = altitude_c self.xVaGoal[8] = chi_c self.xVaGoal[12] = cmd.airspeed_command # get params for MPC errStates = [2] xVa = np.append(x, state.Va) err = np.linalg.norm(xVa[errStates] - self.xVaGoal[errStates]) if self.errMax is None: self.errMax = err inertia = err / self.errMax * (self.inertiaMax - self.inertiaMin) + self.inertiaMin self.controller.personalWeight = err * self.personalMax / self.errMax self.controller.socialWeight = (1 - err / self.errMax) * self.socialMax self.learnedSys.uFedIn = np.array([0.0, delta_a, delta_r, 0.0]) for i in range(1): u = self.controller.solve_for_next_u(x, self.xVaGoal, self.prevU, self.trimU, inertia=inertia) # u = self.prevU print("u: \t", u[[0, 3]]) self.prevU = u # construct output and commanded states delta = MsgDelta(elevator=u[0], aileron=delta_a, rudder=delta_r, throttle=u[3]) self.commanded_state.altitude = cmd.altitude_command self.commanded_state.Va = cmd.airspeed_command self.commanded_state.phi = phi_c self.commanded_state.theta = theta_c * 0 self.commanded_state.chi = cmd.course_command return delta, self.commanded_state
def is_not_valid_measurement(self, f_x, f_y, true_state): angle_to_check = wrap(np.arctan2((f_y-true_state[1]), (f_x-true_state[0])) - wrap(true_state[2])) return abs(angle_to_check) > self.vision_angle