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

estimator_sensor_bias: add bias variance

This commit is contained in:
Daniel Agar 2020-09-01 14:47:31 -04:00
parent c54a0ff0c7
commit e5879f1bb6
2 changed files with 13 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/estimator_sensor_bias.msg
View File

@ -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 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 # 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 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 # 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 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 # magnetometer in-run bias in body frame (Gauss)
float32[3] mag_bias_variance

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

@ -1058,7 +1058,6 @@ void EKF2::Run()
bias.accel_device_id = _estimator_status_pub.get().accel_device_id; bias.accel_device_id = _estimator_status_pub.get().accel_device_id;
bias.mag_device_id = _estimator_status_pub.get().mag_device_id; bias.mag_device_id = _estimator_status_pub.get().mag_device_id;
// In-run bias estimates
_ekf.getGyroBias().copyTo(bias.gyro_bias); _ekf.getGyroBias().copyTo(bias.gyro_bias);
_ekf.getAccelBias().copyTo(bias.accel_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[1] = _last_valid_mag_cal[1];
bias.mag_bias[2] = _last_valid_mag_cal[2]; 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(); bias.timestamp = _replay_mode ? now : hrt_absolute_time();
_estimator_sensor_bias_pub.publish(bias); _estimator_sensor_bias_pub.publish(bias);
} }