def analyse_ekf( ulog: ULog, check_levels: Dict[str, float], red_thresh: float = 1.0, amb_thresh: float = 0.5, min_flight_duration_seconds: float = 5.0, in_air_margin_seconds: float = 5.0, pos_checks_when_sensors_not_fused: bool = False) -> \ Tuple[str, Dict[str, str], Dict[str, float], Dict[str, float]]: """ :param ulog: :param check_levels: :param red_thresh: :param amb_thresh: :param min_flight_duration_seconds: :param in_air_margin_seconds: :param pos_checks_when_sensors_not_fused: :return: """ try: estimator_status = ulog.get_dataset('estimator_status').data print('found estimator_status data') except: raise PreconditionError('could not find estimator_status data') try: _ = ulog.get_dataset('ekf2_innovations').data print('found ekf2_innovation data') except: raise PreconditionError('could not find ekf2_innovation data') try: in_air = InAirDetector( ulog, min_flight_time_seconds=min_flight_duration_seconds, in_air_margin_seconds=0.0) in_air_no_ground_effects = InAirDetector( ulog, min_flight_time_seconds=min_flight_duration_seconds, in_air_margin_seconds=in_air_margin_seconds) except Exception as e: raise PreconditionError(str(e)) if in_air_no_ground_effects.take_off is None: raise PreconditionError('no airtime detected.') airtime_info = { 'in_air_transition_time': round(in_air.take_off + in_air.log_start, 2), 'on_ground_transition_time': round(in_air.landing + in_air.log_start, 2)} control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status) sensor_checks, innov_fail_checks = find_checks_that_apply( control_mode, estimator_status, pos_checks_when_sensors_not_fused=pos_checks_when_sensors_not_fused) metrics = calculate_ecl_ekf_metrics( ulog, innov_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects, red_thresh=red_thresh, amb_thresh=amb_thresh) check_status, master_status = perform_ecl_ekf_checks( metrics, sensor_checks, innov_fail_checks, check_levels) return master_status, check_status, metrics, airtime_info
def calculate_imu_metrics(ulog: ULog, in_air_no_ground_effects: InAirDetector) -> dict: ekf2_innovation_data = ulog.get_dataset('ekf2_innovations').data estimator_status_data = ulog.get_dataset('estimator_status').data imu_metrics = dict() # calculates the median of the output tracking error ekf innovations for signal, result in [ ('output_tracking_error[0]', 'output_obs_ang_err_median'), ('output_tracking_error[1]', 'output_obs_vel_err_median'), ('output_tracking_error[2]', 'output_obs_pos_err_median') ]: imu_metrics[result] = calculate_stat_from_signal( ekf2_innovation_data, 'ekf2_innovations', signal, in_air_no_ground_effects, np.median) # calculates peak and mean for IMU vibration checks for signal, result in [('vibe[0]', 'imu_coning'), ('vibe[1]', 'imu_hfdang'), ('vibe[2]', 'imu_hfdvel')]: peak = calculate_stat_from_signal(estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.amax) if peak > 0.0: imu_metrics['{:s}_peak'.format(result)] = peak imu_metrics['{:s}_mean'.format( result)] = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.mean) # IMU bias checks imu_metrics['imu_dang_bias_median'] = np.sqrt( np.sum([ np.square( calculate_stat_from_signal(estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)) for signal in ['states[10]', 'states[11]', 'states[12]'] ])) imu_metrics['imu_dvel_bias_median'] = np.sqrt( np.sum([ np.square( calculate_stat_from_signal(estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)) for signal in ['states[13]', 'states[14]', 'states[15]'] ])) return imu_metrics
def testing_args(): """ arguments for testing. :return: test arguments """ flight_logs_path = os.path.join(os.path.dirname(__file__), 'flight_logs') log_est_format_v1 = ULog( os.path.join(flight_logs_path, 'short_f450_log.ulg')) log_est_format_v2 = ULog( os.path.join(flight_logs_path, 'SITL_VTOL_standard_d7b958.ulg')) return { 'log_est_format_v1': log_est_format_v1, 'log_est_format_v2': log_est_format_v2 }
def testing_args(): """ arguments for testing. :return: test arguments """ flight_logs_path = os.path.join(os.path.dirname(__file__), 'flight_logs') log_est_format_v1 = ULog( os.path.join(flight_logs_path, 'short_f450_log.ulg')) log_est_format_v2 = ULog( os.path.join(flight_logs_path, 'estimator_innovations.ulg')) return { 'est_format_version_1': log_est_format_v1, 'est_format_version_2': log_est_format_v2 }
def __init__(self, ulog: ULog): """ :param ulog: """ super(EclCheckRunner, self).__init__() try: estimator_status_data = ulog.get_dataset('estimator_status').data print('found estimator_status data') control_mode_flags, innov_flags, _ = get_estimator_check_flags( estimator_status_data) self.append( MagnetometerCheck(ulog, innov_flags, control_mode_flags)) self.append( MagneticHeadingCheck(ulog, innov_flags, control_mode_flags)) self.append(VelocityCheck(ulog, innov_flags, control_mode_flags)) self.append(PositionCheck(ulog, innov_flags, control_mode_flags)) self.append(HeightCheck(ulog, innov_flags, control_mode_flags)) self.append( HeightAboveGroundCheck(ulog, innov_flags, control_mode_flags)) self.append(AirspeedCheck(ulog, innov_flags, control_mode_flags)) self.append(SideSlipCheck(ulog, innov_flags, control_mode_flags)) self.append(OpticalFlowCheck(ulog, innov_flags, control_mode_flags)) self.append(IMU_Vibration_Check(ulog)) self.append(IMU_Bias_Check(ulog)) self.append(IMU_Output_Predictor_Check(ulog)) self.append(NumericalCheck(ulog)) except Exception as e: capture_message(str(e)) self.error_message = str(e) self.analysis_status = -3
def __init__(self, ulog: ULog, min_flight_time_seconds: float = 0.0, in_air_margin_seconds: float = 0.0) -> None: """ initializes an InAirDetector instance. :param ulog: :param min_flight_time_seconds: set this value to return only airtimes that are at least min_flight_time_seconds long :param in_air_margin_seconds: removes a margin of in_air_margin_seconds from the airtime to avoid ground effects. """ self._ulog = ulog self._min_flight_time_seconds = min_flight_time_seconds self._in_air_margin_seconds = in_air_margin_seconds try: self._vehicle_land_detected = ulog.get_dataset('vehicle_land_detected').data self._landed = self._vehicle_land_detected['landed'] except: self._in_air = [] raise PreconditionError( 'InAirDetector: Could not find vehicle land detected message and/or landed field' ' and thus not find any airtime.') self._log_start = self._ulog.start_timestamp / 1.0e6 self._in_air = self._detect_airtime()
def calculate_ecl_ekf_metrics( ulog: ULog, innov_flags: Dict[str, float], innov_fail_checks: List[str], sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector, multi_instance: int = 0, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]: sensor_metrics = calculate_sensor_metrics( ulog, sensor_checks, in_air, in_air_no_ground_effects, red_thresh=red_thresh, amb_thresh=amb_thresh) innov_fail_metrics = calculate_innov_fail_metrics( innov_flags, innov_fail_checks, in_air, in_air_no_ground_effects) imu_metrics = calculate_imu_metrics(ulog, multi_instance, in_air_no_ground_effects) estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data # Check for internal filter nummerical faults ekf_metrics = {'filter_faults_max': np.amax(estimator_status_data['filter_fault_flags'])} # TODO - process these bitmask's when they have been properly documented in the uORB topic # estimator_status['health_flags'] # estimator_status['timeout_flags'] # combine the metrics combined_metrics = dict() combined_metrics.update(imu_metrics) combined_metrics.update(sensor_metrics) combined_metrics.update(innov_fail_metrics) combined_metrics.update(ekf_metrics) return combined_metrics
def calculate_ecl_ekf_metrics( ulog: ULog, innov_flags: Dict[str, float], innov_fail_checks: List[str], sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]: sensor_metrics = calculate_sensor_metrics( ulog, sensor_checks, in_air, in_air_no_ground_effects, red_thresh=red_thresh, amb_thresh=amb_thresh) innov_fail_metrics = calculate_innov_fail_metrics( innov_flags, innov_fail_checks, in_air, in_air_no_ground_effects) imu_metrics = calculate_imu_metrics(ulog, in_air_no_ground_effects) estimator_status_data = ulog.get_dataset('estimator_status').data # Check for internal filter nummerical faults ekf_metrics = {'filter_faults_max': np.amax(estimator_status_data['filter_fault_flags'])} # TODO - process these bitmask's when they have been properly documented in the uORB topic # estimator_status['health_flags'] # estimator_status['timeout_flags'] # combine the metrics combined_metrics = dict() combined_metrics.update(imu_metrics) combined_metrics.update(sensor_metrics) combined_metrics.update(innov_fail_metrics) combined_metrics.update(ekf_metrics) return combined_metrics
def __init__( self, ulog: ULog, min_flight_time_seconds: float = 0.0, in_air_margin_seconds: float = 0.0) -> None: """ initializes an InAirDetector instance. :param ulog: :param min_flight_time_seconds: set this value to return only airtimes that are at least min_flight_time_seconds long :param in_air_margin_seconds: removes a margin of in_air_margin_seconds from the airtime to avoid ground effects. """ self._ulog = ulog self._min_flight_time_seconds = min_flight_time_seconds self._in_air_margin_seconds = in_air_margin_seconds try: self._vehicle_land_detected = ulog.get_dataset('vehicle_land_detected').data self._landed = self._vehicle_land_detected['landed'] except: self._in_air = [] raise PreconditionError( 'InAirDetector: Could not find vehicle land detected message and/or landed field' ' and thus not find any airtime.') self._log_start = self._ulog.start_timestamp / 1.0e6 self._in_air = self._detect_airtime()
def getAllData(logfile): log = ULog(logfile) v_local = np.matrix([getData(log, 'vehicle_local_position', 'vx'), getData(log, 'vehicle_local_position', 'vy'), getData(log, 'vehicle_local_position', 'vz')]) t_v_local = ms2s(getData(log, 'vehicle_local_position', 'timestamp')) accel = np.matrix([getData(log, 'sensor_combined', 'accelerometer_m_s2[0]'), getData(log, 'sensor_combined', 'accelerometer_m_s2[1]'), getData(log, 'sensor_combined', 'accelerometer_m_s2[2]')]) t_accel = ms2s(getData(log, 'sensor_combined', 'timestamp')) q = np.matrix([getData(log, 'vehicle_attitude', 'q[0]'), getData(log, 'vehicle_attitude', 'q[1]'), getData(log, 'vehicle_attitude', 'q[2]'), getData(log, 'vehicle_attitude', 'q[3]')]) t_q = ms2s(getData(log, 'vehicle_attitude', 'timestamp')) dist_bottom = getData(log, 'vehicle_local_position', 'dist_bottom') t_dist_bottom = ms2s(getData(log, 'vehicle_local_position', 'timestamp')) (t_aligned, v_body_aligned, accel_aligned) = alignData(t_v_local, v_local, t_accel, accel, t_q, q, t_dist_bottom, dist_bottom) t_aligned -= t_aligned[0] return (t_aligned, v_body_aligned, accel_aligned)
def getBarometerData(ulog: ULog) -> pd.DataFrame: vehicle_air_data = ulog.get_dataset("vehicle_air_data").data baro = pd.DataFrame({'timestamp': vehicle_air_data['timestamp'], 'sensor' : 'baro', 'baro_alt_meter': vehicle_air_data["baro_alt_meter"]}) return baro
def ulog2docx(ulg_file_name, doc_file_name=""): ulog = ULog(ulg_file_name) ### position plot config = {} config["layout"] = [1, 1] config["axeses"] = [] axes = {} axes["title"] = None axes["xlabel"] = "East/m" axes["ylabel"] = "North/m" axes["lines"] = [{"xdata": []}] ### altitude estimate config = {} config["layout"] = [2, 1] config["suptitle"] = "Alt Estimate" config["axeses"] = [] # axes 1 axes = {} axes["title"] = "" axes["xlabel"] = "Time" axes["ylabel"] = "Alt/m" axes["lines"] = [{ "xdata": "vehicle_global_position.timestamp", "ydata": [rescale, "vehicle_global_position.alt", 0.001, 0], "specs": "-r", "label": "Fused Alt" }, { "xdata": "vehicle_air_data.timestamp", "ydata": ["vehicle_air_data", "baro_alt_meter"], "specs": "-k", "label": "Baro Alt" }, { "xdata": ["vehicle_gps_position", "timestamp"], "process": lambda x: x * 0.001, "ydata": [lambda: "vehicle_gps_position", "alt"], "spec": "-b", "label": "GPS Alt" }] config["axeses"].append(axes) # axes 2 axes = {} axes["title"] = "" axes["xlabel"] = "Time/sec" axes["ylabel"] = "Thrust" axes["lines"] = [{ "xdata": ["actuator_controls_0", "timestamp"], "ydata": ["actuator_controls_0", "control[3]"], "spec": "-r", "process": None, "label": "Thrust[0,100]" }] config["axeses"].append(axes) # plot plot2d(ulog, config)
def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects: InAirDetector) -> dict: estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data imu_metrics = dict() # calculates the median of the output tracking error ekf innovations for signal, result in [('output_tracking_error[0]', 'output_obs_ang_err_median'), ('output_tracking_error[1]', 'output_obs_vel_err_median'), ('output_tracking_error[2]', 'output_obs_pos_err_median')]: imu_metrics[result] = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median) # calculates peak and mean for IMU vibration checks for imu_status_instance in range(4): try: vehicle_imu_status_data = ulog.get_dataset('vehicle_imu_status', imu_status_instance).data if vehicle_imu_status_data['accel_device_id'][0] == estimator_status_data['accel_device_id'][0]: for signal, result in [('gyro_coning_vibration', 'imu_coning'), ('gyro_vibration_metric', 'imu_hfgyro'), ('accel_vibration_metric', 'imu_hfaccel')]: peak = calculate_stat_from_signal(vehicle_imu_status_data, 'vehicle_imu_status', signal, in_air_no_ground_effects, np.amax) if peak > 0.0: imu_metrics['{:s}_peak'.format(result)] = peak imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal(vehicle_imu_status_data, 'vehicle_imu_status', signal, in_air_no_ground_effects, np.mean) except: pass # IMU bias checks estimator_states_data = ulog.get_dataset('estimator_states', multi_instance).data imu_metrics['imu_dang_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal( estimator_states_data, 'estimator_states', signal, in_air_no_ground_effects, np.median)) for signal in ['states[10]', 'states[11]', 'states[12]']])) imu_metrics['imu_dvel_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal( estimator_states_data, 'estimator_states', signal, in_air_no_ground_effects, np.median)) for signal in ['states[13]', 'states[14]', 'states[15]']])) return imu_metrics
def process_logdata_ekf(filename: str, check_level_dict_filename: str, check_table_filename: str, plot: bool = True, sensor_safety_margins: bool = True): ## load the log and extract the necessary data for the analyses try: ulog = ULog(filename) except: raise PreconditionError('could not open {:s}'.format(filename)) try: # get the dictionary of fail and warning test thresholds from a csv file with open(check_level_dict_filename, 'r') as file: reader = csv.DictReader(file) check_levels = { row['check_id']: float(row['threshold']) for row in reader } print('Using test criteria loaded from {:s}'.format( check_level_dict_filename)) except: raise PreconditionError( 'could not find {:s}'.format(check_level_dict_filename)) in_air_margin = 5.0 if sensor_safety_margins else 0.0 # perform the ekf analysis master_status, check_status, metrics, airtime_info = analyse_ekf( ulog, check_levels, red_thresh=1.0, amb_thresh=0.5, min_flight_duration_seconds=5.0, in_air_margin_seconds=in_air_margin) test_results = create_results_table(check_table_filename, master_status, check_status, metrics, airtime_info) # write metadata to a .csv file with open('{:s}.mdat.csv'.format(filename), "w") as file: file.write("name,value,description\n") # loop through the test results dictionary and write each entry on a separate row, with data comma separated # save data in alphabetical order key_list = list(test_results.keys()) key_list.sort() for key in key_list: file.write(key + "," + str(test_results[key][0]) + "," + test_results[key][1] + "\n") print('Test results written to {:s}.mdat.csv'.format(filename)) if plot: create_pdf_report(ulog, '{:s}.pdf'.format(filename)) print('Plots saved to {:s}.pdf'.format(filename)) return test_results
def getAirspeedData(ulog: ULog) -> pd.DataFrame: airspeed = ulog.get_dataset("airspeed").data airspeed = pd.DataFrame({'timestamp': airspeed['timestamp'], 'sensor' : 'airspeed', 'true_as': airspeed["true_airspeed_m_s"], 'indicated_as': airspeed["indicated_airspeed_m_s"] }) return airspeed
def getMagnetometerData(ulog: ULog) -> pd.DataFrame: vehicle_magnetometer = ulog.get_dataset("vehicle_magnetometer").data mag = pd.DataFrame({'timestamp': vehicle_magnetometer['timestamp'], 'sensor' : 'mag', 'magnetometer_ga[0]': vehicle_magnetometer["magnetometer_ga[0]"], 'magnetometer_ga[1]': vehicle_magnetometer["magnetometer_ga[1]"], 'magnetometer_ga[2]': vehicle_magnetometer["magnetometer_ga[2]"]}) return mag
def test_in_air_detector_on_flight_log(testing_args): """ tests the basics of the in air detector using the basic test flight log file. :param testing_args: :return: """ basic_test_log_filename = os.path.join( testing_args['test_flight_logs_path'], testing_args['simple_test_flight_log']) ulog = ULog(basic_test_log_filename) original_take_offs(ulog)
def getImuData(ulog: ULog) -> pd.DataFrame: sensor_combined = ulog.get_dataset("sensor_combined").data imu = pd.DataFrame({'timestamp': sensor_combined['timestamp'], 'sensor' : 'imu', 'accel_m_s2[0]': sensor_combined["accelerometer_m_s2[0]"], 'accel_m_s2[1]': sensor_combined["accelerometer_m_s2[1]"], 'accel_m_s2[2]': sensor_combined["accelerometer_m_s2[2]"], 'gyro_rad[0]': sensor_combined["gyro_rad[0]"], 'gyro_rad[1]': sensor_combined["gyro_rad[1]"], 'gyro_rad[2]': sensor_combined["gyro_rad[2]"]}) return imu
def calculate_sensor_metrics(ulog: ULog, sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector, multi_instance: int = 0, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Dict[str, float]: estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data sensor_metrics = dict() # calculates peak, mean, percentage above 0.5 std, and percentage above std metrics for # estimator status variables for signal, result_id in [('hgt_test_ratio', 'hgt'), ('mag_test_ratio', 'mag'), ('vel_test_ratio', 'vel'), ('pos_test_ratio', 'pos'), ('tas_test_ratio', 'tas'), ('hagl_test_ratio', 'hagl')]: # only run sensor checks, if they apply. if result_id in sensor_checks: if result_id == 'mag' or result_id == 'hgt': in_air_detector = in_air_no_ground_effects else: in_air_detector = in_air # the percentage of samples above / below std dev sensor_metrics['{:s}_percentage_red'.format( result_id)] = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_detector, lambda x: 100.0 * np.mean(x > red_thresh)) sensor_metrics['{:s}_percentage_amber'.format(result_id)] = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_detector, lambda x: 100.0 * np.mean(x > amb_thresh)) - \ sensor_metrics['{:s}_percentage_red'.format(result_id)] # the peak and mean ratio of samples above / below std dev peak = calculate_stat_from_signal(estimator_status_data, 'estimator_status', signal, in_air_detector, np.amax) if peak > 0.0: sensor_metrics['{:s}_test_max'.format(result_id)] = peak sensor_metrics['{:s}_test_mean'.format( result_id)] = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_detector, np.mean) return sensor_metrics
def getOpticalFlowData(ulog: ULog) -> pd.DataFrame: optical_flow = ulog.get_dataset("optical_flow").data flow = pd.DataFrame({'timestamp': optical_flow['timestamp'], 'sensor' : 'flow', 'pixel_flow_x_integral': optical_flow["pixel_flow_x_integral"], 'pixel_flow_y_integral': optical_flow["pixel_flow_y_integral"], 'gyro_x_rate_integral': optical_flow["gyro_x_rate_integral"], 'gyro_y_rate_integral': optical_flow["gyro_y_rate_integral"], 'gyro_z_rate_integral': optical_flow["gyro_z_rate_integral"], 'quality': optical_flow["quality"] }) return flow
def getGpsData(ulog: ULog) -> pd.DataFrame: vehicle_gps_position = ulog.get_dataset("vehicle_gps_position").data gps = pd.DataFrame({'timestamp': vehicle_gps_position['timestamp'], 'sensor' : 'gps', 'alt': vehicle_gps_position["alt"], 'lon': vehicle_gps_position["lon"], 'lat': vehicle_gps_position["lat"], 'vel_N': vehicle_gps_position["vel_n_m_s"], 'vel_E': vehicle_gps_position["vel_e_m_s"], 'vel_D': vehicle_gps_position["vel_d_m_s"], }) return gps
def check_if_field_name_exists_in_message(ulog: ULog, message: str, field_name: str) -> bool: """ Check if a field is part of a message in a certain log """ exists = True if message not in [elem.name for elem in ulog.data_list]: exists = False elif field_name not in ulog.get_dataset(message).data.keys(): exists = False return exists
def test_position_analyzer(testing_args): """ :param testing_args: :return: """ filename = os.path.join(testing_args['golden_flight_logs_path'], testing_args['golden_flight_logs'][0]) position_analyzer = PositionAnalyzer(ULog(filename)) position_analyzer.set_min_ground_distance(0.2) assert position_analyzer.get_valid_position('sensor_combined') == [], \ 'returned valid positions were not empty'
def getRangeFinderData(ulog: ULog) -> pd.DataFrame: range = pd.DataFrame() try: range_0 = ulog.get_dataset("distance_sensor", 0).data rng_0 = pd.DataFrame({'timestamp': range_0['timestamp'], 'sensor' : 'range', 'data': range_0["current_distance"], 'quality': range_0["signal_quality"] }) range = pd.concat([range, rng_0], ignore_index=True, sort=False) except: pass try: range_1 = ulog.get_dataset("distance_sensor", 1).data rng_1 = pd.DataFrame({'timestamp': range_1['timestamp'], 'sensor' : 'range', 'data': range_1["current_distance"], 'quality': range_1["signal_quality"] }) range = pd.concat([range, rng_1], ignore_index=True, sort=False) except: pass try: range_2 = ulog.get_dataset("distance_sensor", 2).data rng_2 = pd.DataFrame({'timestamp': range_2['timestamp'], 'sensor' : 'range', 'data': range_2["current_distance"], 'quality': range_2["signal_quality"] }) range = pd.concat([range, rng_2], ignore_index=True, sort=False) except: pass return range
def calculate_imu_metrics( ulog: ULog, in_air_no_ground_effects: InAirDetector) -> dict: ekf2_innovation_data = ulog.get_dataset('ekf2_innovations').data estimator_status_data = ulog.get_dataset('estimator_status').data imu_metrics = dict() # calculates the median of the output tracking error ekf innovations for signal, result in [('output_tracking_error[0]', 'output_obs_ang_err_median'), ('output_tracking_error[1]', 'output_obs_vel_err_median'), ('output_tracking_error[2]', 'output_obs_pos_err_median')]: imu_metrics[result] = calculate_stat_from_signal( ekf2_innovation_data, 'ekf2_innovations', signal, in_air_no_ground_effects, np.median) # calculates peak and mean for IMU vibration checks for signal, result in [('vibe[0]', 'imu_coning'), ('vibe[1]', 'imu_hfdang'), ('vibe[2]', 'imu_hfdvel')]: peak = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.amax) if peak > 0.0: imu_metrics['{:s}_peak'.format(result)] = peak imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.mean) # IMU bias checks imu_metrics['imu_dang_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)) for signal in ['states[10]', 'states[11]', 'states[12]']])) imu_metrics['imu_dvel_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)) for signal in ['states[13]', 'states[14]', 'states[15]']])) return imu_metrics
def load_ulog_file(file_name): """ load an ULog file :return: ULog object """ # The reason to put this method into helper is that the main module gets # (re)loaded on each page request. Thus the caching would not work there. # load only the messages we really need msg_filter = ['battery_status', 'estimator_status', 'sensor_combined', 'cpuload', 'vehicle_gps_position', 'vehicle_local_position', 'vehicle_global_position', 'vehicle_attitude', 'vehicle_rates_setpoint', 'vehicle_attitude_groundtruth', 'vehicle_local_position_groundtruth', 'vehicle_status', 'airspeed', 'rate_ctrl_status', 'vehicle_air_data', 'vehicle_magnetometer', 'system_power'] # has been removed , because useless: # position_setpoint_triplet # 'actuator_controls_1' ,'actuator_controls_0','actuator_outputs' # distance_sensor # 'vehicle_local_position_setpoint', 'vehicle_angular_velocity','vehicle_attitude_setpoint' # 'tecs_status' # 'rc_channels', 'input_rc', # 'manual_control_setpoint','vehicle_visual_odometry' try: ulog = ULog(file_name, msg_filter, disable_str_exceptions=False) except FileNotFoundError: print("Error: file %s not found" % file_name) raise # catch all other exceptions and turn them into an ULogException except Exception as error: traceback.print_exception(*sys.exc_info()) raise ULogException() # filter messages with timestamp = 0 (these are invalid). # The better way is not to publish such messages in the first place, and fix # the code instead (it goes against the monotonicity requirement of ulog). # So we display the values such that the problem becomes visible. # for d in ulog.data_list: # t = d.data['timestamp'] # non_zero_indices = t != 0 # if not np.all(non_zero_indices): # d.data = np.compress(non_zero_indices, d.data, axis=0) return ulog
def check_if_field_name_exists_in_message(ulog: ULog, message_name: str, field_name: str) -> bool: """ Check if a field is part of a message in a certain log """ if field_name is None: return False msg_data = ulog.get_dataset(message_name).data field_name_list = dict.keys(msg_data) for elem in field_name_list: if field_name == field_name_wo_brackets(elem): return True return False
def getVioData(ulog: ULog) -> pd.DataFrame: vehicle_visual_odometry = ulog.get_dataset("vehicle_visual_odometry").data vio = pd.DataFrame({'timestamp': vehicle_visual_odometry['timestamp'], 'sensor' : 'vio', 'x': vehicle_visual_odometry["x"], 'y': vehicle_visual_odometry["y"], 'z': vehicle_visual_odometry["z"], 'qw': vehicle_visual_odometry["q[0]"], 'qx': vehicle_visual_odometry["q[1]"], 'qy': vehicle_visual_odometry["q[2]"], 'qz': vehicle_visual_odometry["q[3]"], 'vx': vehicle_visual_odometry["vx"], 'vy': vehicle_visual_odometry["vy"], 'vz': vehicle_visual_odometry["vz"] }) return vio
def test_basics_in_air_detector(testing_args): """ tests the basics of the in air detector on a dummy log file. :param testing_args: :return: """ dummy_log_filename = os.path.join(testing_args['test_flight_logs_path'], testing_args['dummy_log_file']) ulog = ULog(dummy_log_filename) always_on_ground(ulog) always_in_air(ulog) start_in_air(ulog) take_off_at_second_time_stamp(ulog) multiple_take_offs(ulog)
def getInputOutputData(logfile, axis, instance=0): log = ULog(logfile) y_data = get_data(log, 'vehicle_angular_velocity', 'xyz[{}]'.format(axis)) t_y_data = ms2s(get_data(log, 'vehicle_angular_velocity', 'timestamp')) actuator_controls_n = 'actuator_controls_{}'.format(instance) u_data = get_data(log, actuator_controls_n, 'control[{}]'.format(axis)) t_u_data = ms2s(get_data(log, actuator_controls_n, 'timestamp')) (t_aligned, u_aligned, y_aligned) = extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis) t_aligned -= t_aligned[0] return (t_aligned, u_aligned, y_aligned)
def convert_ulog2csv(ulog_file_name): """ Coverts and ULog file to a CSV file. :param ulog_file_name: The ULog filename to open and read :param messages: A list of message names :param output: Output file path :param delimiter: CSV delimiter :return: None """ msg_filter = None disable_str_exceptions=False delimiter = ',' ulog = ULog(ulog_file_name, msg_filter, disable_str_exceptions) data = ulog.data_list for d in data: # use same field order as in the log, except for the timestamp data_keys = [f.field_name for f in d.field_data] data_keys.remove('timestamp') # data_keys.insert(0, 'timestamp') # we want timestamp at first position # we don't use np.savetxt, because we have multiple arrays with # potentially different data types. However the following is quite # slow... # write the header # print(delimiter.join(data_keys) + '\n') # write the data # last_elem = len(data_keys)-1 # for i in range(len(d.data['timestamp'])): # for k in range(len(data_keys)): # csvfile.write(str(d.data[data_keys[k]][i])) # if k != last_elem: # csvfile.write(delimiter) # csvfile.write('\n') print("Key =", data_keys) for g in data_keys: keylist.append(g) # print("Data =", d.data) print("Keylist =", keylist) print("One item", keylist[10])
def calculate_sensor_metrics( ulog: ULog, sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Dict[str, float]: estimator_status_data = ulog.get_dataset('estimator_status').data sensor_metrics = dict() # calculates peak, mean, percentage above 0.5 std, and percentage above std metrics for # estimator status variables for signal, result_id in [('hgt_test_ratio', 'hgt'), ('mag_test_ratio', 'mag'), ('vel_test_ratio', 'vel'), ('pos_test_ratio', 'pos'), ('tas_test_ratio', 'tas'), ('hagl_test_ratio', 'hagl')]: # only run sensor checks, if they apply. if result_id in sensor_checks: if result_id == 'mag' or result_id == 'hgt': in_air_detector = in_air_no_ground_effects else: in_air_detector = in_air # the percentage of samples above / below std dev sensor_metrics['{:s}_percentage_red'.format(result_id)] = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_detector, lambda x: 100.0 * np.mean(x > red_thresh)) sensor_metrics['{:s}_percentage_amber'.format(result_id)] = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_detector, lambda x: 100.0 * np.mean(x > amb_thresh)) - \ sensor_metrics['{:s}_percentage_red'.format(result_id)] # the peak and mean ratio of samples above / below std dev peak = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_detector, np.amax) if peak > 0.0: sensor_metrics['{:s}_test_max'.format(result_id)] = peak sensor_metrics['{:s}_test_mean'.format(result_id)] = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_detector, np.mean) return sensor_metrics
def info_to_df(ulog, verbose, file_name): """Show general information from an ULog""" time_s = int((ulog.last_timestamp - ulog.start_timestamp) / 1e6) print("time_s: ", time_s) file_size = os.path.getsize( file_log) / 1e6 # getsize give the size in bytes , converted th Mb log_debit = file_size / time_s # so mb/s" print("file_size: " + str(file_size) + " mb \t log_debit: " + str(log_debit) + " mb/s") sd_log = ulog.initial_parameters["SDLOG_PROFILE"] print("sdlog_mode: ", sd_log) # print("") # print("{:<41} {:7}, {:10}".format("Name (multi id, message size in bytes)", # "number of data points", "frequency")) data = [] data_list_sorted = sorted(ulog.data_list, key=lambda d: d.name + str(d.multi_id)) for d in data_list_sorted: message_size = sum( [ULog.get_field_size(f.type_str) for f in d.field_data]) num_data_points = len(d.data['timestamp']) name_id = "{:}".format(d.name) data.append( dict({ 'name': name_id, 'mode': sd_log, 'frequency': num_data_points / time_s, 'mess_size': message_size, 'file': file_name, 'debit': log_debit })) # # print(" {:<40} {:2f}".format(name_id, num_data_points / time_s)) df = pd.DataFrame(data) return df
def getAllData(logfile): log = ULog(logfile) v_local = np.matrix([ getData(log, 'vehicle_local_position', 'vx'), getData(log, 'vehicle_local_position', 'vy'), getData(log, 'vehicle_local_position', 'vz') ]) t_local = ms2s(getData(log, 'vehicle_local_position', 'timestamp')) dist_bottom = getData(log, 'vehicle_local_position', 'dist_bottom') baro = getData(log, 'vehicle_air_data', 'baro_alt_meter') t_baro = ms2s(getData(log, 'vehicle_air_data', 'timestamp')) baro_bias = getData(log, 'estimator_baro_bias', 'bias') t_baro_bias = ms2s(getData(log, 'estimator_baro_bias', 'timestamp')) q = np.matrix([ getData(log, 'vehicle_attitude', 'q[0]'), getData(log, 'vehicle_attitude', 'q[1]'), getData(log, 'vehicle_attitude', 'q[2]'), getData(log, 'vehicle_attitude', 'q[3]') ]) t_q = ms2s(getData(log, 'vehicle_attitude', 'timestamp')) gnss_h = getData(log, 'vehicle_gps_position', 'alt') * 1e-3 t_gnss = ms2s(getData(log, 'vehicle_gps_position', 'timestamp')) (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned) = alignData(t_local, v_local, dist_bottom, t_q, q, baro, t_baro, t_gnss, gnss_h, t_baro_bias, baro_bias) t_aligned -= t_aligned[0] return (t_aligned, v_body_aligned, baro_aligned, v_local_z_aligned, gnss_h_aligned, baro_bias_aligned)
def process_logdata_ekf(filename: str, plot: bool = False) -> List[dict]: """ main function for processing the logdata for ekf analysis. :param filename: :param plot: :return: """ try: ulog = ULog(filename) except Exception as e: raise PreconditionError('could not open {:s}'.format(filename)) from e test_results = analyse_logdata_ekf(ulog) with open('{:s}.json'.format(os.path.splitext(filename)[0]), 'w') as file: json.dump(test_results, file, indent=2) if plot: create_pdf_report(ulog, '{:s}.pdf'.format(filename)) print('Plots saved to {:s}.pdf'.format(filename)) return test_results
def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: """ creates a pdf report of the ekf analysis. :param ulog: :param output_plot_filename: :return: """ # create summary plots # save the plots to PDF try: estimator_status = ulog.get_dataset('estimator_status').data print('found estimator_status data') except: raise PreconditionError('could not find estimator_status data') try: ekf2_innovations = ulog.get_dataset('ekf2_innovations').data print('found ekf2_innovation data') except: raise PreconditionError('could not find ekf2_innovation data') try: sensor_preflight = ulog.get_dataset('sensor_preflight').data print('found sensor_preflight data') except: raise PreconditionError('could not find sensor_preflight data') control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status) status_time = 1e-6 * estimator_status['timestamp'] b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, \ on_ground_transition_time = detect_airtime(control_mode, status_time) with PdfPages(output_plot_filename) as pdf_pages: # plot IMU consistency data if ('accel_inconsistency_m_s_s' in sensor_preflight.keys()) and ( 'gyro_inconsistency_rad_s' in sensor_preflight.keys()): data_plot = TimeSeriesPlot( sensor_preflight, [['accel_inconsistency_m_s_s'], ['gyro_inconsistency_rad_s']], x_labels=['data index', 'data index'], y_labels=['acceleration (m/s/s)', 'angular rate (rad/s)'], plot_title='IMU Consistency Check Levels', pdf_handle=pdf_pages) data_plot.save() data_plot.close() # vertical velocity and position innovations data_plot = InnovationPlot( ekf2_innovations, [('vel_pos_innov[2]', 'vel_pos_innov_var[2]'), ('vel_pos_innov[5]', 'vel_pos_innov_var[5]')], x_labels=['time (sec)', 'time (sec)'], y_labels=['Down Vel (m/s)', 'Down Pos (m)'], plot_title='Vertical Innovations', pdf_handle=pdf_pages) data_plot.save() data_plot.close() # horizontal velocity innovations data_plot = InnovationPlot( ekf2_innovations, [('vel_pos_innov[0]', 'vel_pos_innov_var[0]'), ('vel_pos_innov[1]','vel_pos_innov_var[1]')], x_labels=['time (sec)', 'time (sec)'], y_labels=['North Vel (m/s)', 'East Vel (m/s)'], plot_title='Horizontal Velocity Innovations', pdf_handle=pdf_pages) data_plot.save() data_plot.close() # horizontal position innovations data_plot = InnovationPlot( ekf2_innovations, [('vel_pos_innov[3]', 'vel_pos_innov_var[3]'), ('vel_pos_innov[4]', 'vel_pos_innov_var[4]')], x_labels=['time (sec)', 'time (sec)'], y_labels=['North Pos (m)', 'East Pos (m)'], plot_title='Horizontal Position Innovations', pdf_handle=pdf_pages) data_plot.save() data_plot.close() # magnetometer innovations data_plot = InnovationPlot( ekf2_innovations, [('mag_innov[0]', 'mag_innov_var[0]'), ('mag_innov[1]', 'mag_innov_var[1]'), ('mag_innov[2]', 'mag_innov_var[2]')], x_labels=['time (sec)', 'time (sec)', 'time (sec)'], y_labels=['X (Gauss)', 'Y (Gauss)', 'Z (Gauss)'], plot_title='Magnetometer Innovations', pdf_handle=pdf_pages) data_plot.save() data_plot.close() # magnetic heading innovations data_plot = InnovationPlot( ekf2_innovations, [('heading_innov', 'heading_innov_var')], x_labels=['time (sec)'], y_labels=['Heading (rad)'], plot_title='Magnetic Heading Innovations', pdf_handle=pdf_pages) data_plot.save() data_plot.close() # air data innovations data_plot = InnovationPlot( ekf2_innovations, [('airspeed_innov', 'airspeed_innov_var'), ('beta_innov', 'beta_innov_var')], x_labels=['time (sec)', 'time (sec)'], y_labels=['innovation (m/sec)', 'innovation (rad)'], sub_titles=['True Airspeed Innovations', 'Synthetic Sideslip Innovations'], pdf_handle=pdf_pages) data_plot.save() data_plot.close() # optical flow innovations data_plot = InnovationPlot( ekf2_innovations, [('flow_innov[0]', 'flow_innov_var[0]'), ('flow_innov[1]', 'flow_innov_var[1]')], x_labels=['time (sec)', 'time (sec)'], y_labels=['X (rad/sec)', 'Y (rad/sec)'], plot_title='Optical Flow Innovations', pdf_handle=pdf_pages) data_plot.save() data_plot.close() # plot normalised innovation test levels # define variables to plot variables = [['mag_test_ratio'], ['vel_test_ratio', 'pos_test_ratio'], ['hgt_test_ratio']] y_labels = ['mag', 'vel, pos', 'hgt'] legend = [['mag'], ['vel', 'pos'], ['hgt']] if np.amax(estimator_status['hagl_test_ratio']) > 0.0: # plot hagl test ratio, if applicable variables[-1].append('hagl_test_ratio') y_labels[-1] += ', hagl' legend[-1].append('hagl') if np.amax(estimator_status[ 'tas_test_ratio']) > 0.0: # plot airspeed sensor test ratio, if applicable variables.append(['tas_test_ratio']) y_labels.append('TAS') legend.append(['airspeed']) data_plot = CheckFlagsPlot( status_time, estimator_status, variables, x_label='time (sec)', y_labels=y_labels, plot_title='Normalised Innovation Test Levels', pdf_handle=pdf_pages, annotate=True, legend=legend ) data_plot.save() data_plot.close() # plot control mode summary A data_plot = ControlModeSummaryPlot( status_time, control_mode, [['tilt_aligned', 'yaw_aligned'], ['using_gps', 'using_optflow', 'using_evpos'], ['using_barohgt', 'using_gpshgt', 'using_rnghgt', 'using_evhgt'], ['using_magyaw', 'using_mag3d', 'using_magdecl']], x_label='time (sec)', y_labels=['aligned', 'pos aiding', 'hgt aiding', 'mag aiding'], annotation_text=[['tilt alignment', 'yaw alignment'], ['GPS aiding', 'optical flow aiding', 'external vision aiding'], ['Baro aiding', 'GPS aiding', 'rangefinder aiding', 'external vision aiding'], ['magnetic yaw aiding', '3D magnetoemter aiding', 'magnetic declination aiding']], plot_title='EKF Control Status - Figure A', pdf_handle=pdf_pages) data_plot.save() data_plot.close() # plot control mode summary B # construct additional annotations for the airborne plot airborne_annotations = list() if np.amin(np.diff(control_mode['airborne'])) > -0.5: airborne_annotations.append( (on_ground_transition_time, 'air to ground transition not detected')) else: airborne_annotations.append((on_ground_transition_time, 'on-ground at {:.1f} sec'.format( on_ground_transition_time))) if in_air_duration > 0.0: airborne_annotations.append(((in_air_transition_time + on_ground_transition_time) / 2, 'duration = {:.1f} sec'.format(in_air_duration))) if np.amax(np.diff(control_mode['airborne'])) < 0.5: airborne_annotations.append( (in_air_transition_time, 'ground to air transition not detected')) else: airborne_annotations.append( (in_air_transition_time, 'in-air at {:.1f} sec'.format(in_air_transition_time))) data_plot = ControlModeSummaryPlot( status_time, control_mode, [['airborne'], ['estimating_wind']], x_label='time (sec)', y_labels=['airborne', 'estimating wind'], annotation_text=[[], []], additional_annotation=[airborne_annotations, []], plot_title='EKF Control Status - Figure B', pdf_handle=pdf_pages) data_plot.save() data_plot.close() # plot innovation_check_flags summary data_plot = CheckFlagsPlot( status_time, innov_flags, [['vel_innov_fail', 'posh_innov_fail'], ['posv_innov_fail', 'hagl_innov_fail'], ['magx_innov_fail', 'magy_innov_fail', 'magz_innov_fail', 'yaw_innov_fail'], ['tas_innov_fail'], ['sli_innov_fail'], ['ofx_innov_fail', 'ofy_innov_fail']], x_label='time (sec)', y_labels=['failed', 'failed', 'failed', 'failed', 'failed', 'failed'], y_lim=(-0.1, 1.1), legend=[['vel NED', 'pos NE'], ['hgt absolute', 'hgt above ground'], ['mag_x', 'mag_y', 'mag_z', 'yaw'], ['airspeed'], ['sideslip'], ['flow X', 'flow Y']], plot_title='EKF Innovation Test Fails', annotate=False, pdf_handle=pdf_pages) data_plot.save() data_plot.close() # gps_check_fail_flags summary data_plot = CheckFlagsPlot( status_time, gps_fail_flags, [['nsat_fail', 'gdop_fail', 'herr_fail', 'verr_fail', 'gfix_fail', 'serr_fail'], ['hdrift_fail', 'vdrift_fail', 'hspd_fail', 'veld_diff_fail']], x_label='time (sec)', y_lim=(-0.1, 1.1), y_labels=['failed', 'failed'], sub_titles=['GPS Direct Output Check Failures', 'GPS Derived Output Check Failures'], legend=[['N sats', 'GDOP', 'horiz pos error', 'vert pos error', 'fix type', 'speed error'], ['horiz drift', 'vert drift', 'horiz speed', 'vert vel inconsistent']], annotate=False, pdf_handle=pdf_pages) data_plot.save() data_plot.close() # filter reported accuracy data_plot = CheckFlagsPlot( status_time, estimator_status, [['pos_horiz_accuracy', 'pos_vert_accuracy']], x_label='time (sec)', y_labels=['accuracy (m)'], plot_title='Reported Accuracy', legend=[['horizontal', 'vertical']], annotate=False, pdf_handle=pdf_pages) data_plot.save() data_plot.close() # Plot the EKF IMU vibration metrics scaled_estimator_status = {'vibe[0]': 1000. * estimator_status['vibe[0]'], 'vibe[1]': 1000. * estimator_status['vibe[1]'], 'vibe[2]': estimator_status['vibe[2]'] } data_plot = CheckFlagsPlot( status_time, scaled_estimator_status, [['vibe[0]'], ['vibe[1]'], ['vibe[2]']], x_label='time (sec)', y_labels=['Del Ang Coning (mrad)', 'HF Del Ang (mrad)', 'HF Del Vel (m/s)'], plot_title='IMU Vibration Metrics', pdf_handle=pdf_pages, annotate=True) data_plot.save() data_plot.close() # Plot the EKF output observer tracking errors scaled_innovations = { 'output_tracking_error[0]': 1000. * ekf2_innovations['output_tracking_error[0]'], 'output_tracking_error[1]': ekf2_innovations['output_tracking_error[1]'], 'output_tracking_error[2]': ekf2_innovations['output_tracking_error[2]'] } data_plot = CheckFlagsPlot( 1e-6 * ekf2_innovations['timestamp'], scaled_innovations, [['output_tracking_error[0]'], ['output_tracking_error[1]'], ['output_tracking_error[2]']], x_label='time (sec)', y_labels=['angles (mrad)', 'velocity (m/s)', 'position (m)'], plot_title='Output Observer Tracking Error Magnitudes', pdf_handle=pdf_pages, annotate=True) data_plot.save() data_plot.close() # Plot the delta angle bias estimates data_plot = CheckFlagsPlot( 1e-6 * estimator_status['timestamp'], estimator_status, [['states[10]'], ['states[11]'], ['states[12]']], x_label='time (sec)', y_labels=['X (rad)', 'Y (rad)', 'Z (rad)'], plot_title='Delta Angle Bias Estimates', annotate=False, pdf_handle=pdf_pages) data_plot.save() data_plot.close() # Plot the delta velocity bias estimates data_plot = CheckFlagsPlot( 1e-6 * estimator_status['timestamp'], estimator_status, [['states[13]'], ['states[14]'], ['states[15]']], x_label='time (sec)', y_labels=['X (m/s)', 'Y (m/s)', 'Z (m/s)'], plot_title='Delta Velocity Bias Estimates', annotate=False, pdf_handle=pdf_pages) data_plot.save() data_plot.close() # Plot the earth frame magnetic field estimates declination, field_strength, inclination = magnetic_field_estimates_from_status( estimator_status) data_plot = CheckFlagsPlot( 1e-6 * estimator_status['timestamp'], {'strength': field_strength, 'declination': declination, 'inclination': inclination}, [['declination'], ['inclination'], ['strength']], x_label='time (sec)', y_labels=['declination (deg)', 'inclination (deg)', 'strength (Gauss)'], plot_title='Earth Magnetic Field Estimates', annotate=False, pdf_handle=pdf_pages) data_plot.save() data_plot.close() # Plot the body frame magnetic field estimates data_plot = CheckFlagsPlot( 1e-6 * estimator_status['timestamp'], estimator_status, [['states[19]'], ['states[20]'], ['states[21]']], x_label='time (sec)', y_labels=['X (Gauss)', 'Y (Gauss)', 'Z (Gauss)'], plot_title='Magnetometer Bias Estimates', annotate=False, pdf_handle=pdf_pages) data_plot.save() data_plot.close() # Plot the EKF wind estimates data_plot = CheckFlagsPlot( 1e-6 * estimator_status['timestamp'], estimator_status, [['states[22]'], ['states[23]']], x_label='time (sec)', y_labels=['North (m/s)', 'East (m/s)'], plot_title='Wind Velocity Estimates', annotate=False, pdf_handle=pdf_pages) data_plot.save() data_plot.close()