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

ekf2: backend apply code style (generated code still exempt)

Browse Source
This commit is contained in:
Daniel Agar 2021-07-15 12:46:06 -04:00
parent 12ad7b17ce
commit c8e6d93cc0
17 changed files with 387 additions and 241 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
src/modules/ekf2/EKF/EKFGSF_yaw.cpp
View File

@ -13,7 +13,7 @@ EKFGSF_yaw::EKFGSF_yaw()
_ahrs_accel.zero();
}
void EKFGSF_yaw::update(const imuSample& imu_sample,
void EKFGSF_yaw::update(const imuSample &imu_sample,
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
float airspeed, // true airspeed used for centripetal accel compensation - set to 0 when not required.
const Vector3f &imu_gyro_bias) // estimated rate gyro bias (rad/sec)
@ -39,11 +39,13 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
const bool ok_to_align = (accel_norm_sq > sq(lower_accel_limit)) && (accel_norm_sq < sq(upper_accel_limit));
if (ok_to_align) {
initialiseEKFGSF();
ahrsAlignTilt();
_ahrs_ekf_gsf_tilt_aligned = true;
}
return;
}
@ -52,6 +54,7 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
// AHRS prediction cycle for each model - this always runs
_ahrs_accel_fusion_gain = ahrsCalcAccelGain();
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
predictEKF(model_index);
}
@ -61,14 +64,18 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
if (!_ekf_gsf_vel_fuse_started) {
initialiseEKFGSF();
ahrsAlignYaw();
// Initialise to gyro bias estimate from main filter because there could be a large
// uncorrected rate gyro bias error about the gravity vector
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
}
_ekf_gsf_vel_fuse_started = true;
} else {
bool bad_update = false;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
// subsequent measurements are fused as direct state observations
if (!updateEKF(model_index)) {
@ -81,24 +88,29 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
// calculate weighting for each model assuming a normal distribution
const float min_weight = 1e-5f;
uint8_t n_weight_clips = 0;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
_model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index);
if (_model_weights(model_index) < min_weight) {
n_weight_clips++;
_model_weights(model_index) = min_weight;
}
total_weight += _model_weights(model_index);
}
// normalise the weighting function
if (n_weight_clips < N_MODELS_EKFGSF) {
_model_weights /= total_weight;
} else {
// all weights have collapsed due to excessive innovation variances so reset filters
initialiseEKFGSF();
}
}
}
} else if (_ekf_gsf_vel_fuse_started && !_run_ekf_gsf) {
// wait to fly again
_ekf_gsf_vel_fuse_started = false;
@ -108,18 +120,21 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
// To avoid issues with angle wrapping, the yaw state is converted to a vector with length
// equal to the weighting value before it is summed.
Vector2f yaw_vector;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
yaw_vector(0) += _model_weights(model_index) * cosf(_ekf_gsf[model_index].X(2));
yaw_vector(1) += _model_weights(model_index) * sinf(_ekf_gsf[model_index].X(2));
}
_gsf_yaw = atan2f(yaw_vector(1),yaw_vector(0));
_gsf_yaw = atan2f(yaw_vector(1), yaw_vector(0));
// calculate a composite variance for the yaw state from a weighted average of the variance for each model
// models with larger innovations are weighted less
_gsf_yaw_variance = 0.0f;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw);
_gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2,2) + yaw_delta * yaw_delta);
_gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta);
}
// prevent the same velocity data being used more than once
@ -138,6 +153,7 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
// Perform angular rate correction using accel data and reduce correction as accel magnitude moves away from 1 g (reduces drift when vehicle picked up and moved).
// During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward
Vector3f tilt_correction;
if (_ahrs_accel_fusion_gain > 0.0f) {
Vector3f accel = _ahrs_accel;
@ -158,13 +174,16 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
// Gyro bias estimation
constexpr float gyro_bias_limit = 0.05f;
const float spinRate = ang_rate.length();
if (spinRate < 0.175f) {
_ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * _delta_ang_dt);
_ahrs_ekf_gsf[model_index].gyro_bias = matrix::constrain(_ahrs_ekf_gsf[model_index].gyro_bias, -gyro_bias_limit, gyro_bias_limit);
_ahrs_ekf_gsf[model_index].gyro_bias = matrix::constrain(_ahrs_ekf_gsf[model_index].gyro_bias, -gyro_bias_limit,
gyro_bias_limit);
}
// delta angle from previous to current frame
const Vector3f delta_angle_corrected = _delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) * _delta_ang_dt;
const Vector3f delta_angle_corrected = _delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) *
_delta_ang_dt;
// Apply delta angle to rotation matrix
_ahrs_ekf_gsf[model_index].R = ahrsPredictRotMat(_ahrs_ekf_gsf[model_index].R, delta_angle_corrected);
@ -182,7 +201,7 @@ void EKFGSF_yaw::ahrsAlignTilt()
const Vector3f down_in_bf = -_delta_vel.normalized();
// Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf'
const Vector3f i_vec_bf(1.0f,0.0f,0.0f);
const Vector3f i_vec_bf(1.0f, 0.0f, 0.0f);
Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf.dot(down_in_bf));
north_in_bf.normalize();
@ -207,7 +226,7 @@ void EKFGSF_yaw::ahrsAlignYaw()
{
// Align yaw angle for each model
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
Dcmf& R = _ahrs_ekf_gsf[model_index].R;
Dcmf &R = _ahrs_ekf_gsf[model_index].R;
const float yaw = wrap_pi(_ekf_gsf[model_index].X(2));
R = updateYawInRotMat(yaw, R);
@ -226,10 +245,10 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
}
// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
const Dcmf& R = _ahrs_ekf_gsf[model_index].R;
const Dcmf &R = _ahrs_ekf_gsf[model_index].R;
_ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ?
getEuler321Yaw(R) :
getEuler312Yaw(R);
getEuler321Yaw(R) :
getEuler312Yaw(R);
// calculate delta velocity in a horizontal front-right frame
const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;
@ -284,8 +303,9 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
// constrain variances
const float min_var = 1e-6f;
for (unsigned index = 0; index < 3; index++) {
_ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var);
_ekf_gsf[model_index].P(index, index) = fmaxf(_ekf_gsf[model_index].P(index, index), min_var);
}
}
@ -374,8 +394,9 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
// constrain variances
const float min_var = 1e-6f;
for (unsigned index = 0; index < 3; index++) {
_ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var);
_ekf_gsf[model_index].P(index, index) = fmaxf(_ekf_gsf[model_index].P(index, index), min_var);
}
// test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
@ -421,6 +442,7 @@ void EKFGSF_yaw::initialiseEKFGSF()
memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));
const float yaw_increment = 2.0f * _m_pi / (float)N_MODELS_EKFGSF;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
// evenly space initial yaw estimates in the region between +-Pi
_ekf_gsf[model_index].X(2) = -_m_pi + (0.5f * yaw_increment) + ((float)model_index * yaw_increment);
@ -428,11 +450,11 @@ void EKFGSF_yaw::initialiseEKFGSF()
// take velocity states and corresponding variance from last measurement
_ekf_gsf[model_index].X(0) = _vel_NE(0);
_ekf_gsf[model_index].X(1) = _vel_NE(1);
_ekf_gsf[model_index].P(0,0) = sq(_vel_accuracy);
_ekf_gsf[model_index].P(1,1) = _ekf_gsf[model_index].P(0,0);
_ekf_gsf[model_index].P(0, 0) = sq(_vel_accuracy);
_ekf_gsf[model_index].P(1, 1) = _ekf_gsf[model_index].P(0, 0);
// use half yaw interval for yaw uncertainty
_ekf_gsf[model_index].P(2,2) = sq(0.5f * yaw_increment);
_ekf_gsf[model_index].P(2, 2) = sq(0.5f * yaw_increment);
}
}
@ -444,19 +466,23 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const
return _m_2pi_inv * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist);
}
bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const
bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const
{
if (_ekf_gsf_vel_fuse_started) {
*yaw_composite = _gsf_yaw;
*yaw_variance = _gsf_yaw_variance;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
yaw[model_index] = _ekf_gsf[model_index].X(2);
innov_VN[model_index] = _ekf_gsf[model_index].innov(0);
innov_VE[model_index] = _ekf_gsf[model_index].innov(1);
weight[model_index] = _model_weights(model_index);
}
return true;
}
return false;
}
@ -496,23 +522,25 @@ Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g)
// Renormalise rows
for (uint8_t r = 0; r < 3; r++) {
const float rowLengthSq = ret.row(r).norm_squared();
if (rowLengthSq > FLT_EPSILON) {
// Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0
const float rowLengthInv = 1.5f - 0.5f * rowLengthSq;
ret.row(r) *= rowLengthInv;
}
}
}
return ret;
}
bool EKFGSF_yaw::getYawData(float *yaw, float *yaw_variance) const
{
if(_ekf_gsf_vel_fuse_started) {
if (_ekf_gsf_vel_fuse_started) {
*yaw = _gsf_yaw;
*yaw_variance = _gsf_yaw_variance;
return true;
}
return false;
}

26
src/modules/ekf2/EKF/EKFGSF_yaw.h
View File

@ -29,16 +29,16 @@ using namespace estimator;
class EKFGSF_yaw
{
public:
EKFGSF_yaw();
EKFGSF_yaw();
// Update Filter States - this should be called whenever new IMU data is available
// Update Filter States - this should be called whenever new IMU data is available
void update(const imuSample &imu_sample,
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
float airspeed, // true airspeed used for centripetal accel compensation - set to 0 when not required.
const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec)
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
float airspeed, // true airspeed used for centripetal accel compensation - set to 0 when not required.
const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec)
void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s)
float accuracy); // 1-sigma accuracy of velocity measurement (m/s)
float accuracy); // 1-sigma accuracy of velocity measurement (m/s)
// get solution data for logging
bool getLogData(float *yaw_composite,
@ -48,9 +48,9 @@ public:
float innov_VE[N_MODELS_EKFGSF],
float weight[N_MODELS_EKFGSF]) const;
// get yaw estimate and the corresponding variance
// return false if no yaw estimate available
bool getYawData(float *yaw, float *yaw_variance) const;
// get yaw estimate and the corresponding variance
// return false if no yaw estimate available
bool getYawData(float *yaw, float *yaw_variance) const;
private:
@ -68,14 +68,14 @@ private:
float _delta_vel_dt{}; // _delta_vel integration time interval (sec)
float _true_airspeed{}; // true airspeed used for centripetal accel compensation (m/s)
struct _ahrs_ekf_gsf_struct{
struct _ahrs_ekf_gsf_struct {
Dcmf R; // matrix that rotates a vector from body to earth frame
Vector3f gyro_bias; // gyro bias learned and used by the quaternion calculation
bool aligned; // true when AHRS has been aligned
float vel_NE[2]; // NE velocity vector from last GPS measurement (m/s)
bool fuse_gps; // true when GPS should be fused on that frame
float accel_dt; // time step used when generating _simple_accel_FR data (sec)
} _ahrs_ekf_gsf[N_MODELS_EKFGSF]{};
} _ahrs_ekf_gsf[N_MODELS_EKFGSF] {};
bool _ahrs_ekf_gsf_tilt_aligned{}; // true the initial tilt alignment has been calculated
float _ahrs_accel_fusion_gain{}; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation
@ -99,13 +99,13 @@ private:
// Declarations used by a bank of N_MODELS_EKFGSF EKFs
struct _ekf_gsf_struct{
struct _ekf_gsf_struct {
matrix::Vector3f X; // Vel North (m/s), Vel East (m/s), yaw (rad)s
matrix::SquareMatrix<float, 3> P; // covariance matrix
matrix::SquareMatrix<float, 2> S_inverse; // inverse of the innovation covariance matrix
float S_det_inverse; // inverse of the innovation covariance matrix determinant
matrix::Vector2f innov; // Velocity N,E innovation (m/s)
} _ekf_gsf[N_MODELS_EKFGSF]{};
} _ekf_gsf[N_MODELS_EKFGSF] {};
bool _vel_data_updated{}; // true when velocity data has been updated
bool _run_ekf_gsf{}; // true when operating condition is suitable for to run the GSF and EKF models and fuse velocity data

