Skip to content

Commit

Permalink
ekf2: EKFGSF always run
Browse files Browse the repository at this point in the history 
 - if not in air the accel noise is doubled
 - if landed don't init unless GPS velocity is non-negligible
 - when inactive continue seeding with EKF gyro bias
 - reset yaw estimator if GPS fusion is stopped
  • Loading branch information
@dagar
dagar committed Oct 18, 2023
1 parent bdaf0ac commit b66a826
Show file tree
Hide file tree
Showing 3 changed files with 125 additions and 91 deletions.
178 changes: 99 additions & 79 deletions src/modules/ekf2/EKF/EKFGSF_yaw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,60 +39,79 @@

EKFGSF_yaw::EKFGSF_yaw()
{
initialiseEKFGSF();
reset();
}

void EKFGSF_yaw::update(const imuSample &imu_sample,
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
const Vector3f &imu_gyro_bias) // estimated rate gyro bias (rad/sec)
void EKFGSF_yaw::reset()
{
// to reduce effect of vibration, filter using an LPF whose time constant is 1/10 of the AHRS tilt correction time constant
const float filter_coef = fminf(10.f * imu_sample.delta_vel_dt * _tilt_gain, 1.f);
const Vector3f accel = imu_sample.delta_vel / fmaxf(imu_sample.delta_vel_dt, 0.001f);
_ahrs_accel = _ahrs_accel * (1.f - filter_coef) + accel * filter_coef;
_vel_data_updated = false;
_ekf_gsf_vel_fuse_started = false;

_gsf_yaw_variance = INFINITY;
}

void EKFGSF_yaw::update(const imuSample &imu_sample, bool in_air)
{
const Vector3f accel = imu_sample.delta_vel / imu_sample.delta_vel_dt;

if (imu_sample.delta_vel_dt > 0.001f) {
// to reduce effect of vibration, filter using an LPF whose time constant is 1/10 of the AHRS tilt correction time constant
const float filter_coef = fminf(10.f * imu_sample.delta_vel_dt * _tilt_gain, 1.f);
_ahrs_accel = _ahrs_accel * (1.f - filter_coef) + accel * filter_coef;

} else {
return;
}

// Initialise states first time
if (!_ahrs_ekf_gsf_tilt_aligned) {
// check for excessive acceleration to reduce likelihood of large initial roll/pitch errors
// due to vehicle movement
const float accel_norm_sq = accel.norm_squared();
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));
const float accel_lpf_norm_sq = _ahrs_accel.norm_squared();

static constexpr float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
static constexpr 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))
&& (accel_lpf_norm_sq > sq(lower_accel_limit)) && (accel_lpf_norm_sq < sq(upper_accel_limit));

if (ok_to_align) {
ahrsAlignTilt(imu_sample.delta_vel);
_ahrs_ekf_gsf_tilt_aligned = true;
}

return;
} else {
return;
}
}

// calculate common values used by the AHRS complementary filter models
_ahrs_accel_norm = _ahrs_accel.norm();

// AHRS prediction cycle for each model - this always runs
_ahrs_accel_fusion_gain = ahrsCalcAccelGain();
// we don't start running the EKF part of the algorithm until there are regular velocity observations
if (!_ekf_gsf_vel_fuse_started && _vel_data_updated) {

for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
predictEKF(model_index, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt);
}
_vel_data_updated = false;

// The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference
if (run_EKF && _vel_data_updated) {
if (!_ekf_gsf_vel_fuse_started) {
initialiseEKFGSF(_vel_NE, _vel_accuracy);
initialiseEKFGSF(_vel_NE, _vel_accuracy);

// Initialise to gyro bias estimate from main filter because there could be a large
// uncorrected rate gyro bias error about the gravity vector
ahrsAlignYaw(imu_gyro_bias);
ahrsAlignYaw();

// don't start until in air or velocity is not negligible
if (in_air || _vel_NE.longerThan(_vel_accuracy)) {
_ekf_gsf_vel_fuse_started = true;
}
}

for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
predictEKF(model_index, imu_sample.delta_ang, imu_sample.delta_ang_dt,
imu_sample.delta_vel, imu_sample.delta_vel_dt, in_air);
}

if (_ekf_gsf_vel_fuse_started) {
if (_vel_data_updated) {
_vel_data_updated = false;

} else {
bool bad_update = false;

for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
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, _vel_NE, _vel_accuracy)) {
bad_update = true;
Expand Down Expand Up @@ -122,39 +141,32 @@ void EKFGSF_yaw::update(const imuSample &imu_sample,

} else {
// all weights have collapsed due to excessive innovation variances so reset filters
initialiseEKFGSF(_vel_NE, _vel_accuracy);
reset();
}
}
}

} else if (_ekf_gsf_vel_fuse_started && !run_EKF) {
// wait to fly again
_ekf_gsf_vel_fuse_started = false;
}

