dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

estimator_status split out estimator_states

This commit is contained in:
Daniel Agar 2020-09-01 14:25:58 -04:00
parent 33a7fed240
commit 9ccc1db649
17 changed files with 150 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Tools/ecl_ekf/analyse_logdata_ekf.py
View File

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

2
Tools/ecl_ekf/analysis/data_version_handler.py
View File

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

6
Tools/ecl_ekf/analysis/metrics.py
View File

@ -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

14
Tools/ecl_ekf/analysis/post_processing.py
View File

@ -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

4
Tools/ecl_ekf/plotting/pdf_report.py
View File

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

1
msg/CMakeLists.txt
View File

@ -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

6
msg/estimator_states.msg Normal file
View File

@ -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

4
msg/estimator_status.msg
View File

@ -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

2
msg/tools/uorb_rtps_message_ids.yaml
View File

@ -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

4
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View File

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

3
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
View File

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

82
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
View File

@ -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;
}

76
src/modules/ekf2/EKF2.cpp
View File

@ -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];
}
}
}

2
src/modules/ekf2/EKF2.hpp
View File

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

43
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

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

2
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

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

1
src/modules/logger/logged_topics.cpp
View File

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