20
src/modules/ekf2/EKF/airspeed_fusion.cpp
View File

@ -55,7 +55,7 @@ void Ekf::fuseAirspeed()
// Variance for true airspeed measurement - (m/sec)^2
const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
// determine if we need the sideslip fusion to correct states other than wind
const bool update_wind_only = !_is_wind_dead_reckoning;
@ -65,11 +65,13 @@ void Ekf::fuseAirspeed()
const float HK1 = ve - vwe;
const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2);
const float v_tas_pred = sqrtf(HK2); // predicted airspeed
//const float HK3 = powf(HK2, -1.0F/2.0F);
if (v_tas_pred < 1.0f) {
// calculation can be badly conditioned for very low airspeed values so don't fuse this time
return;
}
const float HK3 = 1.0f / v_tas_pred;
const float HK4 = HK0*HK3;
const float HK5 = HK1*HK3;
@ -86,11 +88,13 @@ void Ekf::fuseAirspeed()
//const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
// innovation variance - check for badly conditioned calculation
_airspeed_innov_var = (-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
_airspeed_innov_var = (-HK10 * HK14 + HK10 * HK9 + HK12 * HK13 - HK13 * HK15 + HK6 * HK7 * vd + R_TAS);
if (_airspeed_innov_var < R_TAS) { //
// Reset the estimator covariance matrix
// if we are getting aiding from other sources, warn and reset the wind states and covariances only
const char* action_string = nullptr;
const char *action_string = nullptr;
if (update_wind_only) {
resetWindStates();
resetWindCovariance();
@ -101,12 +105,14 @@ void Ekf::fuseAirspeed()
_state.wind_vel.setZero();
action_string = "full";
}
ECL_ERR("airspeed badly conditioned - %s covariance reset", action_string);
_fault_status.flags.bad_airspeed = true;
return;
}
const float HK16 = HK3 / _airspeed_innov_var;
_fault_status.flags.bad_airspeed = false;
@ -119,6 +125,7 @@ void Ekf::fuseAirspeed()
Hfusion.at<23>() = -HK5;
Vector24f Kfusion; // Kalman gain vector
if (!update_wind_only) {
// we have no other source of aiding, so use airspeed measurements to correct states
for (unsigned row = 0; row <= 3; row++) {
@ -164,7 +171,8 @@ void Ekf::fuseAirspeed()
void Ekf::get_true_airspeed(float *tas) const
{
const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2)));
const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(
_state.vel(2)));
memcpy(tas, &tempvar, sizeof(float));
}
@ -174,8 +182,8 @@ void Ekf::get_true_airspeed(float *tas) const
void Ekf::resetWindStates()
{
const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
? getEuler321Yaw(_state.quat_nominal)
: getEuler312Yaw(_state.quat_nominal);
? getEuler321Yaw(_state.quat_nominal)
: getEuler312Yaw(_state.quat_nominal);
if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available

8
src/modules/ekf2/EKF/common.h
View File

@ -99,7 +99,7 @@ struct imuSample {
Vector3f delta_vel; ///< delta velocity in body frame (integrated accelerometer measurements) (m/sec)
float delta_ang_dt; ///< delta angle integration period (sec)
float delta_vel_dt; ///< delta velocity integration period (sec)
bool delta_vel_clipping[3]{}; ///< true (per axis) if this sample contained any accelerometer clipping
bool delta_vel_clipping[3] {}; ///< true (per axis) if this sample contained any accelerometer clipping
};
struct gpsSample {
@ -300,8 +300,8 @@ struct parameters {
float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
// vision position fusion
float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD)
float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD)
float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD)
float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD)
// optical flow fusion
float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec)
@ -487,7 +487,7 @@ union filter_control_status_u {
uint32_t value;
};
// Mavlink bitmask containing state of estimator solution
// Mavlink bitmask containing state of estimator solution
union ekf_solution_status {
struct {
uint16_t attitude : 1; ///< 0 - True if the attitude estimate is good

192
src/modules/ekf2/EKF/control.cpp
View File

@ -59,7 +59,8 @@ void Ekf::controlFusionModes()
_control_status.flags.tilt_align = true;
// send alignment status message to the console
const char* height_source = nullptr;
const char *height_source = nullptr;
if (_control_status.flags.baro_hgt) {
height_source = "baro";
@ -76,9 +77,10 @@ void Ekf::controlFusionModes()
height_source = "unknown";
}
if (height_source){
if (height_source) {
ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
(unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
(unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
}
}
}
@ -100,7 +102,9 @@ void Ekf::controlFusionModes()
// 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 & MASK_USE_GEO_DECL) && (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))) {
if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_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_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
_control_status.flags.synthetic_mag_z = true;
@ -120,16 +124,16 @@ void Ekf::controlFusionModes()
}
{
// Get range data from buffer and check validity
const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(is_rng_data_ready);
// Get range data from buffer and check validity
const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(is_rng_data_ready);
// update range sensor angle parameters in case they have changed
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
// update range sensor angle parameters in case they have changed
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
_range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth);
_range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth);
}
if (_range_sensor.isDataHealthy()) {
@ -195,7 +199,9 @@ void Ekf::controlExternalVisionFusion()
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
// needs to be calculated and the observations rotated into the EKF frame of reference
if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) {
if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS)
|| (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) {
// rotate EV measurements into the EKF Navigation frame
calcExtVisRotMat();
}
@ -218,7 +224,9 @@ void Ekf::controlExternalVisionFusion()
}
// external vision yaw aiding selection logic
if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw
&& _control_status.flags.tilt_align) {
// don't start using EV data unless data is arriving frequently
if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
if (resetYawToEv()) {
@ -242,6 +250,7 @@ void Ekf::controlExternalVisionFusion()
// Use an incremental position fusion method for EV position data if GPS is also used
if (_params.fusion_mode & MASK_USE_GPS) {
_fuse_hpos_as_odom = true;
} else {
_fuse_hpos_as_odom = false;
}
@ -277,10 +286,12 @@ void Ekf::controlExternalVisionFusion()
// use the absolute position
Vector3f ev_pos_meas = _ev_sample_delayed.pos;
Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
if (_params.fusion_mode & MASK_ROTATE_EV) {
ev_pos_meas = _R_ev_to_ekf * ev_pos_meas;
ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
}
_ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0);
_ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1);
@ -326,7 +337,7 @@ void Ekf::controlExternalVisionFusion()
ev_vel_innov_gates.setAll(fmaxf(_params.ev_vel_innov_gate, 1.0f));
fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates,_last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates, _last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, _last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
}
@ -366,8 +377,7 @@ void Ekf::controlOpticalFlowFusion()
const float delta_time_min = fmaxf(0.7f * _delta_time_of, 0.001f);
const float delta_time_max = fminf(1.3f * _delta_time_of, 0.2f);
const bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min
&& _flow_sample_delayed.dt <= delta_time_max;
const bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max;
const bool is_body_rate_comp_available = calcOptFlowBodyRateComp();
if (is_quality_good
@ -405,19 +415,19 @@ void Ekf::controlOpticalFlowFusion()
// Check if we are in-air and require optical flow to control position drift
const bool is_flow_required = _control_status.flags.in_air
&& (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)
|| (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
&& (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)
|| (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
const bool preflight_motion_not_ok = !_control_status.flags.in_air
&& ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5))
|| (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6)));
&& ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5))
|| (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6)));
const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
_inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required)
|| !_control_status.flags.tilt_align;
|| !_control_status.flags.tilt_align;
// Handle cases where we are using optical flow but we should not use it anymore
if (_control_status.flags.opt_flow) {
@ -431,9 +441,8 @@ void Ekf::controlOpticalFlowFusion()
// optical flow fusion mode selection logic
if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
&& !_control_status.flags.opt_flow // we are not yet using flow data
&& !_inhibit_flow_use)
{
&& !_control_status.flags.opt_flow // we are not yet using flow data
&& !_inhibit_flow_use) {
// If the heading is valid and use is not inhibited , start using optical flow aiding
if (_control_status.flags.yaw_align) {
// set the flag and reset the fusion timeout
@ -472,7 +481,9 @@ void Ekf::controlOpticalFlowFusion()
}
}
} else if (_control_status.flags.opt_flow && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)10e6)) {
} else if (_control_status.flags.opt_flow
&& (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)10e6)) {
stopFlowFusion();
}
}
@ -483,9 +494,9 @@ void Ekf::updateOnGroundMotionForOpticalFlowChecks()
const float accel_norm = _accel_vec_filt.norm();
const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
|| (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
|| (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
|| (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively
|| (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
|| (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
|| (_R_to_earth(2, 2) < cosf(math::radians(30.0f)))); // tilted excessively
if (motion_is_excessive) {
_time_bad_motion_us = _imu_sample_delayed.time_us;
@ -536,24 +547,29 @@ void Ekf::controlGpsFusion()
}
} else if (_control_status.flags.gps
&& (!(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.yaw_align)) {
&& (!(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.yaw_align)) {
stopGpsFusion();
}
// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
if (_control_status.flags.gps && gps_checks_failing && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
stopGpsFusion();
// Reset position state to external vision if we are going to use absolute values
if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
resetHorizontalPosition();
}
_warning_events.flags.gps_quality_poor = true;
ECL_WARN("GPS quality poor - stopping use");
}
// handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before
// we can start using GPS
const bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset && isTimedOut(_ekfgsf_yaw_reset_time, 5000000);
const bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset
&& isTimedOut(_ekfgsf_yaw_reset_time, 5000000);
if (align_yaw_using_gsf) {
if (resetYawToEKFGSF()) {
_ekfgsf_yaw_reset_time = _time_last_imu;
@ -566,9 +582,9 @@ void Ekf::controlGpsFusion()
// We are relying on aiding to constrain drift so after a specified time
// with no aiding we need to do something
bool do_vel_pos_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
// We haven't had an absolute position fix for a longer time so need to do something
do_vel_pos_reset = do_vel_pos_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
@ -604,6 +620,7 @@ void Ekf::controlGpsFusion()
(_time_last_hor_pos_fuse > _time_last_on_ground_us);
bool is_yaw_failure = false;
if ((recent_takeoff_nav_failure || inflight_nav_failure) && _time_last_hor_vel_fuse > 0) {
// Do sanity check to see if the innovation failures is likely caused by a yaw angle error
// by measuring the angle between the velocity estimate and the last velocity observation
@ -720,12 +737,14 @@ void Ekf::controlGpsFusion()
stopGpsFusion();
_warning_events.flags.gps_data_stopped = true;
ECL_WARN("GPS data stopped");
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6)
&& isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
// Handle the case where we are fusing another position source along GPS,
// stop waiting for GPS after 1 s of lost signal
stopGpsFusion();
_warning_events.flags.gps_data_stopped_using_alternate = true;
ECL_WARN("GPS data stopped, using only EV, OF or air data" );
ECL_WARN("GPS data stopped, using only EV, OF or air data");
}
}
@ -747,10 +766,10 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
const bool is_gps_yaw_data_intermittent = !isRecent(_time_last_gps_yaw_data, 2 * GPS_MAX_INTERVAL);
const bool starting_conditions_passing = continuing_conditions_passing
&& _control_status.flags.tilt_align
&& gps_checks_passing
&& !is_gps_yaw_data_intermittent
&& !_gps_hgt_intermittent;
&& _control_status.flags.tilt_align
&& gps_checks_passing
&& !is_gps_yaw_data_intermittent
&& !_gps_hgt_intermittent;
_time_last_gps_yaw_data = _time_last_imu;
@ -779,6 +798,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
stopGpsYawFusion();
}
// TODO: should we give a new reset credit when the fusion does not fail for some time?
}
@ -788,7 +808,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
}
} else {
if (starting_conditions_passing) {
if (starting_conditions_passing) {
// Try to activate GPS yaw fusion
if (resetYawToGps()) {
_control_status.flags.yaw_align = true;
@ -798,8 +818,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
}
}
} else if (_control_status.flags.gps_yaw
&& isTimedOut(_time_last_gps_yaw_data, _params.reset_timeout_max)) {
} else if (_control_status.flags.gps_yaw && isTimedOut(_time_last_gps_yaw_data, _params.reset_timeout_max)) {
// No yaw data in the message anymore. Stop until it comes back.
stopGpsYawFusion();
}
@ -834,8 +853,8 @@ void Ekf::controlHeightSensorTimeouts()
if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
bool request_height_reset = false;
const char* failing_height_source = nullptr;
const char* new_height_source = nullptr;
const char *failing_height_source = nullptr;
const char *new_height_source = nullptr;
if (_control_status.flags.baro_hgt) {
// check if GPS height is available
@ -847,7 +866,7 @@ void Ekf::controlHeightSensorTimeouts()
// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
const bool reset_to_gps = !_gps_hgt_intermittent &&
((gps_hgt_accurate && !prev_bad_vert_accel) || _baro_hgt_faulty);
((gps_hgt_accurate && !prev_bad_vert_accel) || _baro_hgt_faulty);
if (reset_to_gps) {
// set height sensor health
@ -873,12 +892,11 @@ void Ekf::controlHeightSensorTimeouts()
// check the baro height source for consistency and freshness
const baroSample &baro_init = _baro_buffer.get_newest();
const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9,9)) * sq(_params.baro_innov_gate);
const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate);
// if baro data is acceptable and GPS data is inaccurate, reset height to baro
const bool reset_to_baro = !_baro_hgt_faulty &&
((baro_data_consistent && !gps_hgt_accurate) ||
_gps_hgt_intermittent);
((baro_data_consistent && !gps_hgt_accurate) || _gps_hgt_intermittent);
if (reset_to_baro) {
startBaroHgtFusion();
@ -918,8 +936,8 @@ void Ekf::controlHeightSensorTimeouts()
failing_height_source = "ev";
new_height_source = "ev";
// Fallback to rangefinder data if available
} else if (_range_sensor.isHealthy()) {
// Fallback to rangefinder data if available
setControlRangeHeight();
request_height_reset = true;
failing_height_source = "ev";
@ -957,6 +975,7 @@ void Ekf::checkVerticalAccelerationHealth()
// Don't use stale innovation data.
bool is_inertial_nav_falling = false;
bool are_vertical_pos_and_vel_independant = false;
if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) {
if (isRecent(_vert_vel_fuse_time_us, 1000000)) {
// If vertical position and velocity come from independent sensors then we can
@ -966,6 +985,7 @@ void Ekf::checkVerticalAccelerationHealth()
are_vertical_pos_and_vel_independant = !(using_gps_for_both || using_ev_for_both);
is_inertial_nav_falling |= _vert_vel_innov_ratio > _params.vert_innov_test_lim && _vert_pos_innov_ratio > 0.0f;
is_inertial_nav_falling |= _vert_pos_innov_ratio > _params.vert_innov_test_lim && _vert_vel_innov_ratio > 0.0f;
} else {
// only height sensing available
is_inertial_nav_falling = _vert_pos_innov_ratio > _params.vert_innov_test_lim;
@ -977,17 +997,19 @@ void Ekf::checkVerticalAccelerationHealth()
const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] ||
_imu_sample_delayed.delta_vel_clipping[1] ||
_imu_sample_delayed.delta_vel_clipping[2];
if (is_clipping &&_clip_counter < clip_count_limit) {
if (is_clipping && _clip_counter < clip_count_limit) {
_clip_counter++;
} else if (_clip_counter > 0) {
_clip_counter--;
}
const bool is_clipping_frequently = _clip_counter > 0;
// if vertical velocity and position are independent and agree, then do not require evidence of clipping if
// innovations are large
const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) &&
is_inertial_nav_falling;
const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) && is_inertial_nav_falling;
if (bad_vert_accel) {
_time_bad_vert_accel = _time_last_imu;
@ -1091,21 +1113,27 @@ void Ekf::controlHeightFusion()
if (!_gps_hgt_intermittent && _gps_checks_passed) {
// GPS quality OK
startGpsHgtFusion();
} else if (!_baro_hgt_faulty) {
// Use baro as a fallback
startBaroHgtFusion();
}
} else if (_control_status.flags.baro_hgt && !do_range_aid && !_gps_hgt_intermittent && _gps_checks_passed) {
// In baro fallback mode and GPS has recovered so start using it
startGpsHgtFusion();
}
if (_control_status.flags.gps_hgt && _gps_data_ready) {
fuse_height = true;
} else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
fuse_height = true;
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
fuse_height = true;
}
break;
case VDIST_SENSOR_EV:
@ -1114,18 +1142,21 @@ void Ekf::controlHeightFusion()
if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
fuse_height = true;
setControlEVHeight();
if (!_control_status_prev.flags.rng_hgt) {
resetHeight();
resetHeight();
}
}
if (_control_status.flags.ev_hgt && _ev_data_ready) {
fuse_height = true;
fuse_height = true;
} else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
fuse_height = true;
fuse_height = true;
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
fuse_height = true;
}
fuse_height = true;
}
break;
}
@ -1174,9 +1205,10 @@ void Ekf::controlHeightFusion()
}
}
}
// fuse height information
fuseVerticalPosition(_baro_hgt_innov,baro_hgt_innov_gate,
baro_hgt_obs_var, _baro_hgt_innov_var,_baro_hgt_test_ratio);
fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate,
baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
} else if (_control_status.flags.gps_hgt) {
Vector2f gps_hgt_innov_gate;
@ -1187,23 +1219,23 @@ void Ekf::controlHeightFusion()
// innovation gate size
gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
// fuse height information
fuseVerticalPosition(_gps_pos_innov,gps_hgt_innov_gate,
gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate,
gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
} else if (_control_status.flags.rng_hgt) {
Vector2f rng_hgt_innov_gate;
Vector3f rng_hgt_obs_var;
// use range finder with tilt correction
_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
// observation variance - user parameter defined
rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
+ sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
// innovation gate size
rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
// fuse height information
fuseVerticalPosition(_rng_hgt_innov,rng_hgt_innov_gate,
rng_hgt_obs_var, _rng_hgt_innov_var,_rng_hgt_test_ratio);
fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate,
rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
} else if (_control_status.flags.ev_hgt) {
Vector2f ev_hgt_innov_gate;
@ -1215,8 +1247,8 @@ void Ekf::controlHeightFusion()
// innovation gate size
ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
// fuse height information
fuseVerticalPosition(_ev_pos_innov,ev_hgt_innov_gate,
ev_hgt_obs_var, _ev_pos_innov_var,_ev_pos_test_ratio);
fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate,
ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
}
}
}
@ -1257,6 +1289,7 @@ void Ekf::controlAirDataFusion()
// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
if (_control_status.flags.wind &&
(_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)))) {
_control_status.flags.wind = false;
@ -1291,6 +1324,7 @@ void Ekf::controlBetaFusion()
// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fuson had been enabled externally:
const bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us);
if (beta_fusion_time_triggered &&
_control_status.flags.fuse_beta &&
_control_status.flags.in_air) {
@ -1315,15 +1349,16 @@ void Ekf::controlDragFusion()
!_using_synthetic_position &&
_control_status.flags.in_air &&
!_mag_inhibit_yaw_reset_req) {
if (!_control_status.flags.wind) {
// reset the wind states and covariances when starting drag accel fusion
_control_status.flags.wind = true;
resetWindStates();
resetWindCovariance();
} else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
fuseDrag();
}
if (!_control_status.flags.wind) {
// reset the wind states and covariances when starting drag accel fusion
_control_status.flags.wind = true;
resetWindStates();
resetWindCovariance();
} else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
fuseDrag();
}
}
}
@ -1355,6 +1390,7 @@ void Ekf::controlFakePosFusion()
}
}
_time_last_fake_pos = _time_last_imu;
Vector3f fake_pos_obs_var;
@ -1376,7 +1412,7 @@ void Ekf::controlFakePosFusion()
const Vector2f fake_pos_innov_gate(3.0f, 3.0f);
fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var,
_gps_pos_innov_var, _gps_pos_test_ratio, true);
_gps_pos_innov_var, _gps_pos_test_ratio, true);
}
} else {
@ -1398,7 +1434,7 @@ void Ekf::controlAuxVelFusion()
_last_vel_obs_var = _aux_vel_innov_var;
fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,
_aux_vel_innov_var, _aux_vel_test_ratio);
_aux_vel_innov_var, _aux_vel_test_ratio);
// Can be enabled after bit for this is added to EKF_AID_MASK
// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,

