fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 13:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
-111
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()
{