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

ekf2: remove realignYawGPS() (replaced with yaw estimator)

This commit is contained in:
Daniel Agar 2022-10-17 12:39:16 -04:00
parent c267cf71c3
commit ed558e199f
4 changed files with 39 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/ekf2/EKF/ekf.h
View File

@ -519,7 +519,6 @@ private:
bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable
bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected
uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset
uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited
float _last_static_yaw{NAN}; ///< last yaw angle recorded when on ground motion checks were passing (rad)
@ -776,10 +775,6 @@ private:
// return true if successful
bool resetYawToEv();
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
bool realignYawGPS(const Vector3f &mag);
// Return the magnetic declination in radians to be used by the alignment and fusion processing
float getMagDeclination();
@ -899,7 +894,7 @@ private:
void runOnGroundYawReset();
bool canResetMagHeading() const;
void runInAirYawReset(const Vector3f &mag);
void runInAirYawReset();
void selectMagAuto();
void check3DMagFusionSuitability();

111
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -298,117 +298,6 @@ void Ekf::alignOutputFilter()
_output_new = _output_buffer.get_newest();
}
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle only.
bool Ekf::realignYawGPS(const Vector3f &mag)
{
const float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1)));
// Need at least 5 m/s of GPS horizontal speed and
// ratio of velocity error to velocity < 0.15 for a reliable alignment
const bool gps_yaw_alignment_possible = (gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed));
if (!gps_yaw_alignment_possible) {
// attempt a normal alignment using the magnetometer
return resetMagHeading();
}
// check for excessive horizontal GPS velocity innovations
const float gps_vel_test_ratio = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]);
const bool badVelInnov = (gps_vel_test_ratio > 1.0f) && _control_status.flags.gps;
// calculate GPS course over ground angle
const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
// calculate course yaw angle
const float ekfCOG = atan2f(_state.vel(1), _state.vel(0));
// Check the EKF and GPS course over ground for consistency
const float courseYawError = wrap_pi(gpsCOG - ekfCOG);
// If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
const bool badYawErr = fabsf(courseYawError) > 0.5f;
const bool badMagYaw = (badYawErr && badVelInnov);
if (badMagYaw) {
_num_bad_flight_yaw_events++;
}
// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
if (badMagYaw || !_control_status.flags.yaw_align) {
_warning_events.flags.bad_yaw_using_gps_course = true;
ECL_WARN("bad yaw, using GPS course");
// declare the magnetometer as failed if a bad yaw has occurred more than once
if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2)
&& !_control_status.flags.mag_fault) {
_warning_events.flags.stopping_mag_use = true;
ECL_WARN("stopping mag use");
_control_status.flags.mag_fault = true;
}
// calculate new yaw estimate
float yaw_new;
if (!_control_status.flags.mag_aligned_in_flight) {
// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
const float current_yaw = getEulerYaw(_R_to_earth);
yaw_new = current_yaw + courseYawError;
_control_status.flags.mag_aligned_in_flight = true;
} else if (_control_status.flags.wind) {
// we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is
// aligned with the wind relative GPS velocity vector
yaw_new = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)),
(_gps_sample_delayed.vel(0) - _state.wind_vel(0)));
} else {
// we don't have wind estimates, so align yaw to the GPS velocity vector
yaw_new = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
}
// use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment
const float SpdErrorVariance = sq(_gps_sample_delayed.sacc) + P(4, 4) + P(5, 5);
const float sineYawError = math::constrain(sqrtf(SpdErrorVariance) / gpsSpeed, 0.0f, 1.0f);
const float yaw_variance_new = sq(asinf(sineYawError));
// Apply updated yaw and yaw variance to states and covariances
resetQuatStateYaw(yaw_new, yaw_variance_new);
// Use the last magnetometer measurements to reset the field states
_state.mag_B.zero();
_state.mag_I = _R_to_earth * mag;
resetMagCov();
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
// If heading was bad, then we also need to reset the velocity and position states
if (badMagYaw) {
resetVelocityToGps(_gps_sample_delayed);
resetHorizontalPositionToGps(_gps_sample_delayed);
}
return true;
} else {
// align mag states only
// calculate initial earth magnetic field states
_state.mag_I = _R_to_earth * mag;
resetMagCov();
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
return true;
}
}
// Reset heading and magnetic field states
bool Ekf::resetMagHeading()
{

11
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -127,18 +127,9 @@ void Ekf::controlGpsFusion()
if (resetYawToEKFGSF()) {
ECL_WARN("GPS emergency yaw reset");
}
} else {
// use GPS velocity data to check and correct yaw angle if a FW vehicle
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
// if flying a fixed wing aircraft, do a complete reset that includes yaw
_mag_yaw_reset_req = true;
}
_warning_events.flags.gps_fusion_timout = true;
ECL_WARN("GPS fusion timeout - resetting");
}
ECL_WARN("GPS fusion timeout - resetting");
resetVelocityToGps(gps_sample);
resetHorizontalPositionToGps(gps_sample);
}

42
src/modules/ekf2/EKF/mag_control.cpp
View File

@ -94,7 +94,6 @@ void Ekf::controlMagFusion()
// re-initialised next time we achieve flight altitude
if (!_control_status.flags.in_air) {
_control_status.flags.mag_aligned_in_flight = false;
_num_bad_flight_yaw_events = 0;
}
if (_params.mag_fusion_type >= MagFuseType::NONE
@ -141,7 +140,7 @@ void Ekf::controlMagFusion()
if (_control_status.flags.in_air) {
checkHaglYawResetReq();
runInAirYawReset(mag_sample.mag);
runInAirYawReset();
} else {
runOnGroundYawReset();
@ -197,13 +196,43 @@ bool Ekf::canResetMagHeading() const
return !_control_status.flags.mag_field_disturbed && (_params.mag_fusion_type != MagFuseType::NONE);
}
void Ekf::runInAirYawReset(const Vector3f &mag_sample)
void Ekf::runInAirYawReset()
{
if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) {
bool has_realigned_yaw = false;
if (_control_status.flags.gps && _control_status.flags.fixed_wing) {
has_realigned_yaw = realignYawGPS(mag_sample);
// use yaw estimator if available
if (_control_status.flags.gps && isYawEmergencyEstimateAvailable() &&
(_mag_counter != 0) && isNewestSampleRecent(_time_last_mag_buffer_push, 500'000) // mag LPF available
) {
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
_information_events.flags.yaw_aligned_to_imu_gps = true;
_ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us;
// 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;
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
_state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred);
} 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();
_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)
);
resetMagCov();
}
if (!has_realigned_yaw && canResetMagHeading()) {
@ -215,6 +244,9 @@ void Ekf::runInAirYawReset(const Vector3f &mag_sample)
_control_status.flags.yaw_align = true;
_control_status.flags.mag_aligned_in_flight = true;
// record the time for the magnetic field alignment event
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
if (_mag_inhibit_yaw_reset_req) {