102
src/modules/ekf2/EKF/covariance.cpp
View File

@ -140,7 +140,7 @@ void Ekf::predictCovariance()
_accel_vec_filt = alpha * dt_inv * _imu_sample_delayed.delta_vel + beta * _accel_vec_filt;
const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
|| _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
|| _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
const bool do_inhibit_all_axes = (_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)
|| is_manoeuvre_level_high
@ -172,7 +172,7 @@ void Ekf::predictCovariance()
// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
float mag_I_sig;
if (_control_status.flags.mag_3D && (P(16,16) + P(17,17) + P(18,18)) < 0.1f) {
if (_control_status.flags.mag_3D && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) {
mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
} else {
@ -182,7 +182,7 @@ void Ekf::predictCovariance()
// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
float mag_B_sig;
if (_control_status.flags.mag_3D && (P(19,19) + P(20,20) + P(21,21)) < 0.1f) {
if (_control_status.flags.mag_3D && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) {
mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
} else {
@ -210,15 +210,15 @@ void Ekf::predictCovariance()
// These are kinematic states and their error growth is controlled separately by the IMU noise variances
// delta angle bias states
process_noise.slice<3,1>(10,0) = sq(d_ang_bias_sig);
process_noise.slice<3, 1>(10, 0) = sq(d_ang_bias_sig);
// delta_velocity bias states
process_noise.slice<3,1>(13,0) = sq(d_vel_bias_sig);
process_noise.slice<3, 1>(13, 0) = sq(d_vel_bias_sig);
// earth frame magnetic field states
process_noise.slice<3,1>(16,0) = sq(mag_I_sig);
process_noise.slice<3, 1>(16, 0) = sq(mag_I_sig);
// body frame magnetic field states
process_noise.slice<3,1>(19,0) = sq(mag_B_sig);
process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig);
// wind velocity states
process_noise.slice<2,1>(22,0) = sq(wind_vel_sig);
process_noise.slice<2, 1>(22, 0) = sq(wind_vel_sig);
// assign IMU noise variances
// inputs to the system are 3 delta angles and 3 delta velocities
@ -246,11 +246,13 @@ void Ekf::predictCovariance()
dvxVar = sq(dt * BADACC_BIAS_PNOISE);
_fault_status.flags.bad_acc_clipping = true;
}
// delta velocity Y: increase process noise if sample contained any Y axis clipping
if (_imu_sample_delayed.delta_vel_clipping[1]) {
dvyVar = sq(dt * BADACC_BIAS_PNOISE);
_fault_status.flags.bad_acc_clipping = true;
}
// delta velocity Z: increase process noise if sample contained any Z axis clipping
if (_imu_sample_delayed.delta_vel_clipping[2]) {
dvzVar = sq(dt * BADACC_BIAS_PNOISE);
@ -585,9 +587,9 @@ void Ekf::predictCovariance()
// process noise contribution for delta angle states can be very small compared to
// the variances, therefore use algorithm to minimise numerical error
for (unsigned i = 10; i <=12; i++) {
const int index = i-10;
nextP(i,i) = kahanSummation(nextP(i,i), process_noise(i), _delta_angle_bias_var_accum(index));
for (unsigned i = 10; i <= 12; i++) {
const int index = i - 10;
nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index));
}
if (!_accel_bias_inhibit[0]) {
@ -610,7 +612,7 @@ void Ekf::predictCovariance()
// add process noise that is not from the IMU
// process noise contribution for delta velocity states can be very small compared to
// the variances, therefore use algorithm to minimise numerical error
nextP(13,13) = kahanSummation(nextP(13,13), process_noise(13), _delta_vel_bias_var_accum(0));
nextP(13, 13) = kahanSummation(nextP(13, 13), process_noise(13), _delta_vel_bias_var_accum(0));
} else {
nextP.uncorrelateCovarianceSetVariance<1>(13, _prev_dvel_bias_var(0));
@ -640,7 +642,7 @@ void Ekf::predictCovariance()
// add process noise that is not from the IMU
// process noise contribution for delta velocity states can be very small compared to
// the variances, therefore use algorithm to minimise numerical error
nextP(14,14) = kahanSummation(nextP(14,14), process_noise(14), _delta_vel_bias_var_accum(1));
nextP(14, 14) = kahanSummation(nextP(14, 14), process_noise(14), _delta_vel_bias_var_accum(1));
} else {
nextP.uncorrelateCovarianceSetVariance<1>(14, _prev_dvel_bias_var(1));
@ -670,7 +672,7 @@ void Ekf::predictCovariance()
// add process noise that is not from the IMU
// process noise contribution for delta velocity states can be very small compared to
// the variances, therefore use algorithm to minimise numerical error
nextP(15,15) = kahanSummation(nextP(15,15), process_noise(15), _delta_vel_bias_var_accum(2));
nextP(15, 15) = kahanSummation(nextP(15, 15), process_noise(15), _delta_vel_bias_var_accum(2));
} else {
nextP.uncorrelateCovarianceSetVariance<1>(15, _prev_dvel_bias_var(2));
@ -802,7 +804,7 @@ void Ekf::predictCovariance()
// add process noise that is not from the IMU
for (unsigned i = 16; i <= 21; i++) {
nextP(i,i) += process_noise(i);
nextP(i, i) += process_noise(i);
}
}
@ -862,18 +864,18 @@ void Ekf::predictCovariance()
// add process noise that is not from the IMU
for (unsigned i = 22; i <= 23; i++) {
nextP(i,i) += process_noise(i);
nextP(i, i) += process_noise(i);
}
}
// stop position covariance growth if our total position variance reaches 100m
// this can happen if we lose gps for some time
if ((P(7,7) + P(8,8)) > 1e4f) {
if ((P(7, 7) + P(8, 8)) > 1e4f) {
for (uint8_t i = 7; i <= 8; i++) {
for (uint8_t j = 0; j < _k_num_states; j++) {
nextP(i,j) = P(i,j);
nextP(j,i) = P(j,i);
nextP(i, j) = P(i, j);
nextP(j, i) = P(j, i);
}
}
}
@ -881,13 +883,13 @@ void Ekf::predictCovariance()
// covariance matrix is symmetrical, so copy upper half to lower half
for (unsigned row = 1; row < _k_num_states; row++) {
for (unsigned column = 0 ; column < row; column++) {
P(row,column) = P(column,row) = nextP(column,row);
P(row, column) = P(column, row) = nextP(column, row);
}
}
// copy variances (diagonals)
for (unsigned i = 0; i < _k_num_states; i++) {
P(i,i) = nextP(i,i);
P(i, i) = nextP(i, i);
}
// fix gross errors in the covariance matrix and ensure rows and
@ -915,22 +917,22 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
for (int i = 0; i <= 3; i++) {
// quaternion states
P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[0]);
P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[0]);
}
for (int i = 4; i <= 6; i++) {
// NED velocity states
P(i,i) = math::constrain(P(i,i), 1e-6f, P_lim[1]);
P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[1]);
}
for (int i = 7; i <= 9; i++) {
// NED position states
P(i,i) = math::constrain(P(i,i), 1e-6f, P_lim[2]);
P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[2]);
}
for (int i = 10; i <= 12; i++) {
// gyro bias states
P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[3]);
P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[3]);
}
// force symmetry on the quaternion, velocity and position state covariances
@ -953,10 +955,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// Skip the check for the inhibited axis
continue;
}
if (P(stateIndex,stateIndex) > maxStateVar) {
maxStateVar = P(stateIndex,stateIndex);
} else if (P(stateIndex,stateIndex) < minSafeStateVar) {
if (P(stateIndex, stateIndex) > maxStateVar) {
maxStateVar = P(stateIndex, stateIndex);
} else if (P(stateIndex, stateIndex) < minSafeStateVar) {
resetRequired = true;
}
}
@ -972,7 +975,9 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// Skip the check for the inhibited axis
continue;
}
P(stateIndex,stateIndex) = math::constrain(P(stateIndex,stateIndex), minAllowedStateVar, sq(0.1f * CONSTANTS_ONE_G * _dt_ekf_avg));
P(stateIndex, stateIndex) = math::constrain(P(stateIndex, stateIndex), minAllowedStateVar,
sq(0.1f * CONSTANTS_ONE_G * _dt_ekf_avg));
}
// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
@ -987,12 +992,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim
&& ( (down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps)
|| (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel) )
&& ( (down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt)
|| (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt)
|| (down_dvel_bias * _rng_hgt_innov(2) < 0.0f && _control_status.flags.rng_hgt)
|| (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt) ) );
&& ((down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps)
|| (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel))
&& ((down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt)
|| (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt)
|| (down_dvel_bias * _rng_hgt_innov(2) < 0.0f && _control_status.flags.rng_hgt)
|| (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt)));
// record the pass/fail
if (!bad_acc_bias) {
@ -1028,11 +1033,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
} else {
// constrain variances
for (int i = 16; i <= 18; i++) {
P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[5]);
P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[5]);
}
for (int i = 19; i <= 21; i++) {
P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[6]);
P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[6]);
}
// force symmetry
@ -1050,7 +1055,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
} else {
// constrain variances
for (int i = 22; i <= 23; i++) {
P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[7]);
P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[7]);
}
// force symmetry
@ -1062,14 +1067,17 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// if the covariance correction will result in a negative variance, then
// the covariance matrix is unhealthy and must be corrected
bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP) {
bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP)
{
bool healthy = true;
for (int i = 0; i < _k_num_states; i++) {
if (P(i, i) < KHP(i, i)) {
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
healthy = false;
}
}
return healthy;
}
@ -1079,7 +1087,8 @@ void Ekf::resetMagRelatedCovariances()
resetMagCov();
}
void Ekf::resetQuatCov(){
void Ekf::resetQuatCov()
{
zeroQuatCov();
// define the initial angle uncertainty as variances for a rotation vector
@ -1152,14 +1161,15 @@ void Ekf::resetWindCovariance()
// it is safer to remove all existing correlations to other states at this time
P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
P(22,22) = R_TAS*sq(cos_yaw) + R_yaw*sq(-Wx*sin_yaw - Wy*cos_yaw) + initial_wind_var_body_y*sq(sin_yaw);
P(22,23) = R_TAS*sin_yaw*cos_yaw + R_yaw*(-Wx*sin_yaw - Wy*cos_yaw)*(Wx*cos_yaw - Wy*sin_yaw) - initial_wind_var_body_y*sin_yaw*cos_yaw;
P(23,22) = P(22,23);
P(23,23) = R_TAS*sq(sin_yaw) + R_yaw*sq(Wx*cos_yaw - Wy*sin_yaw) + initial_wind_var_body_y*sq(cos_yaw);
P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -
initial_wind_var_body_y * sin_yaw * cos_yaw;
P(23, 22) = P(22, 23);
P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw);
// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
P(22,22) += P(4,4);
P(23,23) += P(5,5);
P(22, 22) += P(4, 4);
P(23, 23) += P(5, 5);
} else {
// without airspeed, start with a small initial uncertainty to improve the initial estimate

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

@ -98,8 +98,10 @@ void Ekf::fuseDrag()
// equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed
// parallel to the rotor disc and mass flow through the rotor disc.
float pred_acc = 0.0f; // predicted drag acceleration
if (axis_index == 0) {
float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_x) {
// Use a combination of bluff body and propeller momentum drag
const float bcoef_inv = 1.0f / _params.bcoef_x;
@ -108,10 +110,12 @@ void Ekf::fuseDrag()
const float airspeed = (_params.bcoef_x / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign - rel_wind_body(0) * mcoef_corrrected;
} else if (using_mcoef) {
// Use propeller momentum drag only
Kacc = fmaxf(1e-1f, mcoef_corrrected);
pred_acc = - rel_wind_body(0) * mcoef_corrrected;
} else if (using_bcoef_x) {
// Use bluff body drag only
// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
@ -120,6 +124,7 @@ void Ekf::fuseDrag()
const float bcoef_inv = 1.0f / _params.bcoef_x;
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign;
} else {
// skip this axis
continue;
@ -165,6 +170,7 @@ void Ekf::fuseDrag()
if (_drag_innov_var(0) < R_ACC) {
return;
}
const float HK32 = Kacc / _drag_innov_var(0);
// Observation Jacobians
@ -208,6 +214,7 @@ void Ekf::fuseDrag()
} else if (axis_index == 1) {
float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_y) {
// Use a combination of bluff body and propeller momentum drag
const float bcoef_inv = 1.0f / _params.bcoef_y;
@ -216,10 +223,12 @@ void Ekf::fuseDrag()
const float airspeed = (_params.bcoef_y / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign - rel_wind_body(1) * mcoef_corrrected;
} else if (using_mcoef) {
// Use propeller momentum drag only
Kacc = fmaxf(1e-1f, mcoef_corrrected);
pred_acc = - rel_wind_body(1) * mcoef_corrrected;
} else if (using_bcoef_y) {
// Use bluff body drag only
// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
@ -228,6 +237,7 @@ void Ekf::fuseDrag()
const float bcoef_inv = 1.0f / _params.bcoef_y;
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign;
} else {
// nothing more to do
return;
@ -273,6 +283,7 @@ void Ekf::fuseDrag()
// calculation is badly conditioned
return;
}
const float HK32 = Kacc / _drag_innov_var(1);
// Observation Jacobians

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

@ -262,12 +262,14 @@ void Ekf::resetHeight()
if (_control_status.flags.rng_hgt) {
// a fallback from any other height source to rangefinder happened
if(!_control_status_prev.flags.rng_hgt) {
if (!_control_status_prev.flags.rng_hgt) {
if (_control_status.flags.in_air && isTerrainEstimateValid()) {
_hgt_sensor_offset = _terrain_vpos;
_hgt_sensor_offset = _terrain_vpos;
} else if (_control_status.flags.in_air) {
_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
} else {
_hgt_sensor_offset = _params.rng_gnd_clearance;
}
@ -729,6 +731,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
// reset altitude
_gps_alt_ref = altitude;
resetVerticalPositionTo(_gps_alt_ref - current_alt);
} else {
// reset altitude
_gps_alt_ref = altitude;
@ -1042,9 +1045,9 @@ void Ekf::uncorrelateQuatFromOtherStates()
void Ekf::update_deadreckoning_status()
{
const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel)
&& (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
|| isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
|| isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max));
&& (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
|| isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
|| isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max));
const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max);
const bool airDataAiding = _control_status.flags.wind &&
isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) &&
@ -1316,8 +1319,7 @@ void Ekf::updateBaroHgtOffset()
const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
// apply a 10 second first order low pass filter to baro offset
const float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(2) -
_baro_hgt_offset);
const float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(2) - _baro_hgt_offset);
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
}
}
@ -1340,19 +1342,22 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() const
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
// rotate measurement into correct earth frame if required
switch(_ev_sample_delayed.vel_frame) {
case velocity_frame_t::BODY_FRAME_FRD:
vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
break;
case velocity_frame_t::LOCAL_FRAME_FRD:
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
if (_params.fusion_mode & MASK_ROTATE_EV)
{
vel = _R_ev_to_ekf *_ev_sample_delayed.vel - vel_offset_earth;
} else {
vel = _ev_sample_delayed.vel - vel_offset_earth;
}
break;
switch (_ev_sample_delayed.vel_frame) {
case velocity_frame_t::BODY_FRAME_FRD:
vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
break;
case velocity_frame_t::LOCAL_FRAME_FRD:
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
if (_params.fusion_mode & MASK_ROTATE_EV) {
vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth;
} else {
vel = _ev_sample_delayed.vel - vel_offset_earth;
}
break;
}
return vel;
@ -1363,17 +1368,17 @@ Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
Matrix3f ev_vel_cov = _ev_sample_delayed.velCov;
// rotate measurement into correct earth frame if required
switch(_ev_sample_delayed.vel_frame) {
case velocity_frame_t::BODY_FRAME_FRD:
ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
break;
switch (_ev_sample_delayed.vel_frame) {
case velocity_frame_t::BODY_FRAME_FRD:
ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
break;
case velocity_frame_t::LOCAL_FRAME_FRD:
if(_params.fusion_mode & MASK_ROTATE_EV)
{
ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
}
break;
case velocity_frame_t::LOCAL_FRAME_FRD:
if (_params.fusion_mode & MASK_ROTATE_EV) {
ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
}
break;
}
return ev_vel_cov.diag();

4
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -520,7 +520,9 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
_obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length);
if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length) || !_output_vert_buffer.allocate(_imu_buffer_length)) {
if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length)
|| !_output_vert_buffer.allocate(_imu_buffer_length)) {
printBufferAllocationFailed("IMU and output");
return false;
}