// Calculate a composite yaw vector as a weighted average of the states for each model.
// 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;
// Calculate a composite yaw vector as a weighted average of the states for each model.
// 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));
}
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;
// 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);
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);
}
}

// prevent the same velocity data being used more than once
_vel_data_updated = false;
}

void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt)
Expand All @@ -165,11 +177,16 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_an
const Dcmf R_to_body = _ahrs_ekf_gsf[model_index].R.transpose();
const Vector3f gravity_direction_bf = R_to_body.col(2);

const float ahrs_accel_norm = _ahrs_accel.norm();

// gain from accel vector tilt error to rate gyro correction used by AHRS calculation
const float ahrs_accel_fusion_gain = ahrsCalcAccelGain();

// 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;
Vector3f tilt_correction{};

if (_ahrs_accel_fusion_gain > 0.0f) {
if (ahrs_accel_fusion_gain > 0.f) {

Vector3f accel = _ahrs_accel;

Expand All @@ -182,7 +199,7 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_an
accel -= centripetal_accel_bf;
}

tilt_correction = (gravity_direction_bf % accel) * _ahrs_accel_fusion_gain / _ahrs_accel_norm;
tilt_correction = (gravity_direction_bf % accel) * ahrs_accel_fusion_gain / ahrs_accel_norm;
}

// Gyro bias estimation
Expand All @@ -191,13 +208,13 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_an

if (spin_rate < math::radians(10.f)) {
_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);
Expand Down Expand Up @@ -235,22 +252,19 @@ void EKFGSF_yaw::ahrsAlignTilt(const Vector3f &delta_vel)
}
}

void EKFGSF_yaw::ahrsAlignYaw(const Vector3f &imu_gyro_bias)
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;
const float yaw = wrap_pi(_ekf_gsf[model_index].X(2));
R = updateYawInRotMat(yaw, R);

_ahrs_ekf_gsf[model_index].aligned = true;

_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
const float yaw = wrap_pi(_ekf_gsf[model_index].X(2));
const Dcmf R = _ahrs_ekf_gsf[model_index].R;
_ahrs_ekf_gsf[model_index].R = updateYawInRotMat(yaw, R);
}
}

void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt,
const Vector3f &delta_vel, const float delta_vel_dt)
const Vector3f &delta_vel, const float delta_vel_dt, bool in_air)
{
// generate an attitude reference using IMU data
ahrsPredict(model_index, delta_ang, delta_ang_dt);
Expand All @@ -270,11 +284,15 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang
const float dvx = del_vel_NED(0) * cos_yaw + del_vel_NED(1) * sin_yaw;
const float dvy = - del_vel_NED(0) * sin_yaw + del_vel_NED(1) * cos_yaw;

// Use fixed values for delta velocity and delta angle process noise variances
const float d_vel_var = sq(_accel_noise * delta_vel_dt);
// delta velocity process noise double if we're not in air
const float accel_noise = in_air ? _accel_noise : 2.f * _accel_noise;
const float d_vel_var = sq(accel_noise * delta_vel_dt);

// Use fixed values for delta angle process noise variances
const float d_ang_var = sq(_gyro_noise * delta_ang_dt);

sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, Vector2f(dvx, dvy), d_vel_var, d_ang_var, &_ekf_gsf[model_index].P);
sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P,
Vector2f(dvx, dvy), d_vel_var, d_ang_var, &_ekf_gsf[model_index].P);

// covariance matrix is symmetrical, so copy upper half to lower half
_ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1);
Expand All @@ -293,7 +311,6 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang
_ekf_gsf[model_index].X(1) += del_vel_NED(1);
}

// Update EKF states and covariance for specified model index using velocity measurement
bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, const float vel_accuracy)
{
// set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum
Expand Down Expand Up @@ -365,20 +382,21 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co
void EKFGSF_yaw::initialiseEKFGSF(const Vector2f &vel_NE, const float vel_accuracy)
{
_gsf_yaw = 0.0f;
_ekf_gsf_vel_fuse_started = false;
_gsf_yaw_variance = sq(M_PI_F / 2.f);
_model_weights.setAll(1.0f / (float)N_MODELS_EKFGSF); // All filter models start with the same weight

memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));
const float yaw_increment = 2.0f * M_PI_F / (float)N_MODELS_EKFGSF;
const float yaw_increment = 2.f * M_PI_F / (float)N_MODELS_EKFGSF;

for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
_ekf_gsf[model_index] = {};

// evenly space initial yaw estimates in the region between +-Pi
_ekf_gsf[model_index].X(2) = -M_PI_F + (0.5f * yaw_increment) + ((float)model_index * yaw_increment);

