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

mbe: integrate pre-takeoff mag bias estimate in VehicleMagnetometer

This commit is contained in:
bresch 2021-10-01 14:19:10 +02:00 committed by Daniel Agar
parent 1443f773da
commit e0bcc17f5b
2 changed files with 78 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -174,7 +174,7 @@ void VehicleMagnetometer::MagCalibrationUpdate()
_mag_cal[i].mag_offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias);
_mag_cal[i].mag_bias_variance = bias_variance;
_mag_cal_available = true;
_in_flight_mag_cal_available = true;
if ((old_offset - _mag_cal[i].mag_offset).longerThan(0.01f)) {
PX4_DEBUG("Mag %d (%d) est. offset saved: [% 05.3f % 05.3f % 05.3f] (bias [% 05.3f % 05.3f % 05.3f])",
@ -190,47 +190,62 @@ void VehicleMagnetometer::MagCalibrationUpdate()
}
}
} else if (_mag_cal_available) {
if (_in_flight_mag_cal_available || _on_ground_mag_bias_estimate_available) {
_should_save_on_disarm = true;
}
} else if (_should_save_on_disarm) {
// not armed and mag cal available
bool calibration_param_save_needed = false;
// iterate through available bias estimates and fuse them sequentially using a Kalman Filter scheme
Vector3f state_variance{magb_vref, magb_vref, magb_vref};
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
// first, apply the calibration from the mag bias estimator (not EKF2)
if (_on_ground_mag_bias_estimate_available) {
const Vector3f new_offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
if (_calibration[mag_index].set_offset(new_offset)) {
calibration_param_save_needed = true;
_calibration_estimator_bias[mag_index].zero();
}
}
// apply all valid saved offsets
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if ((_calibration[mag_index].device_id() != 0) && (_mag_cal[i].device_id == _calibration[mag_index].device_id())) {
if (_in_flight_mag_cal_available) {
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if ((_calibration[mag_index].device_id() != 0) && (_mag_cal[i].device_id == _calibration[mag_index].device_id())) {
const Vector3f mag_cal_orig{_calibration[mag_index].offset()};
Vector3f mag_cal_offset{_calibration[mag_index].offset()};
const Vector3f mag_cal_orig{_calibration[mag_index].offset()};
Vector3f mag_cal_offset{_calibration[mag_index].offset()};
// calculate weighting using ratio of variances and update stored bias values
const Vector3f &observation = _mag_cal[i].mag_offset;
const Vector3f &obs_variance = _mag_cal[i].mag_bias_variance;
// calculate weighting using ratio of variances and update stored bias values
const Vector3f &observation = _mag_cal[i].mag_offset;
const Vector3f &obs_variance = _mag_cal[i].mag_bias_variance;
for (int axis_index = 0; axis_index < 3; axis_index++) {
const float innovation_variance = state_variance(axis_index) + obs_variance(axis_index);
const float innovation = mag_cal_offset(axis_index) - observation(axis_index);
const float kalman_gain = state_variance(axis_index) / innovation_variance;
mag_cal_offset(axis_index) -= innovation * kalman_gain;
state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain), 0.f);
}
for (int axis_index = 0; axis_index < 3; axis_index++) {
const float innovation_variance = state_variance(axis_index) + obs_variance(axis_index);
const float innovation = mag_cal_offset(axis_index) - observation(axis_index);
const float kalman_gain = state_variance(axis_index) / innovation_variance;
mag_cal_offset(axis_index) -= innovation * kalman_gain;
state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain), 0.f);
if (_calibration[mag_index].set_offset(mag_cal_offset)) {
PX4_INFO("%d (%" PRIu32 ") EST:%d offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])",
mag_index, _calibration[mag_index].device_id(), i,
(double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2),
(double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
(double)_mag_cal[i].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2));
calibration_param_save_needed = true;
}
// clear
_mag_cal[i].device_id = 0;
_mag_cal[i].mag_offset.zero();
_mag_cal[i].mag_bias_variance.zero();
}
if (_calibration[mag_index].set_offset(mag_cal_offset)) {
PX4_INFO("%d (%" PRIu32 ") EST:%d offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])",
mag_index, _calibration[mag_index].device_id(), i,
(double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2),
(double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
(double)_mag_cal[i].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2));
calibration_param_save_needed = true;
}
// clear
_mag_cal[i].device_id = 0;
_mag_cal[i].mag_offset.zero();
_mag_cal[i].mag_bias_variance.zero();
}
}
}
@ -242,7 +257,29 @@ void VehicleMagnetometer::MagCalibrationUpdate()
}
}
_mag_cal_available = false;
param_notify_changes();
}
_should_save_on_disarm = false;
_on_ground_mag_bias_estimate_available = false;
_in_flight_mag_cal_available = false;
} else {
// Continuous mag calibration is running when not armed
if (_magnetometer_bias_estimate_sub.updated()) {
magnetometer_bias_estimate_s mag_bias_est;
if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) {
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
if (mag_bias_est.valid[mag_index]) {
const Vector3f bias{mag_bias_est.bias_x[mag_index],
mag_bias_est.bias_y[mag_index],
mag_bias_est.bias_z[mag_index]};
_calibration_estimator_bias[mag_index] = bias;
_on_ground_mag_bias_estimate_available = true;
}
}
}
}
}
}
@ -322,6 +359,7 @@ void VehicleMagnetometer::Run()
}
if (_calibration[uorb_index].enabled()) {
if (!was_advertised) {
if (uorb_index > 0) {
/* the first always exists, but for each further sensor, add a new validator */
@ -342,7 +380,7 @@ void VehicleMagnetometer::Run()
}
}
const Vector3f vect{_calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z})};
const Vector3f vect{_calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z}) - _calibration_estimator_bias[uorb_index]};
float mag_array[3] {vect(0), vect(1), vect(2)};
_voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]);

8
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
View File

@ -53,6 +53,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/magnetometer_bias_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_preflight_mag.h>
@ -103,12 +104,15 @@ private:
uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0};
uORB::Subscription _magnetometer_bias_estimate_sub{ORB_ID(magnetometer_bias_estimate)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
// Used to check, save and use learned magnetometer biases
uORB::SubscriptionMultiArray<estimator_sensor_bias_s> _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias};
bool _mag_cal_available{false};
bool _in_flight_mag_cal_available{false}; ///< from navigation filter
bool _on_ground_mag_bias_estimate_available{false}; ///< from pre-takeoff mag_bias_estimator
bool _should_save_on_disarm{false};
struct MagCal {
uint32_t device_id{0};
@ -123,6 +127,8 @@ private:
{this, ORB_ID(sensor_mag), 3}
};
matrix::Vector3f _calibration_estimator_bias[MAX_SENSOR_COUNT] {};
calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
// Magnetometer interference compensation