5
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -113,6 +113,7 @@ public:
} else {
_time_last_in_air = _time_last_imu;
}
_control_status.flags.in_air = in_air;
}
@ -248,8 +249,8 @@ public:
// Getter for the baro sample on the delayed time horizon
const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
const bool& global_origin_valid() const { return _NED_origin_initialised; }
const map_projection_reference_s& global_origin() const { return _pos_ref; }
const bool &global_origin_valid() const { return _NED_origin_initialised; }
const map_projection_reference_s &global_origin() const { return _pos_ref; }
void print_status();

4
src/modules/ekf2/EKF/gps_checks.cpp
View File

@ -215,8 +215,8 @@ bool Ekf::gps_is_good(const gps_message &gps)
// Check the magnitude of the filtered horizontal GPS velocity
const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned.xy()),
-10.0f * _params.req_hdrift,
10.0f * _params.req_hdrift);
-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_drift_metrics[2] = _gps_velNE_filt.norm();
_gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift);

8
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
View File

@ -66,7 +66,7 @@ void Ekf::fuseGpsYaw()
}
// calculate predicted antenna yaw angle
const float predicted_hdg = atan2f(ant_vec_ef(1),ant_vec_ef(0));
const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0));
// using magnetic heading process noise
// TODO extend interface to use yaw uncertainty provided by GPS if available
@ -83,6 +83,7 @@ void Ekf::fuseGpsYaw()
const float HK7 = ecl::powf(q0, 2) - ecl::powf(q3, 2);
const float HK8 = HK4*(HK5 - HK6 + HK7);
const float HK9 = HK3 - HK8;
if (fabsf(HK9) < 1e-3f) {
return;
}
@ -100,6 +101,7 @@ void Ekf::fuseGpsYaw()
const float HK18 = 2/HK17;
// const float HK19 = 1.0F/(-HK3 + HK8);
const float HK19_inverse = -HK3 + HK8;
if (fabsf(HK19_inverse) < 1e-6f) {
return;
}
@ -122,7 +124,7 @@ void Ekf::fuseGpsYaw()
_heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
if (_heading_innov_var < R_YAW) {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_hdg = true;
// we reinitialise the covariance matrix and abort this fusion step
@ -132,7 +134,7 @@ void Ekf::fuseGpsYaw()
}
_fault_status.flags.bad_hdg = false;
const float HK32 = HK18/_heading_innov_var;
const float HK32 = HK18 / _heading_innov_var;
// calculate the innovation and define the innovation gate
const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);

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

