def get_innovation_message(ulog: ULog, topic: str = 'innovation') -> str: """ return the name of the innovation message (old: ekf2_innovations; new: estimator_innovations) :param ulog: :return: str """ ulog_messages = {elem.name for elem in ulog.data_list} message = '' if topic == 'innovation': if 'estimator_innovations' in ulog_messages: message = 'estimator_innovations' elif 'ekf2_innovations' in ulog_messages: message = 'ekf2_innovations' else: raise PreconditionError('innovation message not found.') elif topic == 'innovation_variance': if 'estimator_innovation_variances' in ulog_messages: message = 'estimator_innovation_variances' elif 'ekf2_innovations' in ulog_messages: message = 'ekf2_innovations' else: raise PreconditionError('innovation variances message not found.') elif topic == 'innovation_test_ratio': if 'estimator_innovation_test_ratios' in ulog_messages: message = 'estimator_innovation_test_ratios' elif 'estimator_status' in ulog_messages: message = 'estimator_status' else: raise PreconditionError( 'estimator innovation test ratios message not found.') else: raise NotImplementedError('topic {:s} not supported'.format(topic)) return message
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 get_position_intervals(self, dataset: str, multi_instance: int = 0) -> List[list]: """ return all intervals of the log file that are valid in position :param dataset: :param multi_instance: :return: """ try: data = self._ulog.get_dataset(dataset, multi_instance=multi_instance).data except Exception as e: raise PreconditionError( 'PositionAnalyzer: {:s} not found in log.'.format( dataset)) from e position_intervals = [] for interval in self._position_intervals: position_intervals.append( np.where(((data['timestamp'] - self._ulog.start_timestamp) / 1.0e6 >= interval.lower) & ((data['timestamp'] - self._ulog.start_timestamp) / 1.0e6 < interval.upper))[0]) return position_intervals
def get_innovation_message(ulog: ULog, topic: str = 'innovation') -> str: """ return the name of the innovation message (old: ekf2_innovations; new: estimator_innovations) :param ulog: :return: str """ if topic == 'innovation': for elem in ulog.data_list: if elem.name == "ekf2_innovations": return "ekf2_innovations" if elem.name == "estimator_innovations": return "estimator_innovations" if topic == 'innovation_variance': for elem in ulog.data_list: if elem.name == "ekf2_innovations": return "ekf2_innovations" if elem.name == "estimator_innovations": return "estimator_innovation_variances" if topic == 'innovation_test_ratio': for elem in ulog.data_list: if elem.name == "ekf2_innovations": return None if elem.name == "estimator_innovations": return "estimator_innovation_test_ratios" raise PreconditionError("Could not detect the message")
def get_airtime(self, dataset: str, multi_instance: int = 0) -> list: """ return all indices of the log file that are in air :param dataset: :return: """ try: data = self._ulog.get_dataset(dataset, multi_instance=multi_instance).data except: raise PreconditionError('InAirDetector: {:s} not found in log.'.format(dataset)) return self.get_total_airtime_for_timestamp( data['timestamp'], start_time=self._ulog.start_timestamp, conversion_factor=1.0e-6)
def get_output_tracking_error_message(ulog: ULog) -> str: """ return the name of the message containing the output_tracking_error :param ulog: :return: str """ for elem in ulog.data_list: if elem.name == "ekf2_innovations": return "ekf2_innovations" if elem.name == "estimator_innovations": return "estimator_status" raise PreconditionError( "Could not detect the message containing the output tracking error")
def get_output_tracking_error_message(ulog: ULog) -> str: """ return the name of the message containing the output_tracking_error :param ulog: :return: str """ output_tracking_error_message = '' ulog_messages = {elem.name for elem in ulog.data_list} if 'estimator_innovations' in ulog_messages: output_tracking_error_message = 'estimator_status' elif 'ekf2_innovations' in ulog_messages: output_tracking_error_message = 'ekf2_innovations' else: raise PreconditionError( "Output tracking error message not found in log.") return output_tracking_error_message
def _above_min_ground_distance_intervals( self, ground_distance_meters: float, phase_change_margin_seconds: float = 0.0, min_interval_duration_seconds: float = 0.0) -> intervals.Interval: """ :param ground_distance_meters: :return: """ intervals_above_min_ground_distance = intervals.empty() if 'dist_bottom' not in self._vehicle_local_position: raise PreconditionError( 'Could not find dist_bottom in vehicle_local_position data.') timestamp = self._vehicle_local_position['timestamp'] dist_bottom = self._vehicle_local_position['dist_bottom'] if 'dist_bottom_valid' in self._vehicle_local_position: timestamp = timestamp[np.where( self._vehicle_local_position['dist_bottom_valid'])] dist_bottom = dist_bottom[np.where( self._vehicle_local_position['dist_bottom_valid'])] above_min_ground_distance = dist_bottom > ground_distance_meters interval_starts, interval_ends = detect_flag_value_changes( above_min_ground_distance.astype(int)) for interval_start, interval_end in zip(interval_starts, interval_ends): if (timestamp[interval_end] / 1.0e6 - phase_change_margin_seconds) - \ (timestamp[interval_start] / 1.0e6 + phase_change_margin_seconds) >= min_interval_duration_seconds: intervals_above_min_ground_distance = intervals_above_min_ground_distance | \ intervals.closed( (timestamp[interval_start] - self._ulog.start_timestamp) / \ 1.0e6 + phase_change_margin_seconds, (timestamp[interval_end] - self._ulog.start_timestamp) / \ 1.0e6 - phase_change_margin_seconds) if intervals_above_min_ground_distance.is_empty(): print('PositionAnalyzer: flag was never activated.') return intervals_above_min_ground_distance
def __init__(self, ulog: ULog) -> None: """ initializes a PositionAnalyzer instance. :param ulog: """ self._ulog = ulog self._position_intervals = intervals.empty() self._vehicle_local_position = None try: self._vehicle_local_position = self._ulog.get_dataset( 'vehicle_local_position').data except Exception as e: raise PreconditionError( 'PositionAnalyzer: Could not find vehicle local position message.' ) from e self._position_intervals = intervals.closed( 0.0, (self._ulog.last_timestamp - self._ulog.start_timestamp) / 1.0e6)
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: """ create a pdf analysis report. :param ulog: :param output_plot_filename: :return: """ # create summary plots # save the plots to PDF messages = {elem.name for elem in ulog.data_list} if 'estimator_innovations' in messages: raise NotImplementedError( 'pdf report not implemented for new log file format') if 'estimator_status' not in messages: raise PreconditionError('could not find estimator_status data') if 'ekf2_innovations' not in messages: raise PreconditionError('could not find ekf2_innovation data') if 'sensor_preflight' not in messages: raise PreconditionError('could not find sensor_preflight data') estimator_status = ulog.get_dataset('estimator_status').data ekf2_innovations = ulog.get_dataset('ekf2_innovations').data sensor_preflight = ulog.get_dataset('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 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 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()