def estimator_loop(y, xh, servo):
# get sensors for read_sensor function call.
adc, imu, baro, ubl = air.initialize_sensors()
time.sleep(3)
while True:
initialTime = current_milli_time()
new_gps = air.read_sensor(y, adc, imu, baro,
ubl) # updates values in y
# print(new_gps)
if (new_gps):
pass # do estimation with gps here.
else:
pass # do estimation without gps here.
# write estimated values to the xh array.
time.sleep(0.01 - (current_milli_time() - initialTime) / 1000)
def estimator_loop(y, xh, servo):
# get sensors for read_sensor function call.
sensors = air.initialize_sensors()
imu = sensors[1]
time.sleep(3)
count = 0
# Sensor installation details
Rba = np.array([[0, -1, 0], [-1, 0, 0],
[0, 0, 1]]) # acc frame to body (eg, imu to body)
# Environmental parameters
declination = +3.233 * np.pi / 180 # rad, mag declination is +3deg14' (eg, east) in Stillwater
pres_sl = 1010 # millibars, sea level pressure for the day. Update me! 1mb is 100Pa
rhoSL = 1.225 # kg/m^2, sea level standard density
g = 9.8065 # m/s^2, gravity
# bias calculate
print('WARNING! LEVEL AIRCRAFT UNTIL FURTHER NOTICE!')
master.mav.statustext_send(
mavutil.mavlink.MAV_SEVERITY_NOTICE,
'WARNING! LEVEL AIRCRAFT UNTIL FURTHER NOTICE!')
# Give 10 seconds of warmup
t1 = time.time()
gyro = np.array([[0, 0, 0]])
accel = np.array([[0, 0, 0]])
mag = np.array([[0, 0, 0]])
while time.time() - t1 < 10:
m9a, m9g, m9m = imu.getMotion9()
accel = np.append(accel, [m9a], axis=0)
gyro = np.append(gyro, [m9g], axis=0)
mag = np.append(mag, [m9m], axis=0)
time.sleep(0.1)
gyro_bias = [
np.average(gyro[:, 0]),
np.average(gyro[:, 1]),
np.average(gyro[:, 2])
]
accel_bias = [
np.average(accel[:, 0]),
np.average(accel[:, 1]),
np.average(accel[:, 2])
]
mag_bias = [
np.average(mag[:, 0]),
np.average(mag[:, 1]),
np.average(mag[:, 2])
]
print('CALIBRATION IS DONE!')
master.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_NOTICE,
'CALIBRATION IS DONE!')
# Define Q here
Q = np.eye(15)
R_INS = np.diag([
np.cov(accel[:, 0]),
np.cov(accel[:, 1]),
np.cov(accel[:, 2]), 1, 1, 1, 1, 1, 1, 1,
np.cov(gyro[:, 0]),
np.cov(gyro[:, 1]),
np.cov(gyro[:, 2])
])
R_AHRS = np.diag([
np.cov(accel[:, 0]),
np.cov(accel[:, 1]),
np.cov(accel[:, 2]), 10,
np.cov(gyro[:, 0]),
np.cov(gyro[:, 1]),
np.cov(gyro[:, 2])
])
accel = 0
gyro = 0
mag = 0
# Logging Initialization
# POC: Charlie
now = datetime.now()
date_time = now.strftime('%y-%m-%d_%H:%M:%S')
os.chdir('/home/pi/')
f_logfile = open('log_' + date_time + '.csv', 'w+')
# est_log_string = 'phi_a, theta_a, psi_m, x, y, -h_b, u, v, w, accel_bias, gyro_bias, rcin_0, rcin_1, rcin_2, rcin_3, rcin_4, rcin_5, servo_0, servo_1, servo_2, servo_3, servo_4, servo_5, ax, ay, az, gyro_p, gyro_q, gyro_r, mag_x, mag_y, mag_z, pres_baro, gps_posn_n, gps_posn_e, gps_posn_d, gps_vel_n, gps_vel_e, gps_vel_d\n'
est_log_string = 'x, y, z, Vt, alpha, beta, phi, theta, psi, pe, qe, re, rcin_0, rcin_1, rcin_2, rcin_3, rcin_4, rcin_5, servo_0, servo_1, servo_2, servo_3, servo_4, servo_5, ax, ay, az, gyro_p, gyro_q, gyro_r, mag_x, mag_y, mag_z, pres_baro, gps_posn_n, gps_posn_e, gps_posn_d, gps_vel_n, gps_vel_e, gps_vel_d\n'
f_logfile.write(est_log_string)
# =========================================================================
ax, ay, az, gyro_p, gyro_q, gyro_r, mag_x, mag_y, mag_z = range(9)
pres_baro = 9
gps_posn_n, gps_posn_e, gps_posn_d, gps_vel_n, gps_vel_e, gps_vel_d, gps_fix = range(
10, 17)
adc_a0, adc_a1, adc_a2, adc_a3, adc_a4, adc_a5, est_curr_consumed, last_curr_time = range(
17, 25)
while True:
initial_time = time.time()
new_gps = air.read_sensor(y, sensors) # updates values in y
# initiate
if count == 0:
tp = time.time()
xh_old = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0] # todo check and verify
P_old = numpy.eye(len(xh_old))
v_n_old = v_e_old = 0
# First compute sensor-specific estimates for imu/mag sensors
# Raw sensor data needs to be rotated to stability axes
acc = Rba.dot(
np.array([
y[ax] - accel_bias[0], y[ay] - accel_bias[1],
y[az] - accel_bias[2]
]))
mag = Rba.dot(
np.array([
y[mag_x] - mag_bias[0], y[mag_y] - mag_bias[1],
y[mag_z] - mag_bias[2]
]))
# Magnetic heading (psi_m)
Rhb = np.array([[
np.cos(xh_old[theta]),
np.sin(xh_old[theta]) * np.sin(xh_old[phi]),
np.sin(xh_old[theta]) * np.cos(xh_old[phi])
], [0, np.cos(xh_old[phi]), -np.sin(xh_old[phi])],
[
-np.sin(xh_old[theta]),
np.cos(xh_old[theta]) * np.sin(xh_old[phi]),
np.cos(xh_old[theta]) * np.cos(xh_old[phi])
]]) # rotation from body to horizontal plane
# print(Rhb)
magh = Rhb.dot(mag) # mag vector in horizontal plane components
psi_m = np.arctan2(magh[1], magh[0]) + declination
Rbf = np.array(
[[
cos(xh_old[theta]) * cos(psi_m),
cos(xh_old[theta]) * sin(psi_m), -sin(xh_old[theta])
],
[
sin(xh_old[phi]) * sin(xh_old[theta]) * cos(psi_m) -
cos(xh_old[phi]) * sin(psi_m),
sin(xh_old[phi]) * sin(xh_old[theta]) * sin(psi_m) +
cos(xh_old[phi]) * cos(psi_m),
sin(xh_old[phi]) * cos(xh_old[theta])
],
[
cos(xh_old[phi]) * sin(xh_old[theta]) * cos(psi_m) +
sin(xh_old[phi]) * sin(psi_m),
cos(xh_old[phi]) * sin(xh_old[theta]) * sin(psi_m) -
sin(xh_old[phi]) * cos(psi_m),
cos(xh_old[phi]) * cos(xh_old[theta])
]]) # rotation from fixed to body frame
# Pressure altitude
h_b = -(y[pres_baro] - pres_sl) * 100 / (rhoSL * g
) # *100 from mb to Pa
# =====ESTIMATOR CODE STARTS HERE==================================
xh_new = xh_old
sn = np.array([
acc[0], acc[1], acc[2], y[gyro_p] - gyro_bias[0],
y[gyro_q] - gyro_bias[1], y[gyro_r] - gyro_bias[2]
])
F = F_Find(xh_new, sn)
P = P_old
[xhminus, Phminus] = priori(xh_new, P, F, Q)
# Handle GPS and then fuse all measurements
if (new_gps):
[xh_new[x], xh_new[yy],
xh_new[z]] = [y[gps_posn_n], y[gps_posn_e], y[gps_posn_d]]
[xh_new[V_n], xh_new[V_e], xh_new[V_d]
] = np.dot(Rbf,
np.array([y[gps_vel_n], y[gps_vel_e], y[gps_vel_d]]))
zn = np.array([
acc[0], acc[1], acc[2], psi_m, y[gps_posn_n], y[gps_posn_e],
y[gps_posn_d], y[gps_vel_n], y[gps_vel_e], y[gps_vel_d],
y[gyro_p] - gyro_bias[0], y[gyro_q] - gyro_bias[1],
y[gyro_r] - gyro_bias[2]
]) # todo check and make sure it is correct
H = H_Find_INS(xh_new, sn)
# print('NEW GPS')
[xh_new, P] = posteriori(xhminus, Phminus, zn, H, R_INS)
else:
zn = np.array([
y[ax], y[ay], y[az],
np.arctan2(y[mag_y], y[mag_x]), y[gyro_p] - gyro_bias[0],
y[gyro_q] - gyro_bias[1], y[gyro_r] - gyro_bias[2]
]) # todo check and make sure it is correct
H = H_Find_AHRS(xh_new, sn)
[xh_new, P] = posteriori(xhminus, Phminus, zn, H, R_AHRS)
xh_new[x] = xh_new[x] + round(time.time() - tp, 3) * y[gps_vel_n]
xh_new[yy] = xh_new[yy] + round(time.time() - tp, 3) * y[gps_vel_e]
tp = time.time()
# OUTPUT: write estimated values to the xh array--------------------------------------
xh_new[z] = -h_b
xh_new[psi] = psi_m
vt = np.sqrt(y[gps_vel_n]**2 + y[gps_vel_e]**2 + y[gps_vel_d]**2)
#print('NORTH: ' + str(y[gps_vel_n]))
#print('EAST: ' + str(y[gps_vel_e]))
#print('DOWN: ' + str(y[gps_vel_d]))
#print('MAGNITUDE: ' + str(vt))
#print(np.sqrt(y[gps_vel_n]**2 + y[gps_vel_e]**2 + y[gps_vel_d]**2))
xh_old = xh_new
P_old = P
try:
alpha = numpy.arctan(y[gps_vel_d] / y[gps_vel_n])
except ZeroDivisionError:
alpha = 0.0
if vt == 0:
beta = 0
else:
beta = numpy.arcsin(y[gps_vel_n] / vt)
[pe, qe, re] = np.dot(Rbf.T, [
y[gyro_p] - gyro_bias[0], y[gyro_q] - gyro_bias[1],
y[gyro_r] - gyro_bias[2]
])
# xhat for controller
# define indices of xh for easier access.
# x, y, z, vt, alpha, beta, phi, theta, psi, p, q, r = range(12)
xc = np.array([
xh_new[x], xh_new[yy], xh_new[z], vt, alpha, beta, xh_new[phi],
xh_new[theta], xh_new[psi], pe, qe, re
])
# ==================================================
# ALL CODE ABOVE THIS LINE
# ==================================================
# DONE: Log X Hat, Servos, RCs, Y to CSV
f_logfile.write(', '.join(map(str, xc)) + ', '.join(map(str, servo)) +
', '.join(map(str, y)) + '\n')
# >>> TDB
count = 1
if (count % 8000) == 0:
print("Phi=%3.0f, Theta=%3.0f, Psi=%3.0f" %
(xh_old[phi] * 180 / np.pi, xh_old[theta] * 180 / np.pi,
psi_m * 180 / np.pi))
# ======ESTIMATOR CODE STOPS HERE===================================
# if (0.0125- (time.time()-initialEstTime) < 0): print( 1/(time.time()-initialEstTime) )
# for i in range(len(xh)):
# print(xh[i], end='')
# Copy all the values into the shared xh vector
for i in range(len(xh)):
xh[i] = xc[i]
if (new_gps and print_time_estimator_gps):
loop_time = (time.time() - initial_time)
print('estimator_gps loop time: ' + str(round(loop_time, 5)) +
' sec\t[' + str(int(round(1 / loop_time))) + ' hz]')
elif (not new_gps and print_time_estimator_nogps):
loop_time = (time.time() - initial_time)
print('estimator_nogps loop time: ' + str(round(loop_time, 5)) +
' sec\t[' + str(int(round(1 / loop_time))) + ' hz]')
time.sleep(max(0.0125 - (time.time() - initial_time), 0))
def estimator_loop(y, xh, servo):
# get sensors for read_sensor function call.
adc, imu, baro, ubl = air.initialize_sensors()
#Initialize Variables
ax_rot = ay_rot = az_rot = 0
v_n_old = v_e_old = v_d_old = 0
p_n = p_e = p_d = 0
u = v = w = 0
v_n = v_e = v_d = 0
v_n_dot = v_e_dot = v_d_dot = 0
A = [[-0.028, 0.233, 0, -9.815, 0, 0, 0, 0, 0, 0, 0, 0],
[-0.978, -8.966, 20.1170, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0.102, 0.022, -6.102, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, -0.45, 0, -0.986, 0.635, 0, 0, 0, 0],
[0, 0, 0, 0, 57.028, -72.97, 3.279, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 135.737, -0.588, -4.436, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]
B = [[0, 1, 0, 0], [-23.448, 0, 0, 0], [-50.313, -0.104, 0, 0],
[0, 0, 0, 0], [0, 0, 0, 0.315], [0, 0, 677.27, 18.099],
[0, 0, -8.875, -99.521], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0], [0, 0, 0, 0]]
F_c = [
[0.9997, 0.002, 0.0, -0.098, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[-0.0092, 0.9142, 0.1865, 0.00046, 0, 0, 0, 0, 0, 0, 0, 0],
[0.001, 0.0, 0.94, -4.888, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.01, 1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.989, 0.0, -0.0096, 0.006, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.42, 0.482, 0.02, 0.0015, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 1.32, -0.004, 0.95, 0.004, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.002, 0.007, 0.0, 1, 0.0, 0.0, 0.0, 0.0],
[0.01, 0.0, 0.0, -0.0004, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.00996, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
[0.0, 0.00956, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
]
G_c = [[-0.0002, 0.01, 0.0, 0.0], [-0.272, 0.0, 0.0, 0.0],
[-0.488, 0.0, 0.0, 0.0], [-0.00247, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.00054, 0.00796], [0.0, 0.0, 4.806, 0.1172],
[0.0, 0.0, -0.102, -0.9696], [0.0, 0.0, 0.0269, 0.000679],
[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0],
[-0.0013, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]]
H_c = [[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0]]
time.sleep(3) # Allows for initial reading of sensors
# ==========================================================================
# BIAS REMOVAL
# POC: Brandon
print('WARNING! LEVEL AIRCRAFT UNTIL FURTHER NOTICE!')
master.mav.statustext_send(
mavutil.mavlink.MAV_SEVERITY_NOTICE,
'WARNING! LEVEL AIRCRAFT UNTIL FURTHER NOTICE!')
time.sleep(2)
# Give 10 seconds of warmup
t1 = time.time()
gyro = np.array([[0, 0, 0]])
accel = np.array([[0, 0, 0]])
while time.time() - t1 < 5:
m9a, m9g, m9m = imu.getMotion9()
accel = np.append(accel, [m9a], axis=0)
gyro = np.append(gyro, [m9g], axis=0)
time.sleep(0.05)
gyro_bias = [
np.average(gyro[:, 0]),
np.average(gyro[:, 1]),
np.average(gyro[:, 2])
]
accel_bias = [
np.average(accel[:, 0]),
np.average(accel[:, 1]),
np.average(accel[:, 2])
]
# >>> ADD IN COVARIANCE
#sampled_data = np.array([[np.transpose(gyro[:,0])],
# [np.transpose(gyro[:,0])],
# [np.transpose(gyro[:,0])],
# [np.transpose(accel[:,0])],
# [np.transpose(accel[:,0])],
# [np.transpose(accel[:,0])],
# [np.transpose(accel[:,0])]]
#R = np.cov(sampled_data)
accel = 0 # Free memory
gyro = 0 # Free memory
print('Sensor Bias Calibration Completed')
master.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_NOTICE,
'Sensor Bias Calibration Completed')
# ==========================================================================
# ==========================================================================
# Logging Initialization
# POC: Charlie
now = datetime.now()
date_time = now.strftime('%y_%m_%d__%H_%M_%S')
os.chdir('/home/pi/')
f_logfile = open('log_' + date_time + '.csv', 'w+')
est_log_string = 'phi_a, theta_a, psi_m, x, y, -h_b, u, v, w, a_bias_x, a_bias_y, a_bias_z, gyro_bias_x, gyro_bias_y, gyro_bias_z, rcin_0, rcin_1, rcin_2, rcin_3, rcin_4, rcin_5, servo_0, servo_1, servo_2, servo_3, servo_4, servo_5, ax, ay, az, gyro_q, gyro_p, gyro_r, mag_x, mag_y, mag_z, pres_baro, gps_posn_n, gps_posn_e, gps_posn_d, gps_vel_n, gps_vel_e, gps_vel_d\n'
f_logfile.write(est_log_string)
# ==========================================================================
ax, ay, az, gyro_p, gyro_q, gyro_r, mag_x, mag_y, mag_z = range(9)
pres_baro = 9
gps_posn_n, gps_posn_e, gps_posn_d, gps_vel_n, gps_vel_e, gps_vel_d, gps_fix = range(
10, 17)
adc_a0, adc_a1, adc_a2, adc_a3, adc_a4, adc_a5, est_curr_consumed, last_curr_time = range(
17, 25)
pres_sl = 1010
rhoSL = 1.225
g = 9.8065
pressure_conversion = 100 / (rhoSL * g)
# Define Q here
Q = np.eye(15)
while True:
# ==================================================
# Read Data
new_gps = air.read_sensor(y, adc, imu, baro,
ubl) # updates values in y
# ==================================================
# Create X Hat
# POCs: Ujjval, Nick, Brandon, Charlie
# Correct directionalities
R_imu = np.array([[0, -1, 0], [-1, 0, 0], [0, 0, 1]])
# >>> UJJVAL CHECK HERE
[ax_rot, ay_rot,
az_rot][0] = np.dot(R_imu, np.transpose([y[ax], y[ay], y[az]]))
ax_rot = ax_rot - accel_bias[0]
ay_rot = ay_rot - accel_bias[1]
az_rot = az_rot - accel_bias[2]
# Completing angular relations
phi_a = np.arctan2(ay_rot,
math.sqrt(ax_rot**2 + az_rot**2)) # Reliable
theta_a = np.arctan2(ax_rot,
math.sqrt(ay_rot**2 + az_rot**2)) # Reliable
# psi_a = np.arctan2(math.sqrt(ax_rot**2+ay_rot**2), az_rot**2) # Unreliable
# Get Psi from Magnetometer
mag = np.dot(R_imu,
np.transpose(np.array([y[mag_x], y[mag_y], y[mag_z]])))
psi_m = np.arctan2(mag[1], mag[0])
# Now that we have these values, we make a rotation matrix
Rhb = np.array([[
np.cos(theta_a),
np.sin(theta_a) * np.sin(phi_a),
np.sin(theta_a) * np.cos(phi_a)
], [0, np.cos(phi_a), -np.sin(phi_a)],
[
-np.sin(theta_a),
np.cos(theta_a) * np.sin(phi_a),
np.cos(theta_a) * np.cos(phi_a)
]])
# Barometer Altitude
h_b = (y[pres_baro] - pres_sl) * pressure_conversion
# Accept the rate gyro values
# >>> UJJVAL CHECK HERE
q = y[gyro_p] - gyro_bias[0]
p = y[gyro_q] - gyro_bias[1]
r = y[gyro_r] - gyro_bias[2]
if new_gps:
[v_n_old, v_e_old, v_d_old] = [v_n, v_e, v_d]
[p_n, p_e, p_d, v_n, v_e, v_d] = [
y[gps_posn_n], y[gps_posn_e], y[gps_posn_d], y[gps_vel_n],
y[gps_vel_e], y[gps_vel_d]
]
delta_t = round(time.time() - t1, 3)
t1 = time.time()
try:
[v_n_dot, v_e_dot, v_d_dot
] = ([v_n, v_e, v_d] - [v_n_old, v_e_old, v_d_old]) / delta_t
except:
[v_n_dot, v_e_dot, v_d_dot] = [0, 0, 0]
Rbf = np.array([[
cos(theta_a) * cos(psi_m),
cos(theta_a) * sin(psi_m), -sin(theta_a)
],
[
sin(phi_a) * sin(theta_a) * cos(psi_m) -
cos(phi_a) * sin(psi_m),
sin(phi_a) * sin(theta_a) * sin(psi_m) +
cos(phi_a) * cos(psi_m),
sin(phi_a) * cos(theta_a)
],
[
cos(phi_a) * sin(theta_a) * cos(psi_m) +
sin(phi_a) * sin(psi_m),
cos(phi_a) * sin(theta_a) * sin(psi_m) -
sin(phi_a) * cos(psi_m),
cos(phi_a) * cos(theta_a)
]])
[u, v, w][0] = np.dot(Rbf, np.array([[v_n], [v_e], [v_d]]))
# XTODO: need to define x without GPS. Initialize as zero before loop? -Charlie
#INITIALIZED BEFORE LOOP - BRANDON
# TODO: need to define u, v, w. Not sure where those are comping from. -Charlie
#UPDATED WITH ROTATION MATRIX
xh = np.array([
phi_a, theta_a, psi_m, p_n, p_e, -h_b, u, v, w, accel_bias[0],
accel_bias[1], accel_bias[2], gyro_bias[0], gyro_bias[1],
gyro_bias[2]
])
# ==================================================
# Kalman Matrices
# POC: Ujjval
# Create F Matix
F = F_Find(xh, [y[ax], y[ay], y[az], y[gyro_p], y[gyro_q], y[gyro_r]])
# Create H Matix
# >>> TBD by UJJVAL
# H = H_Find(xh, [y[ax], y[ay], y[az], y[gyro_p], y[gyro_q], y[gyro_r]])
# Kalman Filter Algebra
# >>> TBD
# ==================================================
# ALL CODE ABOVE THIS LINE
# ==================================================
# DONE: Log X Hat, Servos, RCs, Y to CSV
f_logfile.write(', '.join(map(str, xh)) + ', ' +
', '.join(map(str, servo)) + ', ' +
', '.join(map(str, y)) + '\n')
def estimator_loop(y, xh, servo):
# get sensors for read_sensor function call.
adc, imu, baro, ubl = air.initialize_sensors()
time.sleep(3)
while True:
new_gps = air.read_sensor(y, adc, imu, baro, ubl) # updates values in y
# print(new_gps)
if (new_gps):
pass # do estimation with gps here.
else:
pass # do estimation without gps here.
y = np.array(
[0.538385085, -0.035892339, 9.662217659, -0.002443459, 0.133168509, 0.014660753, -5.046, -20.184,
57.246])
A = [[-0.028, 0.233, 0, -9.815, 0, 0, 0, 0, 0, 0, 0, 0]
[-0.978, -8.966, 20.1170, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[0.102, 0.022, -6.102, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[0, 0, 0, 0, -0.45, 0, -0.986, 0.635, 0, 0, 0, 0]
[0, 0, 0, 0, 57.028, -72.97, 3.279, 0, 0, 0, 0, 0]
[0, 0, 0, 0, 135.737, -0.588, -4.436, 0, 0, 0, 0, 0]
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]
B = [[0, 1, 0, 0]
[-23.448, 0, 0, 0]
[-50.313, -0.104, 0, 0]
[0, 0, 0, 0]
[0, 0, 0, 0.315]
[0, 0, 677.27, 18.099]
[0, 0, -8.875, -99.521]
[0, 0, 0, 0]
[0, 0, 0, 0]
[0, 0, 0, 0]
[0, 0, 0, 0]
[0, 0, 0, 0]]
F = np.expm(A*0.01)
def heading_calc(mag_x, mag_y, mag_z, phi, theta):
dt = 3 # declination angle in degrees
M_B = np.array([mag_x, mag_y, mag_z]) # magnetometer output
M_i = Rot_i_B(M_B, phi, theta) # put magnetometer in inertial frame
psi = dt + math.atan(M_i[1] / M_i[2]) # dt is declination angle
return psi
def Rot_i_B(Matrix, phi, theta):
Rot_Mat = np.array([[math.cos(theta), math.sin(theta) * math.sin(phi), math.sin(theta) * math.cos(phi)],
[0, math.cos(phi), -math.sin(phi)],
[-math.sin(theta), math.cos(theta) * math.sin(phi),
math.cos(theta) * math.cos(phi)]])
R_Mat = Matrix * Rot_Mat
return R_Mat
# define initial states from y
ax = y[0]
ay = y[1]
az = y[2]
gyro_p = y[3]
gyro_q = y[4]
gyro_r = y[5]
mag_x = y[6]
mag_y = y[7]
mag_z = y[8]
# definition of noise and biases
AccelVariance = .002 # noise of accelerometer
GyroVariance = 1e-5 # noise of gyro
AttitudeVariance = .3 # attitude noise
AccelBias = np.array([0, 0, 0]) # Bias of Accelerometer... from christian
GyroBias = np.array([0, 0, 0]) # gyro bias
MagBias = np.array([0, 0, 0]) # magnetometer bias
# initial orientation estimate
phi = 0
theta = 0
psi = heading_calc(mag_x, mag_y, mag_z, phi, theta) # need to create function to do this
euler = np.array([phi, theta, psi])
# define x_hat
x_hat = np.array(
[euler, GyroBias, AccelBias]) # 1-3 Euler angles ,,,, #4-6 xyz gyro bias estimates, #7-9 xyz gyro bias
# define R uncertainty in measurement
Variance = np.arrays[AccelVariance,AccelVariance,AccelVariance,GyroVariance,GyroVariance,GyroVariance,AttitudeVariance,AttitudeVariance,AttitudeVariance]
Identity = np.identity(9)
R = Identity * Variance
# Define Q uncertainty in model
Q = np.identity(9)
EKFl = EKF(initial_x=x_hat, initial_P=P)
test = EXF1.step(F, Q, G, U, y, hx, C, R)
def estimator_loop(y,xh,servo):
global u
# get sensors for read_sensor function call.
adc,imu,baro,ubl = air.initialize_sensors()
time.sleep(3)
count=0
#Sensor installation details
Rba=np.array([[0,-1,0], [-1,0,0], [0,0,1]]) #acc frame to body (eg, imu to body)
#Environmental parameters
declination=+3.233*np.pi/180 #rad, mag declination is +3deg14' (eg, east) in Stillwater
pres_sl=1010 #millibars, sea level pressure for the day. Update me! 1mb is 100Pa
rhoSL=1.225 #kg/m^2, sea level standard density
g=9.8065 #m/s^2, gravity
print('Coletti Estimator-Controller V0.9, remember to check V_e and V_d sign conv for H on ins steps')
#bias calculate
print('Performing bias calibration...')
#master.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_NOTICE, 'WARNING! LEVEL AIRCRAFT UNTIL FURTHER NOTICE!')
time.sleep(2)
# Give 10 seconds of warmup
t1 = time.time()
gyro = np.array([[0, 0, 0]])
accel = np.array([[0, 0, 0]])
mag = np.array([[0, 0, 0]])
while time.time() - t1 < 10:
m9a, m9g, m9m = imu.getMotion9()
accel = np.append(accel, [m9a], axis=0)
gyro = np.append(gyro, [m9g], axis=0)
mag = np.append(mag, [m9m], axis=0)
time.sleep(0.05)
gyro_bias = [np.average(gyro[:, 0]), np.average(gyro[:, 1]), np.average(gyro[:, 2])]
accel_bias = [np.average(accel[:, 0]), np.average(accel[:, 1]), np.average(accel[:, 2])]
mag_bias = [np.average(mag[:, 0]), np.average(mag[:, 1]), np.average(mag[:, 2])]
print('bias calibration complete')
# ==========================================================================
# Logging Initialization
# POC: Charlie
now = datetime.now()
date_time = now.strftime('%y_%m_%d__%H_%M_%S')
os.chdir('/home/pi/')
f_logfile = open('log_' + date_time + '.csv', 'w+')
est_log_string = 'p_n, p_e, -h_b, Vt, alpha, beta, phi_a, theta_a, psi_m, p, q, r, rcin_0, rcin_1, rcin_2, rcin_3, rcin_4, rcin_5, rcin_6, servo_0, servo_1, servo_2, servo_3, servo_4, servo_5, ax, ay, az, gyro_q, gyro_p, gyro_r, mag_x, mag_y, mag_z, pres_baro, gps_posn_n, gps_posn_e, gps_posn_d, gps_vel_n, gps_vel_e, gps_vel_d\n'
f_logfile.write(est_log_string)
# ==========================================================================
# Define Q here
Q = np.eye(12)
#cov psi_mag #cov baro, GPS cov: n, e, d, vn, ve vd
R_INS = np.diag([np.cov(accel[:, 0]),np.cov(accel[:, 1]),np.cov(accel[:, 2]),2354.336792,np.cov(gyro[:, 0]),np.cov(gyro[:, 1]),np.cov(gyro[:, 2]),524071.8484, 532.9199581, 23.98810422, 677884.7773, 2.580815226, 2.214463558, 13.63108936,])
R_AHRS = np.diag([np.cov(accel[:, 0]),np.cov(accel[:, 1]),np.cov(accel[:, 2]),2354.336792,np.cov(gyro[:, 0]),np.cov(gyro[:, 1]),np.cov(gyro[:, 2])])
#cov psi_mag
accel = 0
gyro = 0
mag = 0
controlsDer = np.array([0, 1, -23.448, 0, -50.313, -0.104, 8.169, 1137, 30.394, -14.904, -167.131])
Xde, XdT, Zde, ZdT, MdeMwZde, MdTMwZdT, Ydr, Lda, Ldr, Nda, Ndr = controlsDer
B=np.array([[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[Xde, XdT, 0, 0],
[0, 0, 0, Ydr],
[Zde, ZdT, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, Lda, Ldr],
[MdeMwZde, MdTMwZdT, 0, 0],
[0, 0, Nda, Ndr]])
while True:
initialEstTime = time.time()
new_gps = air.read_sensor(y,adc,imu,baro,ubl) # updates values in y
#initiate
if count == 0:
xh_old = [0,0,0,0,0,0,0,0,np.arctan2(y[mag_y], y[mag_x])+declination,0,0,0]
P_old = numpy.eye(len(xh))
#First compute sensor-specific estimates for imu/mag sensors
#Raw sensor data needs to be rotated to stability axes
acc=Rba.dot( np.array([y[ax]-accel_bias[0],y[ay]-accel_bias[1],y[az]-accel_bias[2]]) )
mag=Rba.dot( np.array([y[mag_x]-mag_bias[0],y[mag_y]-mag_bias[1],y[mag_z]-mag_bias[2]]))
#Magnetic heading (psi_m)
Rhb=np.array([[np.cos(xh_old[theta]), np.sin(xh_old[theta])*np.sin(xh_old[phi]), np.sin(xh_old[theta])*np.cos(xh_old[phi])], [0, np.cos(xh_old[phi]), -np.sin(xh_old[phi])], [-np.sin(xh_old[theta]), np.cos(xh_old[theta])*np.sin(xh_old[phi]), np.cos(xh_old[theta])*np.cos(xh_old[phi])]]) #rotation from body to horizontal plane
magh=Rhb.dot(mag) #mag vector in horizontal plane components
psi_m = np.arctan2(magh[1], magh[0]) + declination
Rbf = np.array([[cos(xh_old[theta]) * cos(psi_m), cos(xh_old[theta]) * sin(psi_m), -sin(xh_old[theta])], [sin(xh_old[phi]) * sin(xh_old[theta]) * cos(psi_m) - cos(xh_old[phi]) * sin(psi_m),sin(xh_old[phi]) * sin(xh_old[theta]) * sin(psi_m) + cos(xh_old[phi]) * cos(psi_m), sin(xh_old[phi]) * cos(xh_old[theta])],[cos(xh_old[phi]) * sin(xh_old[theta]) * cos(psi_m) + sin(xh_old[phi]) * sin(psi_m), cos(xh_old[phi]) * sin(xh_old[theta]) * sin(psi_m) - sin(xh_old[phi]) * cos(psi_m), cos(xh_old[phi]) * cos(xh_old[theta])]]) #rotation from fixed to body frame
#Pressure altitude
h_b= -(y[pres_baro]-pres_sl)*100 /(rhoSL*g) #*100 from mb to Pa
dt = 0.125
#=====ESTIMATOR CODE STARTS HERE==================================
xh = xh_old
A = Afunc(xh)
F = FindF(A, dt)
G = FindG(A, F, B, dt)
P = P_old
[xhminus, Pminus] = priori(xh, u, P, F, G)
#Handle GPS and then fuse all measurements
if (new_gps):
z = [y[ax]-accel_bias[0],y[ay]-accel_bias[1],y[az]-accel_bias[2], psi_m, y[gyro_p]-gyro_bias[0], y[gyro_q]-gyro_bias[1], y[gyro_r]-gyro_bias[2], y[gps_posn_n], y[gps_posn_e], h_b, y[gps_vel_n], y[gps_vel_e], y[gps_vel_d]]
H = H_INS(xh)
[xh, P] = posteriori(xhminus, Pminus, z, H, R_INS)
else:
z = [y[ax]-accel_bias[0],y[ay]-accel_bias[1],y[az]-accel_bias[2], psi_m, y[gyro_p]-gyro_bias[0], y[gyro_q]-gyro_bias[1], y[gyro_r]-gyro_bias[2]]
H = H_AHRS(xh)
[xh, P] = posteriori(xhminus, Pminus, z, H, R_AHRS)
#OUTPUT: write estimated values to the xh array--------------------------------------
xh_old = xh
P_old = P
alpha = atan(xh[5]/xh[3]) #w/u
beta = asin(xh[4]/xh[3]) #v/u only good for mostly u flight
# DONE: Log X Hat, Servos, RCs, Y to CSV
f_logfile.write(
', '.join(map(str, xh)) + ', ' + ', '.join(map(str, servo)) + ', ' + ', '.join(map(str, y)) + '\n')
count=count+1
if (count % 8000)==0:
print("Phi=%3.0f, Theta=%3.0f, Psi=%3.0f" % (xh_old[phi]*180/np.pi, xh_old[theta]*180/np.pi, psi_m*180/np.pi))
#======ESTIMATOR CODE STOPS HERE===================================
#if (0.0125- (time.time()-initialEstTime) < 0): print( 1/(time.time()-initialEstTime) )
time.sleep(max(0.0125-(time.time()-initialEstTime),0) )
def estimator_loop(y, xh, servo):
# get sensors for read_sensor function call.
adc, imu, baro, ubl = air.initialize_sensors()
time.sleep(3)
count = 0
#Sensor installation details
Rba = np.array([[0, -1, 0], [-1, 0, 0],
[0, 0, 1]]) #acc frame to body (eg, imu to body)
#Environmental parameters
declination = +3.233 * np.pi / 180 #rad, mag declination is +3deg14' (eg, east) in Stillwater
pres_sl = 1010 #millibars, sea level pressure for the day. Update me! 1mb is 100Pa
rhoSL = 1.225 #kg/m^2, sea level standard density
g = 9.8065 #m/s^2, gravity
while True:
initialEstTime = time.time()
new_gps = air.read_sensor(y, adc, imu, baro,
ubl) # updates values in y
#=====ESTIMATOR CODE STARTS HERE==================================
#PREDICT: Predict step here (use old xh to find new xh)--------------------------------------
#eg, xhatkminus = F*xhatkminusoneplus + G*ukminusone
#CORRECT: Measurement correction step here (depends on which sensors available)--------------
#First compute sensor-specific estimates for imu/mag sensors
#Raw sensor data needs to be rotated to stability axes
acc = Rba.dot(np.array([y[ax], y[ay], y[az]]))
mag = Rba.dot(np.array([y[mag_x], y[mag_y], y[mag_z]]))
#---add a line here for gyro rotation
#Accel based pitch/roll estimater (assumes no accel other than gravity)
phi_a = np.arctan2(acc[1], np.sqrt(acc[0]**2 + acc[2]**2))
theta_a = np.arctan2(-acc[0], np.sqrt(acc[1]**2 + acc[2]**2))
#Gyro based pitch/roll estimator
#--add a routine here integrating the gyros for phi_g, theta_g
#Magnetic heading (psi_m)
Rhb = np.array([[
np.cos(theta_a),
np.sin(theta_a) * np.sin(phi_a),
np.sin(theta_a) * np.cos(phi_a)
], [0, np.cos(phi_a), -np.sin(phi_a)],
[
-np.sin(theta_a),
np.cos(theta_a) * np.sin(phi_a),
np.cos(theta_a) * np.cos(phi_a)
]]) #rotation from body to horizontal plane
magh = Rhb.dot(mag) #mag vector in horizontal plane components
psi_m = np.arctan2(magh[1], magh[0]) + declination
#Pressure altitude
h_b = -(y[pres_baro] - pres_sl) * 100 / (rhoSL * g
) #*100 from mb to Pa
#Handle GPS and then fuse all measurements
if (new_gps):
pass # use gps to find x_g, y_g, z_g, Vt_g, alpha, beta
#---then fuse with xhminus to find xhplus
else:
pass # all non-GPS estimates already found, just fuse with xhminus to find xhplus
#OUTPUT: write estimated values to the xh array--------------------------------------
xh[z] = -h_b
xh[phi] = phi_a
xh[theta] = theta_a
xh[psi] = psi_m
count = count + 1
if (count % 8000) == 0:
print("Phi=%3.0f, Theta=%3.0f, Psi=%3.0f" %
(phi_a * 180 / np.pi, theta_a * 180 / np.pi,
psi_m * 180 / np.pi))
#======ESTIMATOR CODE STOPS HERE===================================
#if (0.0125- (time.time()-initialEstTime) < 0): print( 1/(time.time()-initialEstTime) )
time.sleep(max(0.0125 - (time.time() - initialEstTime), 0))