@ -50,11 +50,11 @@ void Ekf::controlMagFusion()
// yaw fusion is run selectively to enable yaw gyro bias learning when stationary on
// ground and to prevent uncontrolled yaw variance growth
if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
if (noOtherYawAidingThanMag())
{
if (noOtherYawAidingThanMag()) {
_is_yaw_fusion_inhibited = true;
fuseHeading();
}
return;
}
@ -81,12 +81,14 @@ void Ekf::controlMagFusion()
// there are large external disturbances or the more accurate 3-axis fusion
switch (_params.mag_fusion_type) {
default:
/* fallthrough */
case MAG_FUSE_TYPE_AUTO:
selectMagAuto();
break;
case MAG_FUSE_TYPE_INDOOR:
/* fallthrough */
case MAG_FUSE_TYPE_HEADING:
startMagHdgFusion();
@ -169,8 +171,12 @@ void Ekf::runInAirYawReset()
if (_mag_yaw_reset_req && isYawResetAuthorized()) {
bool has_realigned_yaw = false;
if (canRealignYawUsingGps()) { has_realigned_yaw = realignYawGPS(); }
else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); }
if (canRealignYawUsingGps()) {
has_realigned_yaw = realignYawGPS();
} else if (canResetMagHeading()) {
has_realigned_yaw = resetMagHeading(_mag_lpf.getState());
}
if (has_realigned_yaw) {
_mag_yaw_reset_req = false;
@ -266,6 +272,7 @@ void Ekf::checkMagDeclRequired()
void Ekf::checkMagInhibition()
{
_is_yaw_fusion_inhibited = shouldInhibitMag();
if (!_is_yaw_fusion_inhibited) {
_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
}
@ -286,8 +293,8 @@ bool Ekf::shouldInhibitMag() const
const bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR);
const bool heading_not_required_for_navigation = !_control_status.flags.gps
&& !_control_status.flags.ev_pos
&& !_control_status.flags.ev_vel;
&& !_control_status.flags.ev_pos
&& !_control_status.flags.ev_vel;
return (user_selected && heading_not_required_for_navigation)
|| isStrongMagneticDisturbance();
@ -297,8 +304,8 @@ void Ekf::checkMagFieldStrength()
{
if (_params.check_mag_strength) {
_control_status.flags.mag_field_disturbed = _NED_origin_initialised
? !isMeasuredMatchingGpsMagStrength()
: !isMeasuredMatchingAverageMagStrength();
? !isMeasuredMatchingGpsMagStrength()
: !isMeasuredMatchingAverageMagStrength();
} else {
_control_status.flags.mag_field_disturbed = false;
@ -323,13 +330,14 @@ bool Ekf::isMeasuredMatchingAverageMagStrength() const
bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate)
{
return (measured >= expected - gate)
&& (measured <= expected + gate);
&& (measured <= expected + gate);
}
void Ekf::runMagAndMagDeclFusions()
{
if (_control_status.flags.mag_3D) {
run3DMagAndDeclFusions();
} else if (_control_status.flags.mag_hdg) {
fuseHeading();
}
@ -351,6 +359,7 @@ void Ekf::run3DMagAndDeclFusions()
// declination angle at a higher uncertainty to allow some learning of
// declination angle over time.
fuseMag();
if (_control_status.flags.mag_dec) {
fuseDeclination(0.5f);
}
@ -359,10 +368,10 @@ void Ekf::run3DMagAndDeclFusions()
bool Ekf::otherHeadingSourcesHaveStopped()
{
// detect rising edge of noOtherYawAidingThanMag()
bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev;
// detect rising edge of noOtherYawAidingThanMag()
bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev;
_non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag();
_non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag();
return result;
return result;
}

59
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -436,6 +436,7 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
const float SA2 = SA0*q0 + SA1*q1;
const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SA4, SA5_inv;
if (sq(SA3) > 1e-6f) {
SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1;
@ -448,6 +449,7 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
const float SB2 = SB0*q3 + SB1*q2;
const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SB3, SB5_inv;
if (sq(SB2) > 1e-6f) {
SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1;
@ -484,8 +486,10 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
// calculate the yaw innovation and wrap to the interval between +-pi
float innovation;
if (zero_innovation) {
innovation = 0.0f;
} else {
innovation = wrap_pi(atan2f(_R_to_earth(1, 0), _R_to_earth(0, 0)) - measurement);
}
@ -516,6 +520,7 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
const float SA2 = SA0*q0 - SA1*q1;
const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
float SA4, SA5_inv;
if (sq(SA3) > 1e-6f) {
SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1;
@ -528,6 +533,7 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
const float SB2 = -SB0*q3 + SB1*q2;
const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
float SB3, SB5_inv;
if (sq(SB2) > 1e-6f) {
SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1;
@ -563,8 +569,10 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
}
float innovation;
if (zero_innovation) {
innovation = 0.0f;
} else {
// calculate the the innovation and wrap to the interval between +-pi
innovation = wrap_pi(atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)) - measurement);
@ -578,16 +586,18 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
}
// update quaternion states and covariances using the yaw innovation, yaw observation variance and yaw Jacobian
void Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma, const Vector4f& yaw_jacobian)
void Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma,
const Vector4f &yaw_jacobian)
{
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
// calculate the innovation variance
_heading_innov_var = variance;
for (unsigned row = 0; row <= 3; row++) {
float tmp = 0.0f;
for (uint8_t col = 0; col <= 3; col++) {
tmp += P(row,col) * yaw_jacobian(col);
tmp += P(row, col) * yaw_jacobian(col);
}
_heading_innov_var += yaw_jacobian(row) * tmp;
@ -617,7 +627,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
for (uint8_t row = 0; row <= 15; row++) {
for (uint8_t col = 0; col <= 3; col++) {
Kfusion(row) += P(row,col) * yaw_jacobian(col);
Kfusion(row) += P(row, col) * yaw_jacobian(col);
}
Kfusion(row) *= heading_innov_var_inv;
@ -626,7 +636,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
if (_control_status.flags.wind) {
for (uint8_t row = 22; row <= 23; row++) {
for (uint8_t col = 0; col <= 3; col++) {
Kfusion(row) += P(row,col) * yaw_jacobian(col);
Kfusion(row) += P(row, col) * yaw_jacobian(col);
}
Kfusion(row) *= heading_innov_var_inv;
@ -680,11 +690,11 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
KH[3] = Kfusion(row) * yaw_jacobian(3);
for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[0] * P(0,column);
tmp += KH[1] * P(1,column);
tmp += KH[2] * P(2,column);
tmp += KH[3] * P(3,column);
KHP(row,column) = tmp;
float tmp = KH[0] * P(0, column);
tmp += KH[1] * P(1, column);
tmp += KH[2] * P(2, column);
tmp += KH[3] * P(3, column);
KHP(row, column) = tmp;
}
}
@ -711,6 +721,7 @@ void Ekf::fuseHeading()
// Calculate the observation variance
float R_YAW;
if (_control_status.flags.mag_hdg) {
// using magnetic heading tuning parameter
R_YAW = sq(_params.mag_heading_noise);
@ -747,6 +758,7 @@ void Ekf::fuseHeading()
// handle special case where yaw measurement is unavailable
bool fuse_zero_innov = false;
if (_is_yaw_fusion_inhibited) {
// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
// conditioned covariance matrix developing over time.
@ -755,12 +767,14 @@ void Ekf::fuseHeading()
// unconstrained quaternion variance growth and record the predicted heading
// to use as an observation when movement ceases.
// TODO a better way of determining when this is necessary
const float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
if (sumQuatVar > _params.quat_max_variance) {
fuse_zero_innov = true;
R_YAW = 0.25f;
}
_last_static_yaw = predicted_hdg;
} else {
@ -770,6 +784,7 @@ void Ekf::fuseHeading()
measured_hdg = _last_static_yaw;
}
} else {
_last_static_yaw = predicted_hdg;
@ -798,6 +813,7 @@ void Ekf::fuseHeading()
// handle special case where yaw measurement is unavailable
bool fuse_zero_innov = false;
if (_is_yaw_fusion_inhibited) {
// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
// conditioned covariance matrix developing over time.
@ -806,12 +822,14 @@ void Ekf::fuseHeading()
// unconstrained quaterniion variance growth and record the predicted heading
// to use as an observation when movement ceases.
// TODO a better way of determining when this is necessary
const float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
if (sumQuatVar > _params.quat_max_variance) {
fuse_zero_innov = true;
R_YAW = 0.25f;
}
_last_static_yaw = predicted_hdg;
} else {
@ -821,6 +839,7 @@ void Ekf::fuseHeading()
measured_hdg = _last_static_yaw;
}
} else {
_last_static_yaw = predicted_hdg;
@ -848,6 +867,7 @@ void Ekf::fuseDeclination(float decl_sigma)
// calculation is badly conditioned close to +-90 deg declination
return;
}
const float HK0 = ecl::powf(magN, -2);
const float HK1 = HK0*ecl::powf(magE, 2) + 1.0F;
const float HK2 = 1.0F/HK1;
@ -859,6 +879,7 @@ void Ekf::fuseDeclination(float decl_sigma)
const float HK8 = HK5*P(16,16) - P(16,17);
const float innovation_variance = -HK0*HK6*HK7 + HK7*HK8*magE/ecl::powf(magN, 3) + R_DECL;
float HK9;
if (innovation_variance > R_DECL) {
HK9 = HK4/innovation_variance;
} else {
@ -875,6 +896,7 @@ void Ekf::fuseDeclination(float decl_sigma)
// Calculate the Kalman gains
Vector24f Kfusion;
for (unsigned row = 0; row <= 15; row++) {
Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17));
}
@ -903,15 +925,17 @@ void Ekf::limitDeclination()
// set to 50% of the horizontal strength from geo tables if location is known
float decl_reference;
float h_field_min = 0.001f;
if (_params.mag_declination_source & MASK_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)) {
decl_reference = _mag_declination_gps;
h_field_min = fmaxf(h_field_min , 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
} else {
decl_reference = math::radians(_params.mag_declination_deg);
}
} else {
// always use the parameter value
decl_reference = math::radians(_params.mag_declination_deg);
@ -920,18 +944,21 @@ void Ekf::limitDeclination()
// do not allow the horizontal field length to collapse - this will make the declination fusion badly conditioned
// and can result in a reversal of the NE field states which the filter cannot recover from
// apply a circular limit
float h_field = sqrtf(_state.mag_I(0)*_state.mag_I(0) + _state.mag_I(1)*_state.mag_I(1));
float h_field = sqrtf(_state.mag_I(0) * _state.mag_I(0) + _state.mag_I(1) * _state.mag_I(1));
if (h_field < h_field_min) {
if (h_field > 0.001f * h_field_min) {
const float h_scaler = h_field_min / h_field;
_state.mag_I(0) *= h_scaler;
_state.mag_I(1) *= h_scaler;
} else {
// too small to scale radially so set to expected value
const float mag_declination = getMagDeclination();
_state.mag_I(0) = 2.0f * h_field_min * cosf(mag_declination);
_state.mag_I(1) = 2.0f * h_field_min * sinf(mag_declination);
}
h_field = h_field_min;
}
@ -939,17 +966,19 @@ void Ekf::limitDeclination()
constexpr float decl_tolerance = 0.5f;
const float decl_max = decl_reference + decl_tolerance;
const float decl_min = decl_reference - decl_tolerance;
const float decl_estimate = atan2f(_state.mag_I(1) , _state.mag_I(0));
const float decl_estimate = atan2f(_state.mag_I(1), _state.mag_I(0));
if (decl_estimate > decl_max) {
_state.mag_I(0) = h_field * cosf(decl_max);
_state.mag_I(1) = h_field * sinf(decl_max);
} else if (decl_estimate < decl_min) {
_state.mag_I(0) = h_field * cosf(decl_min);
_state.mag_I(1) = h_field * sinf(decl_min);
}
}
float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f& mag_meas, const Vector3f& mag_earth_predicted)
float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)
{
// theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge
// of the earth magnetic field vector at the current location

5
src/modules/ekf2/EKF/optflow_fusion.cpp
View File

@ -240,7 +240,7 @@ void Ekf::fuseOptFlow()
float test_ratio[2];
test_ratio[0] = sq(_flow_innov(0)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(0));
test_ratio[1] = sq(_flow_innov(1)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(1));
_optflow_test_ratio = math::max(test_ratio[0],test_ratio[1]);
_optflow_test_ratio = math::max(test_ratio[0], test_ratio[1]);
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
if (test_ratio[obs_index] > 1.0f) {
@ -262,6 +262,7 @@ void Ekf::fuseOptFlow()
// fuse observation axes sequentially
SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
Vector24f Kfusion; // Optical flow Kalman gains
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
// calculate observation Jocobians and Kalman gains
@ -339,7 +340,7 @@ bool Ekf::calcOptFlowBodyRateComp()
return false;
}
const bool use_flow_sensor_gyro = PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2));
const bool use_flow_sensor_gyro = PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2));
if (use_flow_sensor_gyro) {

8
src/modules/ekf2/EKF/sideslip_fusion.cpp
View File

@ -137,7 +137,8 @@ void Ekf::fuseSideslip()
_fault_status.flags.bad_sideslip = true;
// if we are getting aiding from other sources, warn and reset the wind states and covariances only
const char* action_string = nullptr;
const char *action_string = nullptr;
if (update_wind_only) {
resetWindStates();
resetWindCovariance();
@ -148,12 +149,14 @@ void Ekf::fuseSideslip()
_state.wind_vel.setZero();
action_string = "full";
}
ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string);
return;
}
_fault_status.flags.bad_sideslip = false;
const float HK52 = HK16/_beta_innov_var;
const float HK52 = HK16 / _beta_innov_var;
// Calculate predicted sideslip angle and innovation using small angle approximation
_beta_innov = rel_wind_body(1) / rel_wind_body(0);
@ -184,6 +187,7 @@ void Ekf::fuseSideslip()
// Calculate Kalman gains
Vector24f Kfusion;
if (!update_wind_only) {
Kfusion(0) = HK38*HK52;

10
src/modules/ekf2/EKF/terrain_estimator.cpp
View File

@ -142,9 +142,9 @@ void Ekf::fuseHagl()
_hagl_innov = pred_hagl - meas_hagl;
// calculate the observation variance adding the variance of the vehicles own height uncertainty
const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f)
+ sq(_params.range_noise)
+ sq(_params.range_noise_scaler * _range_sensor.getRange());
const float obs_variance = fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f)
+ sq(_params.range_noise)
+ sq(_params.range_noise_scaler * _range_sensor.getRange());
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
@ -167,6 +167,7 @@ void Ekf::fuseHagl()
} else {
// If we have been rejecting range data for too long, reset to measurement
const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
if (isTimedOut(_time_last_hagl_fuse, timeout)) {
_terrain_vpos = _state.pos(2) + meas_hagl;
_terrain_var = obs_variance;
@ -290,8 +291,7 @@ void Ekf::updateTerrainValidity()
_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));
_hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl()
&& recent_range_fusion
&& (_time_last_fake_hagl_fuse != _time_last_hagl_fuse);
&& recent_range_fusion && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse);
_hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl() && recent_flow_for_terrain_fusion;
}