estimator_sensor_bias: add bias variance

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

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