mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 14:03:34 +08:00
ekf2: mag control cleanup mag reset
This commit is contained in:
Daniel Agar
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
57b3c26ab0
commit
c8738e3a0d
@@ -900,10 +900,6 @@ private:
|
||||
void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
// reset the heading and magnetic field states using the declination and magnetometer measurements
|
||||
// return true if successful
|
||||
bool resetMagHeading();
|
||||
|
||||
// Return the magnetic declination in radians to be used by the alignment and fusion processing
|
||||
float getMagDeclination();
|
||||
|
||||
@@ -1025,7 +1021,8 @@ private:
|
||||
// control fusion of magnetometer observations
|
||||
void controlMagFusion();
|
||||
|
||||
bool magReset();
|
||||
bool resetMagHeading(const Vector3f &mag);
|
||||
bool resetMagStates(const Vector3f &mag);
|
||||
bool haglYawResetReq();
|
||||
|
||||
void selectMagAuto();
|
||||
|
||||
@@ -181,7 +181,7 @@ void Ekf::controlMagFusion()
|
||||
_mag_use_not_inhibit_us = _time_delayed_us;
|
||||
}
|
||||
|
||||
const bool mag_enabled_previously = _control_status_prev.flags.mag_hdg || _control_status_prev.flags.mag_3D;
|
||||
const bool mag_enabled_previously = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
|
||||
|
||||
// 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
|
||||
@@ -208,12 +208,16 @@ void Ekf::controlMagFusion()
|
||||
|
||||
if (_mag_yaw_reset_req || !_control_status.flags.yaw_align || mag_sample.reset || !mag_enabled_previously || haglYawResetReq()) {
|
||||
|
||||
if (magReset()) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -248,7 +252,7 @@ bool Ekf::haglYawResetReq()
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Ekf::magReset()
|
||||
bool Ekf::resetMagStates(const Vector3f &mag)
|
||||
{
|
||||
// prevent a reset being performed more than once on the same frame
|
||||
if ((_flt_mag_align_start_time == _time_delayed_us)
|
||||
@@ -256,55 +260,86 @@ bool Ekf::magReset()
|
||||
return false;
|
||||
}
|
||||
|
||||
bool reset_heading = true;
|
||||
bool has_realigned_yaw = false;
|
||||
|
||||
// use yaw estimator if available
|
||||
if (_control_status.flags.gps && isYawEmergencyEstimateAvailable()
|
||||
&& (_mag_counter > 1) // mag LPF available
|
||||
) {
|
||||
// reinit mag states
|
||||
const Vector3f mag_I_before_reset = _state.mag_I;
|
||||
const Vector3f mag_B_before_reset = _state.mag_B;
|
||||
|
||||
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
|
||||
const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
|
||||
|
||||
_information_events.flags.yaw_aligned_to_imu_gps = true;
|
||||
// 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)) {
|
||||
|
||||
// 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)) {
|
||||
// use predicted earth field to reset states
|
||||
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
|
||||
_state.mag_I = mag_earth_pred;
|
||||
// use expected earth field to reset states
|
||||
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
|
||||
* Vector3f(_mag_strength_gps, 0, 0);
|
||||
|
||||
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
|
||||
_state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred);
|
||||
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)
|
||||
_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)
|
||||
);
|
||||
|
||||
} else {
|
||||
// Use the last magnetometer measurements to reset the field states
|
||||
// calculate initial earth magnetic field states
|
||||
_state.mag_I = _R_to_earth * _mag_lpf.getState();
|
||||
// mag_B: reset
|
||||
_state.mag_B.zero();
|
||||
}
|
||||
|
||||
ECL_DEBUG("resetting mag I: [%.3f, %.3f, %.3f], 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 (reset_heading) {
|
||||
has_realigned_yaw = resetMagHeading(mag);
|
||||
}
|
||||
|
||||
resetMagCov();
|
||||
// mag_I: reset to WMM
|
||||
_state.mag_I = mag_earth_pred;
|
||||
|
||||
has_realigned_yaw = true;
|
||||
} else {
|
||||
// mag_B: reset
|
||||
_state.mag_B.zero();
|
||||
|
||||
if (reset_heading) {
|
||||
has_realigned_yaw = resetMagHeading(mag);
|
||||
}
|
||||
|
||||
// mag_I: use the last magnetometer measurements to reset the field states
|
||||
_state.mag_I = _R_to_earth * mag;
|
||||
}
|
||||
|
||||
if (!has_realigned_yaw) {
|
||||
has_realigned_yaw = resetMagHeading();
|
||||
}
|
||||
resetMagCov();
|
||||
|
||||
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),
|
||||
(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;
|
||||
|
||||
// record the time for the magnetic field alignment event
|
||||
_flt_mag_align_start_time = _time_delayed_us;
|
||||
} else {
|
||||
_flt_mag_align_start_time = 0;
|
||||
_control_status.flags.mag_aligned_in_flight = false;
|
||||
}
|
||||
|
||||
return true;
|
||||
@@ -475,16 +510,24 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::resetMagHeading()
|
||||
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;
|
||||
}
|
||||
|
||||
const Vector3f mag_init = _mag_lpf.getState();
|
||||
// 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();
|
||||
|
||||
const bool mag_available = (_mag_counter > 1) && !magFieldStrengthDisturbed(mag_init);
|
||||
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= math::max(sq(_params.mag_noise), sq(0.01f)))) {
|
||||
mag_bias = _state.mag_B;
|
||||
}
|
||||
|
||||
const Vector3f mag_init = _mag_lpf.getState() - mag_bias;
|
||||
|
||||
const bool mag_available = !magFieldStrengthDisturbed(mag_init);
|
||||
|
||||
// low pass filtered mag required
|
||||
if (!mag_available) {
|
||||
@@ -503,21 +546,16 @@ bool Ekf::resetMagHeading()
|
||||
|
||||
// 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 = sq(fmaxf(_params.mag_heading_noise, 1.e-2f));
|
||||
float yaw_new_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f));
|
||||
|
||||
ECL_INFO("reset mag heading %.3f -> %.3f rad (declination %.1f)", (double)getEulerYaw(_R_to_earth), (double)yaw_new, (double)getMagDeclination());
|
||||
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);
|
||||
|
||||
// set the earth magnetic field states using the updated rotation
|
||||
_state.mag_I = _R_to_earth * mag_init;
|
||||
|
||||
resetMagCov();
|
||||
|
||||
// record the time for the magnetic field alignment event
|
||||
_flt_mag_align_start_time = _time_delayed_us;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user