diff --git a/Tools/ecl_ekf/analyse_logdata_ekf.py b/Tools/ecl_ekf/analyse_logdata_ekf.py index 12ad2755b3..2693e246e8 100644 --- a/Tools/ecl_ekf/analyse_logdata_ekf.py +++ b/Tools/ecl_ekf/analyse_logdata_ekf.py @@ -29,6 +29,12 @@ def analyse_ekf( :return: """ + try: + estimator_states = ulog.get_dataset('estimator_states').data + print('found estimator_states data') + except: + raise PreconditionError('could not find estimator_states data') + try: estimator_status = ulog.get_dataset('estimator_status').data print('found estimator_status data') diff --git a/Tools/ecl_ekf/analysis/data_version_handler.py b/Tools/ecl_ekf/analysis/data_version_handler.py index c2392a6bcd..00164b14c0 100644 --- a/Tools/ecl_ekf/analysis/data_version_handler.py +++ b/Tools/ecl_ekf/analysis/data_version_handler.py @@ -12,7 +12,7 @@ def get_output_tracking_error_message(ulog: ULog) -> str: :param ulog: :return: str """ - for elem in ulog.data_list: + for elem in ulog.data_list: if elem.name == "ekf2_innovations": return "ekf2_innovations" if elem.name == "estimator_innovations": diff --git a/Tools/ecl_ekf/analysis/metrics.py b/Tools/ecl_ekf/analysis/metrics.py index 6ebe8c6211..597593ea32 100644 --- a/Tools/ecl_ekf/analysis/metrics.py +++ b/Tools/ecl_ekf/analysis/metrics.py @@ -158,11 +158,13 @@ def calculate_imu_metrics( in_air_no_ground_effects, np.mean) # IMU bias checks + estimator_states_data = ulog.get_dataset('estimator_states').data + 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)) + 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_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)) + 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 diff --git a/Tools/ecl_ekf/analysis/post_processing.py b/Tools/ecl_ekf/analysis/post_processing.py index ccb64b9522..7aab1f8efa 100644 --- a/Tools/ecl_ekf/analysis/post_processing.py +++ b/Tools/ecl_ekf/analysis/post_processing.py @@ -146,18 +146,18 @@ def get_gps_check_fail_flags(estimator_status: dict) -> dict: return gps_fail_flags -def magnetic_field_estimates_from_status(estimator_status: dict) -> Tuple[float, float, float]: +def magnetic_field_estimates_from_status(estimator_states: dict) -> Tuple[float, float, float]: """ - :param estimator_status: + :param estimator_states: :return: """ rad2deg = 57.2958 field_strength = np.sqrt( - estimator_status['states[16]'] ** 2 + estimator_status['states[17]'] ** 2 + - estimator_status['states[18]'] ** 2) - declination = rad2deg * np.arctan2(estimator_status['states[17]'], - estimator_status['states[16]']) + estimator_states['states[16]'] ** 2 + estimator_states['states[17]'] ** 2 + + estimator_states['states[18]'] ** 2) + declination = rad2deg * np.arctan2(estimator_states['states[17]'], + estimator_states['states[16]']) inclination = rad2deg * np.arcsin( - estimator_status['states[18]'] / np.maximum(field_strength, np.finfo(np.float32).eps)) + estimator_states['states[18]'] / np.maximum(field_strength, np.finfo(np.float32).eps)) return declination, field_strength, inclination diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index 797bd4c5c6..153c4cc0cf 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -275,7 +275,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # Plot the delta angle bias estimates data_plot = CheckFlagsPlot( - 1e-6 * estimator_status['timestamp'], estimator_status, + 1e-6 * estimator_states['timestamp'], estimator_states, [['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) @@ -284,7 +284,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # Plot the delta velocity bias estimates data_plot = CheckFlagsPlot( - 1e-6 * estimator_status['timestamp'], estimator_status, + 1e-6 * estimator_states['timestamp'], estimator_states, [['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) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3451651961..69090e8b66 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -62,6 +62,7 @@ set(msg_files esc_status.msg estimator_innovations.msg estimator_sensor_bias.msg + estimator_states.msg estimator_status.msg follow_target.msg geofence_result.msg diff --git a/msg/estimator_states.msg b/msg/estimator_states.msg new file mode 100644 index 0000000000..d66fd39c4c --- /dev/null +++ b/msg/estimator_states.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +float32[24] states # Internal filter states +uint8 n_states # Number of states effectively used + +float32[24] covariances # Diagonal Elements of Covariance Matrix diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 391595c7a4..f8897da16e 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -1,14 +1,10 @@ uint64 timestamp # time since system start (microseconds) -float32[24] states # Internal filter states -uint8 n_states # Number of states effectively used float32[3] vibe # IMU vibration metrics in the following array locations # 0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) # 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) # 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) -float32[24] covariances # Diagonal Elements of Covariance Matrix - float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m) uint16 gps_check_fail_flags # Bitmask to indicate status of GPS checks - see definition below diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 904420cb59..3375d97b97 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -281,6 +281,8 @@ rtps: id: 133 - msg: sensor_preflight_mag id: 134 + - msg: estimator_states + id: 135 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 8942b7bcda..73f4e22a6d 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -232,6 +232,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail, checkGNSS)) { failed = true; } + + if (!ekf2CheckStates(mavlink_log_pub, reportFailures && report_ekf_fail)) { + failed = true; + } } /* ---- Failure Detector ---- */ diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index 1e17228886..110111bf49 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -110,6 +110,9 @@ private: const bool prearm); static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional, const bool report_fail, const bool enforce_gps_required); + + static bool ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool report_fail); + static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, const bool prearm); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index 51522d523e..079063e3e7 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -145,40 +146,6 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & goto out; } - // check accelerometer delta velocity bias estimates - param_get(param_find("COM_ARM_EKF_AB"), &test_limit); - - for (uint8_t index = 13; index < 16; index++) { - // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives - // adjust test threshold by 3-sigma - float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f)); - - if (fabsf(status.states[index]) > test_limit + test_uncertainty) { - - if (report_fail) { - PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)status.states[index], (double)test_limit, - (double)test_uncertainty); - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias"); - } - - success = false; - goto out; - } - } - - // check gyro delta angle bias estimates - param_get(param_find("COM_ARM_EKF_GB"), &test_limit); - - if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit - || fabsf(status.states[12]) > test_limit) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias"); - } - - success = false; - goto out; - } - // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing if (enforce_gps_required || report_fail) { const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS); @@ -259,3 +226,50 @@ out: return success; } + +bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool report_fail) +{ + // Get estimator states data if available and exit with a fail recorded if not + uORB::Subscription states_sub{ORB_ID(estimator_states)}; + estimator_states_s states; + + if (states_sub.copy(&states)) { + + // check accelerometer delta velocity bias estimates + float test_limit = 1.0f; // pass limit re-used for each test + param_get(param_find("COM_ARM_EKF_AB"), &test_limit); + + for (uint8_t index = 13; index < 16; index++) { + // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives + // adjust test threshold by 3-sigma + float test_uncertainty = 3.0f * sqrtf(fmaxf(states.covariances[index], 0.0f)); + + if (fabsf(states.states[index]) > test_limit + test_uncertainty) { + + if (report_fail) { + PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)states.states[index], (double)test_limit, + (double)test_uncertainty); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias"); + } + + return false; + } + } + + // check gyro delta angle bias estimates + param_get(param_find("COM_ARM_EKF_GB"), &test_limit); + + if (fabsf(states.states[10]) > test_limit + || fabsf(states.states[11]) > test_limit + || fabsf(states.states[12]) > test_limit) { + + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias"); + } + + return false; + } + } + + return true; +} diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c3771e62c9..afb922fecf 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -989,6 +989,41 @@ void EKF2::Run() } } + // publish estimator states + estimator_states_s states; + states.n_states = 24; + _ekf.getStateAtFusionHorizonAsVector().copyTo(states.states); + _ekf.covariances_diagonal().copyTo(states.covariances); + states.timestamp = _replay_mode ? now : hrt_absolute_time(); + _estimator_states_pub.publish(states); + + // publish estimator status + estimator_status_s status{}; + _ekf.getOutputTrackingError().copyTo(status.output_tracking_error); + _ekf.get_gps_check_status(&status.gps_check_fail_flags); + // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include + // the GPS Fix bit, which is always checked) + status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1; + status.control_mode_flags = control_status.value; + _ekf.get_filter_fault_status(&status.filter_fault_flags); + _ekf.get_innovation_test_status(status.innovation_check_flags, status.mag_test_ratio, + status.vel_test_ratio, status.pos_test_ratio, + status.hgt_test_ratio, status.tas_test_ratio, + status.hagl_test_ratio, status.beta_test_ratio); + + status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph; + status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv; + _ekf.get_ekf_soln_status(&status.solution_status_flags); + _ekf.getImuVibrationMetrics().copyTo(status.vibe); + status.time_slip = _last_time_slip_us * 1e-6f; + status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed(); + status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed(); + status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed(); + status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed(); + status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed; + + _estimator_status_pub.publish(status); + { // publish all corrected sensor readings and bias estimates after mag calibration is updated above bias.timestamp = now; @@ -1012,39 +1047,6 @@ void EKF2::Run() _estimator_sensor_bias_pub.publish(bias); } - // publish estimator status - estimator_status_s status; - status.timestamp = now; - _ekf.getStateAtFusionHorizonAsVector().copyTo(status.states); - status.n_states = 24; - _ekf.covariances_diagonal().copyTo(status.covariances); - _ekf.getOutputTrackingError().copyTo(status.output_tracking_error); - _ekf.get_gps_check_status(&status.gps_check_fail_flags); - // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include - // the GPS Fix bit, which is always checked) - status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1; - status.control_mode_flags = control_status.value; - _ekf.get_filter_fault_status(&status.filter_fault_flags); - _ekf.get_innovation_test_status(status.innovation_check_flags, status.mag_test_ratio, - status.vel_test_ratio, status.pos_test_ratio, - status.hgt_test_ratio, status.tas_test_ratio, - status.hagl_test_ratio, status.beta_test_ratio); - - status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph; - status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv; - _ekf.get_ekf_soln_status(&status.solution_status_flags); - _ekf.getImuVibrationMetrics().copyTo(status.vibe); - status.time_slip = _last_time_slip_us * 1e-6f; - status.health_flags = 0.0f; // unused - status.timeout_flags = 0.0f; // unused - status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed(); - status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed(); - status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed(); - status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed(); - status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed; - - _estimator_status_pub.publish(status); - // publish GPS drift data only when updated to minimise overhead float gps_drift[3]; bool blocked; @@ -1103,8 +1105,8 @@ void EKF2::Run() bool all_estimates_invalid = false; for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) { - if (status.covariances[axis_index + 19] < min_var_allowed - || status.covariances[axis_index + 19] > max_var_allowed) { + if (states.covariances[axis_index + 19] < min_var_allowed + || states.covariances[axis_index + 19] > max_var_allowed) { all_estimates_invalid = true; } } @@ -1112,9 +1114,9 @@ void EKF2::Run() // Store valid estimates and their associated variances if (!all_estimates_invalid) { for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) { - _last_valid_mag_cal[axis_index] = status.states[axis_index + 19]; + _last_valid_mag_cal[axis_index] = states.states[axis_index + 19]; _valid_cal_available[axis_index] = true; - _last_valid_variance[axis_index] = status.covariances[axis_index + 19]; + _last_valid_variance[axis_index] = states.covariances[axis_index + 19]; } } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index c8fb1e1d33..0a33d05e91 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -271,6 +272,7 @@ private: uORB::Publication _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::Publication _estimator_innovations_pub{ORB_ID(estimator_innovations)}; uORB::Publication _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; + uORB::Publication _estimator_states_pub{ORB_ID(estimator_states)}; uORB::Publication _estimator_status_pub{ORB_ID(estimator_status)}; uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index d8161ae555..a5a43fa4e6 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -712,45 +712,46 @@ void BlockLocalPositionEstimator::publishOdom() void BlockLocalPositionEstimator::publishEstimatorStatus() { + _pub_est_states.get().timestamp = _timeStamp; _pub_est_status.get().timestamp = _timeStamp; for (size_t i = 0; i < n_x; i++) { - _pub_est_status.get().states[i] = _x(i); + _pub_est_states.get().states[i] = _x(i); } // matching EKF2 covariances indexing // quaternion - not determined, as it is a position estimator - _pub_est_status.get().covariances[0] = NAN; - _pub_est_status.get().covariances[1] = NAN; - _pub_est_status.get().covariances[2] = NAN; - _pub_est_status.get().covariances[3] = NAN; + _pub_est_states.get().covariances[0] = NAN; + _pub_est_states.get().covariances[1] = NAN; + _pub_est_states.get().covariances[2] = NAN; + _pub_est_states.get().covariances[3] = NAN; // linear velocity - _pub_est_status.get().covariances[4] = m_P(X_vx, X_vx); - _pub_est_status.get().covariances[5] = m_P(X_vy, X_vy); - _pub_est_status.get().covariances[6] = m_P(X_vz, X_vz); + _pub_est_states.get().covariances[4] = m_P(X_vx, X_vx); + _pub_est_states.get().covariances[5] = m_P(X_vy, X_vy); + _pub_est_states.get().covariances[6] = m_P(X_vz, X_vz); // position - _pub_est_status.get().covariances[7] = m_P(X_x, X_x); - _pub_est_status.get().covariances[8] = m_P(X_y, X_y); - _pub_est_status.get().covariances[9] = m_P(X_z, X_z); + _pub_est_states.get().covariances[7] = m_P(X_x, X_x); + _pub_est_states.get().covariances[8] = m_P(X_y, X_y); + _pub_est_states.get().covariances[9] = m_P(X_z, X_z); // gyro bias - not determined - _pub_est_status.get().covariances[10] = NAN; - _pub_est_status.get().covariances[11] = NAN; - _pub_est_status.get().covariances[12] = NAN; + _pub_est_states.get().covariances[10] = NAN; + _pub_est_states.get().covariances[11] = NAN; + _pub_est_states.get().covariances[12] = NAN; // accel bias - _pub_est_status.get().covariances[13] = m_P(X_bx, X_bx); - _pub_est_status.get().covariances[14] = m_P(X_by, X_by); - _pub_est_status.get().covariances[15] = m_P(X_bz, X_bz); + _pub_est_states.get().covariances[13] = m_P(X_bx, X_bx); + _pub_est_states.get().covariances[14] = m_P(X_by, X_by); + _pub_est_states.get().covariances[15] = m_P(X_bz, X_bz); // mag - not determined for (size_t i = 16; i <= 21; i++) { - _pub_est_status.get().covariances[i] = NAN; + _pub_est_states.get().covariances[i] = NAN; } // replacing the hor wind cov with terrain altitude covariance - _pub_est_status.get().covariances[22] = m_P(X_tz, X_tz); - _pub_est_status.get().covariances[23] = NAN; + _pub_est_states.get().covariances[22] = m_P(X_tz, X_tz); + _pub_est_states.get().covariances[23] = NAN; - _pub_est_status.get().n_states = n_x; + _pub_est_states.get().n_states = n_x; _pub_est_status.get().health_flags = _sensorFault; _pub_est_status.get().timeout_flags = _sensorTimeout; _pub_est_status.get().pos_horiz_accuracy = _pub_gpos.get().eph; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 5c4f6a07ad..312df7420e 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -285,6 +286,7 @@ private: uORB::PublicationData _pub_lpos{ORB_ID(vehicle_local_position)}; uORB::PublicationData _pub_gpos{ORB_ID(vehicle_global_position)}; uORB::PublicationData _pub_odom{ORB_ID(vehicle_odometry)}; + uORB::PublicationData _pub_est_states{ORB_ID(estimator_states)}; uORB::PublicationData _pub_est_status{ORB_ID(estimator_status)}; uORB::PublicationData _pub_innov{ORB_ID(estimator_innovations)}; uORB::PublicationData _pub_innov_var{ORB_ID(estimator_innovation_variances)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index a08b8bd627..cee7449eaa 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -61,6 +61,7 @@ void LoggedTopics::add_default_topics() add_topic("estimator_innovation_variances", 200); add_topic("estimator_innovations", 200); add_topic("estimator_sensor_bias", 1000); + add_topic("estimator_states", 1000); add_topic("estimator_status", 200); add_topic("home_position"); add_topic("hover_thrust_estimate", 100);