From e5879f1bb646c65d6ab75bf6d40c1847318a7454 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 1 Sep 2020 14:47:31 -0400 Subject: [PATCH] estimator_sensor_bias: add bias variance --- msg/estimator_sensor_bias.msg | 3 +++ src/modules/ekf2/EKF2.cpp | 11 ++++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/msg/estimator_sensor_bias.msg b/msg/estimator_sensor_bias.msg index e0884b158c..0ede805560 100644 --- a/msg/estimator_sensor_bias.msg +++ b/msg/estimator_sensor_bias.msg @@ -10,9 +10,12 @@ uint64 timestamp_sample # the timestamp of the raw data (microseconds) uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s) +float32[3] gyro_bias_variance uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2) +float32[3] accel_bias_variance uint32 mag_device_id # unique device ID for the sensor that does not change between power cycles float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss) +float32[3] mag_bias_variance diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 1c2a4b4e79..ab99f37108 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1058,7 +1058,6 @@ void EKF2::Run() bias.accel_device_id = _estimator_status_pub.get().accel_device_id; bias.mag_device_id = _estimator_status_pub.get().mag_device_id; - // In-run bias estimates _ekf.getGyroBias().copyTo(bias.gyro_bias); _ekf.getAccelBias().copyTo(bias.accel_bias); @@ -1066,6 +1065,16 @@ void EKF2::Run() bias.mag_bias[1] = _last_valid_mag_cal[1]; bias.mag_bias[2] = _last_valid_mag_cal[2]; + bias.gyro_bias_variance[0] = states.covariances[10]; + bias.gyro_bias_variance[1] = states.covariances[11]; + bias.gyro_bias_variance[2] = states.covariances[12]; + bias.accel_bias_variance[0] = states.covariances[13]; + bias.accel_bias_variance[1] = states.covariances[14]; + bias.accel_bias_variance[2] = states.covariances[15]; + bias.mag_bias_variance[0] = states.covariances[19]; + bias.mag_bias_variance[1] = states.covariances[20]; + bias.mag_bias_variance[2] = states.covariances[21]; + bias.timestamp = _replay_mode ? now : hrt_absolute_time(); _estimator_sensor_bias_pub.publish(bias); }