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