mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 13:02:25 +08:00
ekf2: move on ground GNSS checks to separate function
This commit is contained in:
bresch
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
66fe3aa2b3
commit
5332010b13
@@ -55,112 +55,41 @@
|
||||
#define MASK_GPS_VSPD (1<<8)
|
||||
#define MASK_GPS_SPOOFED (1<<9)
|
||||
|
||||
bool Ekf::runGnssChecks(const gnssSample &gps)
|
||||
bool Ekf::runGnssChecks(const gnssSample &gnss)
|
||||
{
|
||||
_gps_check_fail_status.flags.spoofed = gps.spoofed;
|
||||
_gps_check_fail_status.flags.spoofed = gnss.spoofed;
|
||||
|
||||
// Check the fix type
|
||||
_gps_check_fail_status.flags.fix = (gps.fix_type < 3);
|
||||
_gps_check_fail_status.flags.fix = (gnss.fix_type < 3);
|
||||
|
||||
// Check the number of satellites
|
||||
_gps_check_fail_status.flags.nsats = (gps.nsats < _params.req_nsats);
|
||||
_gps_check_fail_status.flags.nsats = (gnss.nsats < _params.req_nsats);
|
||||
|
||||
// Check the position dilution of precision
|
||||
_gps_check_fail_status.flags.pdop = (gps.pdop > _params.req_pdop);
|
||||
_gps_check_fail_status.flags.pdop = (gnss.pdop > _params.req_pdop);
|
||||
|
||||
// Check the reported horizontal and vertical position accuracy
|
||||
_gps_check_fail_status.flags.hacc = (gps.hacc > _params.req_hacc);
|
||||
_gps_check_fail_status.flags.vacc = (gps.vacc > _params.req_vacc);
|
||||
_gps_check_fail_status.flags.hacc = (gnss.hacc > _params.req_hacc);
|
||||
_gps_check_fail_status.flags.vacc = (gnss.vacc > _params.req_vacc);
|
||||
|
||||
// Check the reported speed accuracy
|
||||
_gps_check_fail_status.flags.sacc = (gps.sacc > _params.req_sacc);
|
||||
_gps_check_fail_status.flags.sacc = (gnss.sacc > _params.req_sacc);
|
||||
|
||||
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
|
||||
constexpr float filt_time_const = 10.0f;
|
||||
const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp()))
|
||||
* 1e-6f, 0.001f, filt_time_const);
|
||||
const float filter_coef = dt / filt_time_const;
|
||||
|
||||
// The following checks are only valid when the vehicle is at rest
|
||||
const double lat = gps.lat;
|
||||
const double lon = gps.lon;
|
||||
|
||||
if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
|
||||
// Calculate position movement since last measurement
|
||||
float delta_pos_n = 0.0f;
|
||||
float delta_pos_e = 0.0f;
|
||||
|
||||
// calculate position movement since last GPS fix
|
||||
if (_gps_pos_prev.getProjectionReferenceTimestamp() > 0) {
|
||||
_gps_pos_prev.project(lat, lon, delta_pos_n, delta_pos_e);
|
||||
|
||||
} else {
|
||||
// no previous position has been set
|
||||
_gps_pos_prev.initReference(lat, lon, gps.time_us);
|
||||
_gps_alt_prev = gps.alt;
|
||||
}
|
||||
|
||||
// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
|
||||
const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
|
||||
Vector3f delta_pos(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt));
|
||||
|
||||
// Apply a low pass filter
|
||||
_gps_pos_deriv_filt = delta_pos / dt * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
|
||||
|
||||
// Apply anti-windup to the state instead of the input to avoid generating a bias on asymmetric signals
|
||||
_gps_pos_deriv_filt = matrix::constrain(_gps_pos_deriv_filt, -10.0f * vel_limit, 10.0f * vel_limit);
|
||||
|
||||
// hdrift: calculate the horizontal drift speed and fail if too high
|
||||
_gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm();
|
||||
_gps_check_fail_status.flags.hdrift = (_gps_horizontal_position_drift_rate_m_s > _params.req_hdrift);
|
||||
|
||||
// vdrift: fail if the vertical drift speed is too high
|
||||
_gps_vertical_position_drift_rate_m_s = fabsf(_gps_pos_deriv_filt(2));
|
||||
_gps_check_fail_status.flags.vdrift = (_gps_vertical_position_drift_rate_m_s > _params.req_vdrift);
|
||||
|
||||
// hspeed: check the magnitude of the filtered horizontal GNSS velocity
|
||||
const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel.xy()),
|
||||
-10.0f * _params.req_hdrift,
|
||||
10.0f * _params.req_hdrift);
|
||||
_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);
|
||||
_gps_filtered_horizontal_velocity_m_s = _gps_velNE_filt.norm();
|
||||
_gps_check_fail_status.flags.hspeed = (_gps_filtered_horizontal_velocity_m_s > _params.req_hdrift);
|
||||
|
||||
// vspeed: check the magnitude of the filtered vertical GNSS velocity
|
||||
const float gnss_vz_limit = 10.f * _params.req_vdrift;
|
||||
const float gnss_vz = math::constrain(gps.vel(2), -gnss_vz_limit, gnss_vz_limit);
|
||||
_gps_vel_d_filt = gnss_vz * filter_coef + _gps_vel_d_filt * (1.f - filter_coef);
|
||||
|
||||
_gps_check_fail_status.flags.vspeed = (fabsf(_gps_vel_d_filt) > _params.req_vdrift);
|
||||
|
||||
} else if (_control_status.flags.in_air) {
|
||||
// These checks are always declared as passed when flying
|
||||
// If on ground and moving, the last result before movement commenced is kept
|
||||
_gps_check_fail_status.flags.hdrift = false;
|
||||
_gps_check_fail_status.flags.vdrift = false;
|
||||
_gps_check_fail_status.flags.hspeed = false;
|
||||
_gps_check_fail_status.flags.vspeed = false;
|
||||
|
||||
resetGpsDriftCheckFilters();
|
||||
|
||||
} else {
|
||||
// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation
|
||||
resetGpsDriftCheckFilters();
|
||||
}
|
||||
runOnGroundGnssChecks(gnss);
|
||||
|
||||
// force horizontal speed failure if above the limit
|
||||
if (gps.vel.xy().longerThan(_params.velocity_limit)) {
|
||||
if (gnss.vel.xy().longerThan(_params.velocity_limit)) {
|
||||
_gps_check_fail_status.flags.hspeed = true;
|
||||
}
|
||||
|
||||
// force vertical speed failure if above the limit
|
||||
if (fabsf(gps.vel(2)) > _params.velocity_limit) {
|
||||
if (fabsf(gnss.vel(2)) > _params.velocity_limit) {
|
||||
_gps_check_fail_status.flags.vspeed = true;
|
||||
}
|
||||
|
||||
// save GPS fix for next time
|
||||
_gps_pos_prev.initReference(lat, lon, gps.time_us);
|
||||
_gps_alt_prev = gps.alt;
|
||||
_gnss_pos_prev.initReference(gnss.lat, gnss.lon, gnss.time_us);
|
||||
_gnss_alt_prev = gnss.alt;
|
||||
|
||||
// assume failed first time through
|
||||
if (_last_gps_fail_us == 0) {
|
||||
@@ -190,6 +119,81 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::runOnGroundGnssChecks(const gnssSample &gnss)
|
||||
{
|
||||
if (_control_status.flags.in_air) {
|
||||
// These checks are always declared as passed when flying
|
||||
// If on ground and moving, the last result before movement commenced is kept
|
||||
_gps_check_fail_status.flags.hdrift = false;
|
||||
_gps_check_fail_status.flags.vdrift = false;
|
||||
_gps_check_fail_status.flags.hspeed = false;
|
||||
_gps_check_fail_status.flags.vspeed = false;
|
||||
|
||||
resetGpsDriftCheckFilters();
|
||||
return;
|
||||
}
|
||||
|
||||
if (_control_status.flags.vehicle_at_rest) {
|
||||
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
|
||||
constexpr float filt_time_const = 10.0f;
|
||||
const float dt = math::constrain(float(int64_t(gnss.time_us) - int64_t(
|
||||
_gnss_pos_prev.getProjectionReferenceTimestamp()))
|
||||
* 1e-6f, 0.001f, filt_time_const);
|
||||
const float filter_coef = dt / filt_time_const;
|
||||
|
||||
// Calculate position movement since last measurement
|
||||
float delta_pos_n = 0.0f;
|
||||
float delta_pos_e = 0.0f;
|
||||
|
||||
// calculate position movement since last GPS fix
|
||||
if (_gnss_pos_prev.getProjectionReferenceTimestamp() > 0) {
|
||||
_gnss_pos_prev.project(gnss.lat, gnss.lon, delta_pos_n, delta_pos_e);
|
||||
|
||||
} else {
|
||||
// no previous position has been set
|
||||
_gnss_pos_prev.initReference(gnss.lat, gnss.lon, gnss.time_us);
|
||||
_gnss_alt_prev = gnss.alt;
|
||||
}
|
||||
|
||||
// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
|
||||
const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
|
||||
Vector3f delta_pos(delta_pos_n, delta_pos_e, (_gnss_alt_prev - gnss.alt));
|
||||
|
||||
// Apply a low pass filter
|
||||
_gps_pos_deriv_filt = delta_pos / dt * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
|
||||
|
||||
// Apply anti-windup to the state instead of the input to avoid generating a bias on asymmetric signals
|
||||
_gps_pos_deriv_filt = matrix::constrain(_gps_pos_deriv_filt, -10.0f * vel_limit, 10.0f * vel_limit);
|
||||
|
||||
// hdrift: calculate the horizontal drift speed and fail if too high
|
||||
_gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm();
|
||||
_gps_check_fail_status.flags.hdrift = (_gps_horizontal_position_drift_rate_m_s > _params.req_hdrift);
|
||||
|
||||
// vdrift: fail if the vertical drift speed is too high
|
||||
_gps_vertical_position_drift_rate_m_s = fabsf(_gps_pos_deriv_filt(2));
|
||||
_gps_check_fail_status.flags.vdrift = (_gps_vertical_position_drift_rate_m_s > _params.req_vdrift);
|
||||
|
||||
// hspeed: check the magnitude of the filtered horizontal GNSS velocity
|
||||
const Vector2f gps_velNE = matrix::constrain(Vector2f(gnss.vel.xy()),
|
||||
-10.0f * _params.req_hdrift,
|
||||
10.0f * _params.req_hdrift);
|
||||
_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);
|
||||
_gps_filtered_horizontal_velocity_m_s = _gps_velNE_filt.norm();
|
||||
_gps_check_fail_status.flags.hspeed = (_gps_filtered_horizontal_velocity_m_s > _params.req_hdrift);
|
||||
|
||||
// vspeed: check the magnitude of the filtered vertical GNSS velocity
|
||||
const float gnss_vz_limit = 10.f * _params.req_vdrift;
|
||||
const float gnss_vz = math::constrain(gnss.vel(2), -gnss_vz_limit, gnss_vz_limit);
|
||||
_gps_vel_d_filt = gnss_vz * filter_coef + _gps_vel_d_filt * (1.f - filter_coef);
|
||||
|
||||
_gps_check_fail_status.flags.vspeed = (fabsf(_gps_vel_d_filt) > _params.req_vdrift);
|
||||
|
||||
} else {
|
||||
// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation
|
||||
resetGpsDriftCheckFilters();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetGpsDriftCheckFilters()
|
||||
{
|
||||
_gps_velNE_filt.setZero();
|
||||
|
||||
@@ -883,6 +883,7 @@ private:
|
||||
* Checks are adjusted using the EKF2_REQ_* parameters
|
||||
*/
|
||||
bool runGnssChecks(const gnssSample &gps);
|
||||
void runOnGroundGnssChecks(const gnssSample &gnss);
|
||||
|
||||
void controlGnssHeightFusion(const gnssSample &gps_sample);
|
||||
void stopGpsHgtFusion();
|
||||
|
||||
@@ -402,8 +402,8 @@ protected:
|
||||
float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s)
|
||||
float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s)
|
||||
|
||||
MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
|
||||
float _gps_alt_prev{0.0f}; // height from the previous GPS message (m)
|
||||
MapProjection _gnss_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
|
||||
float _gnss_alt_prev{0.0f}; // height from the previous GPS message (m)
|
||||
|
||||
# if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
// innovation consistency check monitoring ratios
|
||||
|
||||
Reference in New Issue
Block a user