mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 17:53:34 +08:00
ekf2: separate mag and mag heading control logic (#21212)
- split mag_3d into new standalone mag fusion and mag fusion allowed to update all states (full mag_3d) - new dedicated control logic for mag/mag_3d fusion and standalone mag heading fusion - if WMM available use for mag_I and mag_B init - mag states reset if external yaw reset (yaw estimator, GPS yaw, etc) - mag reset if declination changed (eliminate _mag_yaw_reset_req) - mag fusion (but not mag_hdg or mag_3d) can be active during gps_yaw or ev_yaw (if yaw aligned north) Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
@@ -41,199 +41,86 @@
|
||||
|
||||
void Ekf::controlMagFusion()
|
||||
{
|
||||
bool mag_data_ready = false;
|
||||
bool mag_data_valid = false;
|
||||
|
||||
magSample mag_sample;
|
||||
|
||||
if (_mag_buffer) {
|
||||
mag_data_ready = _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample);
|
||||
|
||||
if (mag_data_ready) {
|
||||
|
||||
// sensor or calibration has changed, clear any mag bias and reset low pass filter
|
||||
if (mag_sample.reset || (_mag_counter == 0)) {
|
||||
// Zero the magnetometer bias states
|
||||
_state.mag_B.zero();
|
||||
|
||||
// Zero the corresponding covariances and set
|
||||
// variances to the values use for initial alignment
|
||||
P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
|
||||
|
||||
// reset any saved covariance data for re-use when auto-switching between heading and 3-axis fusion
|
||||
_saved_mag_bf_variance.zero();
|
||||
|
||||
_control_status.flags.mag_fault = false;
|
||||
|
||||
_mag_lpf.reset(mag_sample.mag);
|
||||
_mag_counter = 1;
|
||||
|
||||
} else {
|
||||
_mag_lpf.update(mag_sample.mag);
|
||||
_mag_counter++;
|
||||
}
|
||||
|
||||
// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
|
||||
// this is useful if there is a lot of interference on the sensor measurement.
|
||||
if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
|
||||
&& (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))
|
||||
) {
|
||||
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
|
||||
mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred);
|
||||
_control_status.flags.synthetic_mag_z = true;
|
||||
|
||||
} else {
|
||||
_control_status.flags.synthetic_mag_z = false;
|
||||
}
|
||||
|
||||
mag_data_valid = checkMagField(mag_sample.mag);
|
||||
|
||||
// compute mag heading innovation (for estimator_aid_src_mag_heading logging)
|
||||
const Vector3f mag_observation = mag_sample.mag - _state.mag_B;
|
||||
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
|
||||
const Vector3f mag_earth_pred = R_to_earth * mag_observation;
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_mag_heading);
|
||||
_aid_src_mag_heading.timestamp_sample = mag_sample.time_us;
|
||||
_aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
|
||||
_aid_src_mag_heading.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _aid_src_mag_heading.observation);
|
||||
|
||||
// compute magnetometer innovations (for estimator_aid_src_mag logging)
|
||||
// rotate magnetometer earth field state into body frame
|
||||
const Vector3f mag_I_body = _state.quat_nominal.rotateVectorInverse(_state.mag_I);
|
||||
const Vector3f mag_innov = mag_I_body - mag_observation;
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_mag);
|
||||
_aid_src_mag.timestamp_sample = mag_sample.time_us;
|
||||
mag_observation.copyTo(_aid_src_mag.observation);
|
||||
mag_innov.copyTo(_aid_src_mag.innovation);
|
||||
|
||||
} else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) {
|
||||
// No data anymore. Stop until it comes back.
|
||||
stopMagFusion();
|
||||
}
|
||||
}
|
||||
|
||||
// If we are on ground, reset the flight alignment flag so that the mag fields will be
|
||||
// re-initialised next time we achieve flight altitude
|
||||
if (!_control_status.flags.in_air) {
|
||||
// reset the flight alignment flag so that the mag fields will be
|
||||
// re-initialised next time we achieve flight altitude
|
||||
if (!_control_status_prev.flags.in_air && _control_status.flags.in_air) {
|
||||
_control_status.flags.mag_aligned_in_flight = false;
|
||||
}
|
||||
|
||||
if (mag_data_ready && mag_data_valid && !_control_status.flags.tilt_align && !_control_status.flags.yaw_align) {
|
||||
// calculate the initial magnetic field and yaw alignment
|
||||
// but do not mark the yaw alignement complete as it needs to be
|
||||
// reset once the leveling phase is done
|
||||
if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
|
||||
if ((_mag_counter > 1) && isTimedOut(_aid_src_mag_heading.time_last_fuse, (uint64_t)100'000)) {
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
// the angle of the projection onto the horizontal gives the yaw angle
|
||||
const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState();
|
||||
const float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
|
||||
// check mag state observability
|
||||
checkYawAngleObservability();
|
||||
checkMagBiasObservability();
|
||||
|
||||
const float yaw_prev = getEulerYaw(_R_to_earth);
|
||||
|
||||
if (fabsf(yaw_new - yaw_prev) > math::radians(1.f)) {
|
||||
|
||||
ECL_INFO("mag heading init %.3f -> %.3f rad (declination %.1f)", (double)yaw_prev, (double)yaw_new, (double)getMagDeclination());
|
||||
|
||||
// update the rotation matrix using the new yaw value
|
||||
_R_to_earth = updateYawInRotMat(yaw_new, Dcmf(_state.quat_nominal));
|
||||
_state.quat_nominal = _R_to_earth;
|
||||
|
||||
// reset the output predictor state history to match the EKF initial values
|
||||
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
|
||||
|
||||
// set the earth magnetic field states using the updated rotation
|
||||
_state.mag_I = _R_to_earth * _mag_lpf.getState();
|
||||
_state.mag_B.zero();
|
||||
|
||||
_aid_src_mag_heading.time_last_fuse = _time_delayed_us;
|
||||
_time_last_heading_fuse = _time_delayed_us;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
if (_mag_bias_observable || _yaw_angle_observable) {
|
||||
_time_last_mov_3d_mag_suitable = _time_delayed_us;
|
||||
}
|
||||
|
||||
if (_params.mag_fusion_type >= MagFuseType::NONE
|
||||
|| _control_status.flags.mag_fault
|
||||
|| !_control_status.flags.tilt_align) {
|
||||
|
||||
if (_params.mag_fusion_type == MagFuseType::NONE) {
|
||||
stopMagFusion();
|
||||
stopMagHdgFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
_mag_yaw_reset_req |= !_control_status.flags.yaw_align;
|
||||
// stop mag (require a reset before using again) if there was an external yaw reset (yaw estimator, GPS yaw, etc)
|
||||
if (_mag_decl_cov_reset && (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat)) {
|
||||
ECL_INFO("yaw reset, stopping mag fusion to force reinitialization");
|
||||
stopMagFusion();
|
||||
resetMagCov();
|
||||
}
|
||||
|
||||
if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) {
|
||||
magSample mag_sample;
|
||||
|
||||
if (shouldInhibitMag() || !mag_data_valid) {
|
||||
if (uint32_t(_time_delayed_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
|
||||
// If magnetometer use has been inhibited continuously then stop the fusion
|
||||
stopMagFusion();
|
||||
}
|
||||
if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) {
|
||||
|
||||
return;
|
||||
if (mag_sample.reset || (_mag_counter == 0)) {
|
||||
// sensor or calibration has changed, reset low pass filter (reset handled by controlMag3DFusion/controlMagHeadingFusion)
|
||||
_control_status.flags.mag_fault = false;
|
||||
|
||||
_state.mag_B.zero();
|
||||
resetMagCov();
|
||||
|
||||
_mag_lpf.reset(mag_sample.mag);
|
||||
_mag_counter = 1;
|
||||
|
||||
} else {
|
||||
_mag_use_not_inhibit_us = _time_delayed_us;
|
||||
_mag_lpf.update(mag_sample.mag);
|
||||
_mag_counter++;
|
||||
}
|
||||
|
||||
const bool mag_enabled_previously = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
|
||||
const bool starting_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
|
||||
&& checkMagField(mag_sample.mag)
|
||||
&& (_mag_counter > 5) // wait until we have more than a few samples through the filter
|
||||
&& (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame
|
||||
&& (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset
|
||||
&& isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL);
|
||||
|
||||
// Determine if we should use simple magnetic heading fusion which works better when
|
||||
// there are large external disturbances or the more accurate 3-axis fusion
|
||||
switch (_params.mag_fusion_type) {
|
||||
default:
|
||||
// FALLTHROUGH
|
||||
case MagFuseType::AUTO:
|
||||
selectMagAuto();
|
||||
break;
|
||||
// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
|
||||
// this is useful if there is a lot of interference on the sensor measurement.
|
||||
if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
|
||||
&& (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps))
|
||||
) {
|
||||
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
|
||||
* Vector3f(_mag_strength_gps, 0, 0);
|
||||
|
||||
case MagFuseType::INDOOR:
|
||||
mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred);
|
||||
|
||||
/* fallthrough */
|
||||
case MagFuseType::HEADING:
|
||||
startMagHdgFusion();
|
||||
break;
|
||||
_control_status.flags.synthetic_mag_z = true;
|
||||
|
||||
case MagFuseType::MAG_3D:
|
||||
startMag3DFusion();
|
||||
break;
|
||||
} else {
|
||||
_control_status.flags.synthetic_mag_z = false;
|
||||
}
|
||||
|
||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||
controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag);
|
||||
controlMagHeadingFusion(mag_sample, starting_conditions_passing, _aid_src_mag_heading);
|
||||
|
||||
if (_mag_yaw_reset_req || !_control_status.flags.yaw_align || mag_sample.reset || !mag_enabled_previously || haglYawResetReq()) {
|
||||
|
||||
if ((_mag_counter > 1) && resetMagStates(_mag_lpf.getState())) {
|
||||
_mag_yaw_reset_req = false;
|
||||
|
||||
} else {
|
||||
// mag reset failed, try again next time
|
||||
_mag_yaw_reset_req = true;
|
||||
|
||||
if (mag_sample.reset) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!_control_status.flags.yaw_align) {
|
||||
// Having the yaw aligned is mandatory to continue
|
||||
return;
|
||||
}
|
||||
|
||||
checkMagDeclRequired();
|
||||
|
||||
runMagAndMagDeclFusions(mag_sample.mag);
|
||||
} else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) {
|
||||
// No data anymore. Stop until it comes back.
|
||||
stopMagHdgFusion();
|
||||
stopMagFusion();
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::haglYawResetReq()
|
||||
bool Ekf::checkHaglYawResetReq() const
|
||||
{
|
||||
// We need to reset the yaw angle after climbing away from the ground to enable
|
||||
// recovery from ground level magnetic interference.
|
||||
@@ -244,30 +131,20 @@ bool Ekf::haglYawResetReq()
|
||||
static constexpr float mag_anomalies_max_hagl = 1.5f;
|
||||
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
|
||||
return above_mag_anomalies;
|
||||
#else
|
||||
return true;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Ekf::resetMagStates(const Vector3f &mag)
|
||||
void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
|
||||
{
|
||||
// prevent a reset being performed more than once on the same frame
|
||||
if ((_flt_mag_align_start_time == _time_delayed_us)
|
||||
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool reset_heading = true;
|
||||
bool has_realigned_yaw = false;
|
||||
|
||||
// reinit mag states
|
||||
const Vector3f mag_I_before_reset = _state.mag_I;
|
||||
const Vector3f mag_B_before_reset = _state.mag_B;
|
||||
|
||||
const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
|
||||
// reset covariances to default
|
||||
resetMagCov();
|
||||
|
||||
// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
|
||||
if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
|
||||
@@ -276,91 +153,72 @@ bool Ekf::resetMagStates(const Vector3f &mag)
|
||||
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
|
||||
* Vector3f(_mag_strength_gps, 0, 0);
|
||||
|
||||
// mag_B: reset
|
||||
if (isYawEmergencyEstimateAvailable()) {
|
||||
const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth);
|
||||
const Dcmf R_to_body = R_to_earth.transpose();
|
||||
|
||||
// mag_B: reset using WMM and yaw estimator, initialize variances (so resetMagHeading will use mag bias)
|
||||
// mag_B: reset using WMM and yaw estimator
|
||||
_state.mag_B = mag - (R_to_body * mag_earth_pred);
|
||||
P.uncorrelateCovarianceSetVariance<3>(19, R_MAG);
|
||||
_saved_mag_bf_variance = P.slice<3, 3>(19, 19).diag();
|
||||
|
||||
ECL_INFO("using yaw estimator to reset mag bias [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
|
||||
(double)mag_B_before_reset(0), (double)mag_B_before_reset(1), (double)mag_B_before_reset(2),
|
||||
(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
|
||||
);
|
||||
ECL_INFO("resetMagStates using yaw estimator");
|
||||
|
||||
} else if (!reset_heading && _control_status.flags.yaw_align) {
|
||||
// mag_B: reset using WMM
|
||||
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
|
||||
_state.mag_B = mag - (R_to_body * mag_earth_pred);
|
||||
|
||||
} else {
|
||||
// mag_B: reset
|
||||
_state.mag_B.zero();
|
||||
}
|
||||
|
||||
// mag_I: reset, skipped if no change in state and variance good
|
||||
_state.mag_I = mag_earth_pred;
|
||||
|
||||
if (reset_heading) {
|
||||
has_realigned_yaw = resetMagHeading(mag);
|
||||
resetMagHeading(mag);
|
||||
}
|
||||
|
||||
// mag_I: reset to WMM
|
||||
_state.mag_I = mag_earth_pred;
|
||||
// earth field was reset to WMM, skip initial declination fusion
|
||||
_mag_decl_cov_reset = true;
|
||||
|
||||
} else {
|
||||
// mag_B: reset
|
||||
_state.mag_B.zero();
|
||||
|
||||
// Use the magnetometer measurement to reset the field states
|
||||
if (reset_heading) {
|
||||
has_realigned_yaw = resetMagHeading(mag);
|
||||
resetMagHeading(mag);
|
||||
}
|
||||
|
||||
// mag_I: use the last magnetometer measurements to reset the field states
|
||||
_state.mag_I = _R_to_earth * mag;
|
||||
}
|
||||
|
||||
resetMagCov();
|
||||
if (!mag_I_before_reset.longerThan(0.f)) {
|
||||
ECL_INFO("initializing mag I [%.3f, %.3f, %.3f], mag B [%.3f, %.3f, %.3f]",
|
||||
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
|
||||
(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
|
||||
);
|
||||
|
||||
if (has_realigned_yaw) {
|
||||
if (!mag_I_before_reset.longerThan(0.f)) {
|
||||
ECL_INFO("initializing mag I [%.3f, %.3f, %.3f], mag B [%.3f, %.3f, %.3f]",
|
||||
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
|
||||
} else {
|
||||
ECL_INFO("resetting mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
|
||||
(double)mag_I_before_reset(0), (double)mag_I_before_reset(1), (double)mag_I_before_reset(2),
|
||||
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2)
|
||||
);
|
||||
|
||||
if (mag_B_before_reset.longerThan(0.f) || _state.mag_B.longerThan(0.f)) {
|
||||
ECL_INFO("resetting mag B [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
|
||||
(double)mag_B_before_reset(0), (double)mag_B_before_reset(1), (double)mag_B_before_reset(2),
|
||||
(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
|
||||
);
|
||||
|
||||
} else {
|
||||
ECL_INFO("resetting mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
|
||||
(double)mag_I_before_reset(0), (double)mag_I_before_reset(1), (double)mag_I_before_reset(2),
|
||||
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2)
|
||||
);
|
||||
}
|
||||
|
||||
_control_status.flags.yaw_align = true;
|
||||
|
||||
if (_control_status.flags.in_air) {
|
||||
// record the start time for the magnetic field alignment
|
||||
_flt_mag_align_start_time = _time_delayed_us;
|
||||
_control_status.flags.mag_aligned_in_flight = true;
|
||||
|
||||
} else {
|
||||
_flt_mag_align_start_time = 0;
|
||||
_control_status.flags.mag_aligned_in_flight = false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void Ekf::selectMagAuto()
|
||||
{
|
||||
check3DMagFusionSuitability();
|
||||
canUse3DMagFusion() ? startMag3DFusion() : startMagHdgFusion();
|
||||
}
|
||||
|
||||
void Ekf::check3DMagFusionSuitability()
|
||||
{
|
||||
checkYawAngleObservability();
|
||||
checkMagBiasObservability();
|
||||
|
||||
if (_mag_bias_observable || _yaw_angle_observable) {
|
||||
_time_last_mov_3d_mag_suitable = _time_delayed_us;
|
||||
// record the start time for the magnetic field alignment
|
||||
if (_control_status.flags.in_air) {
|
||||
_control_status.flags.mag_aligned_in_flight = true;
|
||||
_flt_mag_align_start_time = _time_delayed_us;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -399,49 +257,10 @@ void Ekf::checkMagBiasObservability()
|
||||
_time_yaw_started = _time_delayed_us;
|
||||
}
|
||||
|
||||
bool Ekf::canUse3DMagFusion() const
|
||||
{
|
||||
// Use of 3D fusion requires an in-air heading alignment but it should not
|
||||
// be used when the heading and mag biases are not observable for more than 2 seconds
|
||||
return _control_status.flags.mag_aligned_in_flight
|
||||
&& ((_time_delayed_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6);
|
||||
}
|
||||
|
||||
void Ekf::checkMagDeclRequired()
|
||||
{
|
||||
// if we are using 3-axis magnetometer fusion, but without external NE aiding,
|
||||
// then the declination must be fused as an observation to prevent long term heading drift
|
||||
// fusing declination when gps aiding is available is optional, but recommended to prevent
|
||||
// problem if the vehicle is static for extended periods of time
|
||||
const bool user_selected = (_params.mag_declination_source & GeoDeclinationMask::FUSE_DECL);
|
||||
const bool not_using_ne_aiding = !_control_status.flags.gps;
|
||||
_control_status.flags.mag_dec = (_control_status.flags.mag_3D && (not_using_ne_aiding || user_selected));
|
||||
}
|
||||
|
||||
bool Ekf::shouldInhibitMag() const
|
||||
{
|
||||
// If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer
|
||||
// if a yaw angle relative to true North is required for navigation. If no GPS or other earth frame aiding
|
||||
// is available, assume that we are operating indoors and the magnetometer should not be used.
|
||||
// Also inhibit mag fusion when a strong magnetic field interference is detected or the user
|
||||
// has explicitly stopped magnetometer use.
|
||||
const bool user_selected = (_params.mag_fusion_type == MagFuseType::INDOOR);
|
||||
|
||||
const bool heading_not_required_for_navigation = !_control_status.flags.gps;
|
||||
|
||||
return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed;
|
||||
}
|
||||
|
||||
bool Ekf::checkMagField(const Vector3f &mag_sample)
|
||||
{
|
||||
_control_status.flags.mag_field_disturbed = false;
|
||||
|
||||
if ((_params.mag_fusion_type == MagFuseType::NONE)
|
||||
|| (_params.mag_fusion_type == MagFuseType::INDOOR && !_control_status.flags.gps)) {
|
||||
//TODO: review this: gps flag cannnot be set if yaw isn't aligned
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_params.mag_check == 0) {
|
||||
// skip all checks
|
||||
return true;
|
||||
@@ -506,105 +325,38 @@ bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected,
|
||||
&& (measured <= expected + gate);
|
||||
}
|
||||
|
||||
void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
|
||||
void Ekf::resetMagHeading(const Vector3f &mag)
|
||||
{
|
||||
if (_control_status.flags.mag_3D) {
|
||||
run3DMagAndDeclFusions(mag);
|
||||
|
||||
} else if (_control_status.flags.mag_hdg) {
|
||||
// Rotate the measurements into earth frame using the zero yaw angle
|
||||
Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
|
||||
|
||||
Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B);
|
||||
|
||||
// the angle of the projection onto the horizontal gives the yaw angle
|
||||
// calculate the yaw innovation and wrap to the interval between +-pi
|
||||
float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
|
||||
|
||||
float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
|
||||
float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);
|
||||
|
||||
_aid_src_mag_heading.observation = measured_hdg;
|
||||
_aid_src_mag_heading.observation_variance = obs_var;
|
||||
_aid_src_mag_heading.innovation = innovation;
|
||||
_aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg;
|
||||
|
||||
fuseYaw(_aid_src_mag_heading);
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
|
||||
{
|
||||
// sanity check mag_B before they are used to constrain heading drift
|
||||
const Vector3f mag_bias_var = P.slice<3, 3>(19, 19).diag();
|
||||
const bool mag_bias_var_good = (mag_bias_var.min() > 0.f) && (mag_bias_var.max() < sq(0.02f));
|
||||
|
||||
const bool update_all_states = _control_status.flags.mag_aligned_in_flight && mag_bias_var_good;
|
||||
|
||||
if (!_mag_decl_cov_reset) {
|
||||
// After any magnetic field covariance reset event the earth field state
|
||||
// covariances need to be corrected to incorporate knowledge of the declination
|
||||
// before fusing magnetometer data to prevent rapid rotation of the earth field
|
||||
// states for the first few observations.
|
||||
fuseDeclination(0.02f);
|
||||
_mag_decl_cov_reset = true;
|
||||
fuseMag(mag, _aid_src_mag, update_all_states);
|
||||
|
||||
} else {
|
||||
// The normal sequence is to fuse the magnetometer data first before fusing
|
||||
// declination angle at a higher uncertainty to allow some learning of
|
||||
// declination angle over time.
|
||||
fuseMag(mag, _aid_src_mag, update_all_states);
|
||||
|
||||
if (_control_status.flags.mag_dec) {
|
||||
fuseDeclination(0.5f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::resetMagHeading(const Vector3f &mag)
|
||||
{
|
||||
// prevent a reset being performed more than once on the same frame
|
||||
if ((_flt_mag_align_start_time == _time_delayed_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// use mag bias if variance good (unless configured for HEADING only)
|
||||
Vector3f mag_bias{0.f, 0.f, 0.f};
|
||||
const Vector3f mag_bias_var = getMagBiasVariance();
|
||||
|
||||
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= math::max(sq(_params.mag_noise), sq(0.01f)))) {
|
||||
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
|
||||
mag_bias = _state.mag_B;
|
||||
}
|
||||
|
||||
const Vector3f mag_init = _mag_lpf.getState() - mag_bias;
|
||||
// calculate mag heading
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
|
||||
|
||||
const bool heading_required_for_navigation = _control_status.flags.gps;
|
||||
// the angle of the projection onto the horizontal gives the yaw angle
|
||||
const Vector3f mag_earth_pred = R_to_earth * (mag - mag_bias);
|
||||
|
||||
if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || ((_params.mag_fusion_type == MagFuseType::INDOOR) && heading_required_for_navigation)) {
|
||||
// calculate the observed yaw angle and yaw variance
|
||||
const float declination = getMagDeclination();
|
||||
float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;
|
||||
float yaw_new_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f));
|
||||
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
|
||||
ECL_INFO("reset mag heading %.3f -> %.3f rad (bias:[%.3f, %.3f, %.3f], declination:%.1f)",
|
||||
(double)getEulerYaw(_R_to_earth), (double)yaw_new,
|
||||
(double)mag_bias(0), (double)mag_bias(1), (double)mag_bias(2),
|
||||
(double)declination);
|
||||
|
||||
// the angle of the projection onto the horizontal gives the yaw angle
|
||||
const Vector3f mag_earth_pred = R_to_earth * mag_init;
|
||||
// update quaternion states and corresponding covarainces
|
||||
resetQuatStateYaw(yaw_new, yaw_new_variance);
|
||||
_mag_heading_last_declination = declination;
|
||||
|
||||
// calculate the observed yaw angle and yaw variance
|
||||
float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
|
||||
float yaw_new_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f));
|
||||
|
||||
ECL_INFO("reset mag heading %.3f -> %.3f rad (bias:[%.3f, %.3f, %.3f], declination:%.1f)",
|
||||
(double)getEulerYaw(_R_to_earth), (double)yaw_new,
|
||||
(double)mag_bias(0), (double)mag_bias(1), (double)mag_bias(2),
|
||||
(double)getMagDeclination());
|
||||
|
||||
// update quaternion states and corresponding covarainces
|
||||
resetQuatStateYaw(yaw_new, yaw_new_variance);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
_time_last_heading_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
float Ekf::getMagDeclination()
|
||||
@@ -616,72 +368,12 @@ float Ekf::getMagDeclination()
|
||||
|
||||
} else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
|
||||
// use parameter value until GPS is available, then use value returned by geo library
|
||||
if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) {
|
||||
if (PX4_ISFINITE(_mag_declination_gps)) {
|
||||
return _mag_declination_gps;
|
||||
|
||||
} else {
|
||||
return math::radians(_params.mag_declination_deg);
|
||||
}
|
||||
|
||||
} else {
|
||||
// always use the parameter value
|
||||
return math::radians(_params.mag_declination_deg);
|
||||
}
|
||||
|
||||
// otherwise use the parameter value
|
||||
return math::radians(_params.mag_declination_deg);
|
||||
}
|
||||
|
||||
void Ekf::stopMagFusion()
|
||||
{
|
||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||
ECL_INFO("stopping all mag fusion");
|
||||
stopMag3DFusion();
|
||||
stopMagHdgFusion();
|
||||
clearMagCov();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopMag3DFusion()
|
||||
{
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
saveMagCovData();
|
||||
|
||||
_control_status.flags.mag_3D = false;
|
||||
_control_status.flags.mag_dec = false;
|
||||
|
||||
_fault_status.flags.bad_mag_x = false;
|
||||
_fault_status.flags.bad_mag_y = false;
|
||||
_fault_status.flags.bad_mag_z = false;
|
||||
|
||||
_fault_status.flags.bad_mag_decl = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopMagHdgFusion()
|
||||
{
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
_control_status.flags.mag_hdg = false;
|
||||
|
||||
_fault_status.flags.bad_hdg = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::startMagHdgFusion()
|
||||
{
|
||||
if (!_control_status.flags.mag_hdg) {
|
||||
stopMag3DFusion();
|
||||
ECL_INFO("starting mag heading fusion");
|
||||
_control_status.flags.mag_hdg = true;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::startMag3DFusion()
|
||||
{
|
||||
if (!_control_status.flags.mag_3D) {
|
||||
|
||||
stopMagHdgFusion();
|
||||
|
||||
zeroMagCov();
|
||||
loadMagCovData();
|
||||
_control_status.flags.mag_3D = true;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user