estimator_status split out estimator_states

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')

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":

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

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

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)

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

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

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

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

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 ---- */

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);
 

src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@@ -38,6 +38,7 @@
 #include <lib/parameters/param.h>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/subsystem_info.h>
 
@@ -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;
+}

src/modules/ekf2/EKF2.cpp

@@ -989,35 +989,16 @@ void EKF2::Run()
 				}
 			}
 
-			{
-				// publish all corrected sensor readings and bias estimates after mag calibration is updated above
-				bias.timestamp = now;
-
-				// take device ids from sensor_selection_s if not using specific vehicle_imu_s
-				if (_imu_sub_index < 0) {
-					bias.gyro_device_id = _sensor_selection.gyro_device_id;
-					bias.accel_device_id = _sensor_selection.accel_device_id;
-				}
-
-				bias.mag_device_id = _param_ekf2_magbias_id.get();
-
-				// In-run bias estimates
-				_ekf.getGyroBias().copyTo(bias.gyro_bias);
-				_ekf.getAccelBias().copyTo(bias.accel_bias);
-
-				bias.mag_bias[0] = _last_valid_mag_cal[0];
-				bias.mag_bias[1] = _last_valid_mag_cal[1];
-				bias.mag_bias[2] = _last_valid_mag_cal[2];
-
-				_estimator_sensor_bias_pub.publish(bias);
-			}
+			// 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;
-			status.timestamp = now;
-			_ekf.getStateAtFusionHorizonAsVector().copyTo(status.states);
-			status.n_states = 24;
-			_ekf.covariances_diagonal().copyTo(status.covariances);
+			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
@@ -1035,8 +1016,6 @@ void EKF2::Run()
 			_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();
@@ -1045,6 +1024,29 @@ void EKF2::Run()
 
 			_estimator_status_pub.publish(status);
 
+			{
+				// publish all corrected sensor readings and bias estimates after mag calibration is updated above
+				bias.timestamp = now;
+
+				// take device ids from sensor_selection_s if not using specific vehicle_imu_s
+				if (_imu_sub_index < 0) {
+					bias.gyro_device_id = _sensor_selection.gyro_device_id;
+					bias.accel_device_id = _sensor_selection.accel_device_id;
+				}
+
+				bias.mag_device_id = _param_ekf2_magbias_id.get();
+
+				// In-run bias estimates
+				_ekf.getGyroBias().copyTo(bias.gyro_bias);
+				_ekf.getAccelBias().copyTo(bias.accel_bias);
+
+				bias.mag_bias[0] = _last_valid_mag_cal[0];
+				bias.mag_bias[1] = _last_valid_mag_cal[1];
+				bias.mag_bias[2] = _last_valid_mag_cal[2];
+
+				_estimator_sensor_bias_pub.publish(bias);
+			}
+
 			// 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];
 						}
 					}
 				}

src/modules/ekf2/EKF2.hpp

@@ -63,6 +63,7 @@
 #include <uORB/topics/ekf_gps_position.h>
 #include <uORB/topics/estimator_innovations.h>
 #include <uORB/topics/estimator_sensor_bias.h>
+#include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/optical_flow.h>
@@ -271,6 +272,7 @@ private:
 	uORB::Publication<estimator_innovations_s>		_estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
 	uORB::Publication<estimator_innovations_s>		_estimator_innovations_pub{ORB_ID(estimator_innovations)};
 	uORB::Publication<estimator_sensor_bias_s>		_estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
+	uORB::Publication<estimator_states_s>			_estimator_states_pub{ORB_ID(estimator_states)};
 	uORB::Publication<estimator_status_s>			_estimator_status_pub{ORB_ID(estimator_status)};
 	uORB::Publication<vehicle_attitude_s>			_att_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_odometry_s>			_vehicle_odometry_pub{ORB_ID(vehicle_odometry)};

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;

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -33,6 +33,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/estimator_innovations.h>
 
@@ -285,6 +286,7 @@ private:
 	uORB::PublicationData<vehicle_local_position_s> _pub_lpos{ORB_ID(vehicle_local_position)};
 	uORB::PublicationData<vehicle_global_position_s> _pub_gpos{ORB_ID(vehicle_global_position)};
 	uORB::PublicationData<vehicle_odometry_s> _pub_odom{ORB_ID(vehicle_odometry)};
+	uORB::PublicationData<estimator_states_s> _pub_est_states{ORB_ID(estimator_states)};
 	uORB::PublicationData<estimator_status_s> _pub_est_status{ORB_ID(estimator_status)};
 	uORB::PublicationData<estimator_innovations_s> _pub_innov{ORB_ID(estimator_innovations)};
 	uORB::PublicationData<estimator_innovations_s> _pub_innov_var{ORB_ID(estimator_innovation_variances)};

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);