# Extracting simulation data and storing it in a list for plotting
S_truth, S1_meas, S2_meas = nl_simulation(count)
alt_truth_list.append(S_truth[0])
v_truth_list.append(S_truth[1])
alt_meas1_list.append(S1_meas[0])
v_meas1_list.append(S1_meas[1])
alt_meas2_list.append(S2_meas[0])
v_meas2_list.append(S2_meas[1])
# Initializing KF for two gps sensor measurements
if count == 0:
S0 = S1_meas
P0 = np.eye((2))
two_sensor_ekf = EKF(S0, P0)
# Running KF with 2 gps sensor measurements
F = [[1, pi / 180], [0, 1]]
H = np.eye((2))
Q = np.power(q, 2) * np.eye((2))
R = np.power(r, 2) * np.eye((2))
two_sensor_ekf.ekf(S1_meas, F, H, Q, R)
# print("alt truth %f ekf alt_est %f " % (S_truth[0], two_sensor_ekf.S_est[0]))
# print("v truth %f ekf v_est %f " % (S_truth[1], two_sensor_ekf.S_est[1]))
ekf_alt_est_list.append(two_sensor_ekf.S_est[0])
count += 1
print("----plotting NL EKF----")
plot_two_sensors(count_list, alt_truth_list, alt_meas1_list,
alt_meas2_list, ekf_alt_est_list)