// 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(fmaxf(vel_accuracy, 0.01f));
_ekf_gsf[model_index].P(1, 1) = _ekf_gsf[model_index].P(0, 0);

Expand Down Expand Up @@ -426,11 +444,13 @@ float EKFGSF_yaw::ahrsCalcAccelGain() const
float attenuation = 2.f;
const bool centripetal_accel_compensation_enabled = PX4_ISFINITE(_true_airspeed) && (_true_airspeed > FLT_EPSILON);

if (centripetal_accel_compensation_enabled && (_ahrs_accel_norm > CONSTANTS_ONE_G)) {
const float ahrs_accel_norm = _ahrs_accel.norm();

if (centripetal_accel_compensation_enabled && (ahrs_accel_norm > CONSTANTS_ONE_G)) {
attenuation = 1.f;
}

const float delta_accel_g = (_ahrs_accel_norm - CONSTANTS_ONE_G) / CONSTANTS_ONE_G;
const float delta_accel_g = (ahrs_accel_norm - CONSTANTS_ONE_G) / CONSTANTS_ONE_G;
return _tilt_gain * sq(1.f - math::min(attenuation * fabsf(delta_accel_g), 1.f));
}

Expand All @@ -454,7 +474,7 @@ Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g)
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;
ret.row(r) *= rowLengthInv;
}
}

Expand Down
28 changes: 18 additions & 10 deletions src/modules/ekf2/EKF/EKFGSF_yaw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,16 +59,25 @@ class EKFGSF_yaw
EKFGSF_yaw();

// 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
const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec)
void update(const imuSample &imu_sample, bool in_air = false);

void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s)
float accuracy); // 1-sigma accuracy of velocity measurement (m/s)


void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; }

void setGyroBias(const Vector3f &imu_gyro_bias)
{
// Initialise to gyro bias estimate from main filter because there could be a large
// uncorrected rate gyro bias error about the gravity vector
if (!_ahrs_ekf_gsf_tilt_aligned || !_ekf_gsf_vel_fuse_started) {
// init gyro bias for each model
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
}
}
}

// get solution data for logging
bool getLogData(float *yaw_composite,
float *yaw_composite_variance,
Expand All @@ -82,6 +91,8 @@ class EKFGSF_yaw
float getYaw() const { return _gsf_yaw; }
float getYawVar() const { return _gsf_yaw_variance; }

void reset();

private:

// Parameters - these could be made tuneable
Expand All @@ -96,13 +107,10 @@ class EKFGSF_yaw
struct {
Dcmf R{matrix::eye<float, 3>()}; // matrix that rotates a vector from body to earth frame
Vector3f gyro_bias{}; // gyro bias learned and used by the quaternion calculation
bool aligned{false}; // true when AHRS has been aligned
} _ahrs_ekf_gsf[N_MODELS_EKFGSF] {};

bool _ahrs_ekf_gsf_tilt_aligned{false}; // true the initial tilt alignment has been calculated
float _ahrs_accel_fusion_gain{0.f}; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation
Vector3f _ahrs_accel{0.f, 0.f, 0.f}; // low pass filtered body frame specific force vector used by AHRS calculation (m/s/s)
float _ahrs_accel_norm{0.f}; // length of _ahrs_accel specific force vector used by AHRS calculation (m/s/s)

// calculate the gain from gravity vector misalingment to tilt correction to be used by all AHRS filters
float ahrsCalcAccelGain() const;
Expand All @@ -114,7 +122,7 @@ class EKFGSF_yaw
void ahrsAlignTilt(const Vector3f &delta_vel);

// align all AHRS yaw orientations to initial values
void ahrsAlignYaw(const Vector3f &imu_gyro_bias = {});
void ahrsAlignYaw();

// Efficient propagation of a delta angle in body frame applied to the body to earth frame rotation matrix
Matrix3f ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g);
Expand All @@ -135,11 +143,11 @@ class EKFGSF_yaw
bool _ekf_gsf_vel_fuse_started{}; // true when the EKF's have started fusing velocity data and the prediction and update processing is active

// initialise states and covariance data for the GSF and EKF filters
void initialiseEKFGSF(const Vector2f &vel_NE = {}, const float vel_accuracy = 0.f);
void initialiseEKFGSF(const Vector2f &vel_NE, const float vel_accuracy);

// predict state and covariance for the specified EKF using inertial data
void predictEKF(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt,
const Vector3f &delta_vel, const float delta_vel_dt);
const Vector3f &delta_vel, const float delta_vel_dt, bool in_air = false);

// update state and covariance for the specified EKF using a NE velocity measurement
// return false if update failed
Expand Down
Loading

0 comments on commit b66a826

Please sign in to comment.