Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-26 01:17:51 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

estimator_sensor_bias: add bias variance

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+10 -1
View File
@@ -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);
}