diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index 51d4f8f90b..df433199d0 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -13,18 +13,15 @@ EKFGSF_yaw::EKFGSF_yaw() _ahrs_accel.zero(); } -void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vector meassured in body frame (rad) - const Vector3f del_vel, // IMU delta velocity vector meassured in body frame (m/s) - const float del_ang_dt, // time interval that del_ang was integrated over (sec) - const float del_vel_dt, // time interval that del_vel was integrated over (sec) +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. { // copy to class variables - _delta_ang = del_ang; - _delta_vel = del_vel; - _delta_ang_dt = del_ang_dt; - _delta_vel_dt = del_vel_dt; + _delta_ang = imu_sample.delta_ang; + _delta_vel = imu_sample.delta_vel; + _delta_ang_dt = imu_sample.delta_ang_dt; + _delta_vel_dt = imu_sample.delta_vel_dt; _run_ekf_gsf = run_EKF; _true_airspeed = airspeed; @@ -62,8 +59,6 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect ahrsAlignYaw(); _ekf_gsf_vel_fuse_started = true; } else { - float total_w = 0.0f; - float newWeight[N_MODELS_EKFGSF]; 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 @@ -73,18 +68,16 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect } if (!bad_update) { + float total_weight = 0.0f; // calculate weighting for each model assuming a normal distribution for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - newWeight[model_index] = fmaxf(gaussianDensity(model_index) * _ekf_gsf[model_index].W, 0.0f); - total_w += newWeight[model_index]; + _model_weights(model_index) = fmaxf(gaussianDensity(model_index) * _model_weights(model_index), 0.0f); + total_weight += _model_weights(model_index); } // normalise the weighting function - if (_ekf_gsf_vel_fuse_started && total_w > 1e-15f && !bad_update) { - float total_w_inv = 1.0f / total_w; - for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - _ekf_gsf[model_index].W = newWeight[model_index] * total_w_inv; - } + if (total_weight > 1e-15f) { + _model_weights /= total_weight; } // Enforce a minimum weighting value. This was added during initial development but has not been needed @@ -95,12 +88,12 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect bool change_mask[N_MODELS_EKFGSF] = {}; // true when the weighting for that model has been increased float unmodified_weights_sum = 0.0f; // sum of unmodified weights for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - if (_ekf_gsf[model_index].W < _weight_min) { - correction_sum += _weight_min - _ekf_gsf[model_index].W; - _ekf_gsf[model_index].W = _weight_min; + if (_model_weights(model_index) < _weight_min) { + correction_sum += _weight_min - _model_weights(model_index); + _model_weights(model_index) = _weight_min; change_mask[model_index] = true; } else { - unmodified_weights_sum += _ekf_gsf[model_index].W; + unmodified_weights_sum += _model_weights(model_index); } } @@ -108,7 +101,7 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect const float scale_factor = (unmodified_weights_sum - correction_sum - _weight_min) / (unmodified_weights_sum - _weight_min); for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { if (!change_mask[model_index]) { - _ekf_gsf[model_index].W = _weight_min + scale_factor * (_ekf_gsf[model_index].W - _weight_min); + _model_weights(model_index) = _weight_min + scale_factor * (_model_weights(model_index) - _weight_min); } } } @@ -122,10 +115,10 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect // 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 legnth // equal to the weighting value before it is summed. - Vector2f yaw_vector = {}; + Vector2f yaw_vector; for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - yaw_vector(0) += _ekf_gsf[model_index].W * cosf(_ekf_gsf[model_index].X(2)); - yaw_vector(1) += _ekf_gsf[model_index].W * sinf(_ekf_gsf[model_index].X(2)); + 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)); @@ -133,8 +126,8 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect // 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 ++) { - float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw); - _gsf_yaw_variance += _ekf_gsf[model_index].W * (_ekf_gsf[model_index].P(2,2) + yaw_delta * yaw_delta); + 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 @@ -145,33 +138,28 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index) { // generate attitude solution using simple complementary filter for the selected model - Vector3f ang_rate = _delta_ang / fmaxf(_delta_ang_dt, 0.001f) - _ahrs_ekf_gsf[model_index].gyro_bias; + const Vector3f ang_rate = _delta_ang / fmaxf(_delta_ang_dt, 0.001f) - _ahrs_ekf_gsf[model_index].gyro_bias; - // Accelerometer correction - // Project 'k' unit vector of earth frame to body frame - // Vector3f k = quaterion.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f)); - // Optimized version with dropped zeros - const Vector3f k(_ahrs_ekf_gsf[model_index].R(2,0), _ahrs_ekf_gsf[model_index].R(2,1), _ahrs_ekf_gsf[model_index].R(2,2)); + const Dcmf R_to_body = _ahrs_ekf_gsf[model_index].R.transpose(); + const Vector3f gravity_direction_bf = R_to_body.col(2); // 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) { Vector3f accel = _ahrs_accel; if (_true_airspeed > FLT_EPSILON) { // turn rate is component of gyro rate about vertical (down) axis - const float turn_rate = _ahrs_ekf_gsf[model_index].R(2,0) * ang_rate(0) - + _ahrs_ekf_gsf[model_index].R(2,1) * ang_rate(1) - + _ahrs_ekf_gsf[model_index].R(2,2) * ang_rate(2); + const float turn_rate = gravity_direction_bf.dot(ang_rate); // use measured airspeed to calculate centripetal acceleration if available - float centripetal_accel = _true_airspeed * turn_rate; + const float centripetal_accel = _true_airspeed * turn_rate; // project Y body axis onto horizontal and multiply by centripetal acceleration to give estimated // centripetal acceleration vector in earth frame due to coordinated turn - Vector3f centripetal_accel_vec_ef = {_ahrs_ekf_gsf[model_index].R(0,1), _ahrs_ekf_gsf[model_index].R(1,1), 0.0f}; + Vector3f centripetal_accel_vec_ef(_ahrs_ekf_gsf[model_index].R(0,1), _ahrs_ekf_gsf[model_index].R(1,1), 0.0f); if (_ahrs_ekf_gsf[model_index].R(2,2) > 0.0f) { // vehicle is upright centripetal_accel_vec_ef *= centripetal_accel; @@ -181,29 +169,26 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index) } // rotate into body frame - Vector3f centripetal_accel_vec_bf = _ahrs_ekf_gsf[model_index].R.transpose() * centripetal_accel_vec_ef; + Vector3f centripetal_accel_vec_bf = R_to_body * centripetal_accel_vec_ef; // correct measured accel for centripetal acceleration accel -= centripetal_accel_vec_bf; } - tilt_correction = (k % accel) * _ahrs_accel_fusion_gain / _ahrs_accel_norm; + tilt_correction = (gravity_direction_bf % accel) * _ahrs_accel_fusion_gain / _ahrs_accel_norm; } // Gyro bias estimation - const float gyro_bias_limit = 0.05f; + 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); - - for (int i = 0; i < 3; i++) { - _ahrs_ekf_gsf[model_index].gyro_bias(i) = math::constrain(_ahrs_ekf_gsf[model_index].gyro_bias(i), -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 - 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); @@ -218,17 +203,14 @@ void EKFGSF_yaw::ahrsAlignTilt() // 2) The vehicle is not accelerating so all of the measured acceleration is due to gravity. // Calculate earth frame Down axis unit vector rotated into body frame - Vector3f down_in_bf = -_delta_vel; - down_in_bf.normalize(); + const Vector3f down_in_bf = -_delta_vel.normalized(); // Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf' - // * operator is overloaded to provide a dot product const Vector3f i_vec_bf(1.0f,0.0f,0.0f); - Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf * down_in_bf); + Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf.dot(down_in_bf)); north_in_bf.normalize(); // Calculate earth frame East axis unit vector rotated into body frame, orthogonal to 'down_in_bf' and 'north_in_bf' - // % operator is overloaded to provide a cross product const Vector3f east_in_bf = down_in_bf % north_in_bf; // Each column in a rotation matrix from earth frame to body frame represents the projection of the @@ -240,6 +222,9 @@ void EKFGSF_yaw::ahrsAlignTilt() R.setRow(1, east_in_bf); R.setRow(2, down_in_bf); + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { + _ahrs_ekf_gsf[model_index].R = R; + } } void EKFGSF_yaw::ahrsAlignYaw() @@ -371,13 +356,12 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index) const float &P22 = _ekf_gsf[model_index].P(2,2); // calculate innovation variance - _ekf_gsf[model_index].S(0,0) = P00 + velObsVar; - _ekf_gsf[model_index].S(1,1) = P11 + velObsVar; - _ekf_gsf[model_index].S(0,1) = P01; - _ekf_gsf[model_index].S(1,0) = P10; + matrix::SquareMatrix S = _ekf_gsf[model_index].P.slice<2, 2>(0, 0); + S(0, 0) += velObsVar; + S(1, 1) += velObsVar; // Update the inverse of the innovation covariance matrix S_inverse - updateInnovCovMatInv(model_index); + updateInnovCovMatInv(model_index, S); // Perform a chi-square innovation consistency test and calculate a compression scale factor that limits the magnitude of innovations to 5-sigma float innov_comp_scale_factor = 1.0f; @@ -470,8 +454,10 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index) // Correct the state vector and capture the change in yaw angle const float oldYaw = _ekf_gsf[model_index].X(2); + _ekf_gsf[model_index].X -= (K * _ekf_gsf[model_index].innov) * innov_comp_scale_factor; - float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw; + + const float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw; // apply the change in yaw angle to the AHRS // take advantage of sparseness in the yaw rotation matrix @@ -493,15 +479,14 @@ void EKFGSF_yaw::initialiseEKFGSF() { _gsf_yaw = 0.0f; _ekf_gsf_vel_fuse_started = false; - _gsf_yaw_variance = M_PI_2_F * M_PI_2_F; + _gsf_yaw_variance = _m_pi2 * _m_pi2; + _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.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_F + (0.5f * yaw_increment) + ((float)model_index * yaw_increment); - - // All filter models start with the same weight - _ekf_gsf[model_index].W = 1.0f / (float)N_MODELS_EKFGSF; + _ekf_gsf[model_index].X(2) = -_m_pi + (0.5f * yaw_increment) + ((float)model_index * yaw_increment); // take velocity states and corresponding variance from last meaurement _ekf_gsf[model_index].X(0) = _vel_NE(0); @@ -516,31 +501,27 @@ void EKFGSF_yaw::initialiseEKFGSF() float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const { - // calculate inv(S) * innovation - const matrix::Vector2f tempVec = _ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov; - // calculate transpose(innovation) * inv(S) * innovation - // * operator is overloaded to provide a dot product - const float normDist = _ekf_gsf[model_index].innov * tempVec; + const float normDist = _ekf_gsf[model_index].innov.dot(_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov); - return M_TWOPI_INV * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist); + return _m_2pi_inv * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist); } -void EKFGSF_yaw::updateInnovCovMatInv(const uint8_t model_index) +void EKFGSF_yaw::updateInnovCovMatInv(const uint8_t model_index, const matrix::SquareMatrix &S) { // calculate determinant for innovation covariance matrix - const float t2 = _ekf_gsf[model_index].S(0,0) * _ekf_gsf[model_index].S(1,1); - const float t5 = _ekf_gsf[model_index].S(0,1) * _ekf_gsf[model_index].S(1,0); + const float t2 = S(0,0) * S(1,1); + const float t5 = S(0,1) * S(1,0); const float t3 = t2 - t5; // calculate determinant inverse and protect against badly conditioned matrix _ekf_gsf[model_index].S_det_inverse = 1.0f / fmaxf(t3 , 1e-12f); // calculate inv(S) - _ekf_gsf[model_index].S_inverse(0,0) = _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(1,1); - _ekf_gsf[model_index].S_inverse(1,1) = _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(0,0); - _ekf_gsf[model_index].S_inverse(0,1) = - _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(0,1); - _ekf_gsf[model_index].S_inverse(1,0) = - _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(1,0); + _ekf_gsf[model_index].S_inverse(0,0) = _ekf_gsf[model_index].S_det_inverse * S(1,1); + _ekf_gsf[model_index].S_inverse(1,1) = _ekf_gsf[model_index].S_det_inverse * S(0,0); + _ekf_gsf[model_index].S_inverse(0,1) = - _ekf_gsf[model_index].S_det_inverse * S(0,1); + _ekf_gsf[model_index].S_inverse(1,0) = - _ekf_gsf[model_index].S_det_inverse * S(1,0); } @@ -553,7 +534,7 @@ bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw 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] = _ekf_gsf[model_index].W; + weight[model_index] = _model_weights(model_index); } return true; } @@ -562,26 +543,26 @@ bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw Dcmf EKFGSF_yaw::taitBryan312ToRotMat(const Vector3f &rot312) { - // Calculate the frame2 to frame 1 rotation matrix from a 312 rotation sequence - const float c2 = cosf(rot312(2)); - const float s2 = sinf(rot312(2)); - const float s1 = sinf(rot312(1)); - const float c1 = cosf(rot312(1)); - const float s0 = sinf(rot312(0)); - const float c0 = cosf(rot312(0)); + // Calculate the frame2 to frame 1 rotation matrix from a 312 rotation sequence + const float c2 = cosf(rot312(2)); + const float s2 = sinf(rot312(2)); + const float s1 = sinf(rot312(1)); + const float c1 = cosf(rot312(1)); + const float s0 = sinf(rot312(0)); + const float c0 = cosf(rot312(0)); - Dcmf R; - R(0, 0) = c0 * c2 - s0 * s1 * s2; - R(1, 1) = c0 * c1; - R(2, 2) = c2 * c1; - R(0, 1) = -c1 * s0; - R(0, 2) = s2 * c0 + c2 * s1 * s0; - R(1, 0) = c2 * s0 + s2 * s1 * c0; - R(1, 2) = s0 * s2 - s1 * c0 * c2; - R(2, 0) = -s2 * c1; - R(2, 1) = s1; + Dcmf R; + R(0, 0) = c0 * c2 - s0 * s1 * s2; + R(1, 1) = c0 * c1; + R(2, 2) = c2 * c1; + R(0, 1) = -c1 * s0; + R(0, 2) = s2 * c0 + c2 * s1 * s0; + R(1, 0) = c2 * s0 + s2 * s1 * c0; + R(1, 2) = s0 * s2 - s1 * c0 * c2; + R(2, 0) = -s2 * c1; + R(2, 1) = s1; - return R; + return R; } void EKFGSF_yaw::ahrsCalcAccelGain() @@ -593,7 +574,7 @@ void EKFGSF_yaw::ahrsCalcAccelGain() // at the min and mas g value. Allow for more acceleration when flying as a fixed wing vehicle using centripetal // acceleration correction as higher and more sustained g will be experienced. // Use a quadratic finstead of linear unction to prevent vibration around 1g reducing the tilt correction effectiveness. - float accel_g = _ahrs_accel_norm / CONSTANTS_ONE_G; + const float accel_g = _ahrs_accel_norm / CONSTANTS_ONE_G; if (accel_g > 1.0f) { if (_true_airspeed > FLT_EPSILON && accel_g < 2.0f) { _ahrs_accel_fusion_gain = _tilt_gain * sq(2.0f - accel_g); @@ -609,7 +590,7 @@ void EKFGSF_yaw::ahrsCalcAccelGain() } } -Matrix3f EKFGSF_yaw::ahrsPredictRotMat(Matrix3f &R, Vector3f &g) +Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g) { Matrix3f ret = R; ret(0,0) += R(0,1) * g(2) - R(0,2) * g(1); @@ -648,7 +629,7 @@ bool EKFGSF_yaw::getYawData(float *yaw, float *yaw_variance) return false; } -void EKFGSF_yaw::setVelocity(Vector2f velocity, float accuracy) +void EKFGSF_yaw::setVelocity(const Vector2f &velocity, float accuracy) { _vel_NE = velocity; _vel_accuracy = accuracy; diff --git a/EKF/EKFGSF_yaw.h b/EKF/EKFGSF_yaw.h index 03ae6586b9..d1aca212e6 100644 --- a/EKF/EKFGSF_yaw.h +++ b/EKF/EKFGSF_yaw.h @@ -4,6 +4,8 @@ #include #include +#include "common.h" + using matrix::AxisAnglef; using matrix::Dcmf; using matrix::Eulerf; @@ -13,35 +15,26 @@ using matrix::Vector2f; using matrix::Vector3f; using matrix::wrap_pi; -#define N_MODELS_EKFGSF 5 +static constexpr uint8_t N_MODELS_EKFGSF = 5; -#ifndef M_PI_F -#define M_PI_F 3.14159265f -#endif +// Required math constants +static constexpr float _m_2pi_inv = 0.159154943f; +static constexpr float _m_pi = 3.14159265f; +static constexpr float _m_pi2 = 1.57079632f; -#ifndef M_PI_2_F -#define M_PI_2_F 1.57079632f -#endif - -#ifndef M_TWOPI_INV -#define M_TWOPI_INV 0.159154943f -#endif +using namespace estimator; class EKFGSF_yaw { public: - // Constructor EKFGSF_yaw(); // Update Filter States - this should be called whenever new IMU data is available - void update(const Vector3f del_ang, // IMU delta angle rotation vector meassured in body frame (rad) - const Vector3f del_vel, // IMU delta velocity vector meassured in body frame (m/s) - const float del_ang_dt, // time interval that del_ang was integrated over (sec) - const float del_vel_dt, // time interval that del_vel was integrated over (sec) + 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. - void setVelocity(Vector2f velocity, // NE velocity measurement (m/s) + void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s) float accuracy); // 1-sigma accuracy of velocity measurement (m/s) // get solution data for logging @@ -57,6 +50,7 @@ public: bool getYawData(float *yaw, float *yaw_variance); private: + // Parameters - these could be made tuneable const float _gyro_noise{1.0e-1f}; // yaw rate noise used for covariance prediction (rad/sec) const float _accel_noise{2.0f}; // horizontal accel noise used for covariance prediction (m/sec**2) @@ -99,15 +93,13 @@ private: void ahrsAlignYaw(); // Efficient propagation of a delta angle in body frame applied to the body to earth frame rotation matrix - Matrix3f ahrsPredictRotMat(Matrix3f &R, Vector3f &g); + Matrix3f ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g); // Declarations used by a bank of N_MODELS_EKFGSF EKFs struct _ekf_gsf_struct{ matrix::Vector3f X; // Vel North (m/s), Vel East (m/s), yaw (rad)s matrix::SquareMatrix P; // covariance matrix - float W = 0.0f; // weighting - matrix::SquareMatrix S; // innovation covariance matrix matrix::SquareMatrix 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) @@ -141,6 +133,7 @@ private: // Declarations used by the Gaussian Sum Filter (GSF) that combines the individual EKF yaw estimates + matrix::Vector _model_weights; float _gsf_yaw; // yaw estimate (rad) float _gsf_yaw_variance; // variance of yaw estimate (rad^2) @@ -148,6 +141,6 @@ private: float gaussianDensity(const uint8_t model_index) const; // update the inverse of the innovation covariance matrix - void updateInnovCovMatInv(const uint8_t model_index); + void updateInnovCovMatInv(const uint8_t model_index, const matrix::SquareMatrix &S); }; diff --git a/EKF/ekf.h b/EKF/ekf.h index 7a721d250e..80acd5e32f 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -290,10 +290,9 @@ public: // Request the EKF reset the yaw to the estimate from the internal EKF-GSF filter // and reset the velocity and position states to the GPS. This will cause the EKF - // to ignore the magnetomer for the remainder of flight. + // to ignore the magnetometer for the remainder of flight. // This should only be used as a last resort before activating a loss of navigation failsafe - // The counter must be incremented for each new reset request - void requestEmergencyNavReset(uint8_t counter) override; + void requestEmergencyNavReset() override; private: @@ -846,7 +845,6 @@ private: int64_t _emergency_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec) uint64_t _time_last_on_ground_us{0}; ///< last tine we were on the ground (uSec) - uint8_t _yaw_extreset_counter{0}; // number of external emergency yaw reset requests bool _do_emergency_yaw_reset{false}; // true when an emergency yaw reset has been requested // Call once per _imu_sample_delayed update after all main EKF data fusion oeprations have been completed diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index cfb054b801..46a35532db 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1785,12 +1785,9 @@ bool Ekf::resetYawToEKFGSF() } -void Ekf::requestEmergencyNavReset(uint8_t counter) +void Ekf::requestEmergencyNavReset() { - if (counter > _yaw_extreset_counter) { - _yaw_extreset_counter = counter; - _do_emergency_yaw_reset = true; - } + _do_emergency_yaw_reset = true; } bool Ekf::getDataEKFGSF(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]) @@ -1830,11 +1827,11 @@ void Ekf::runYawEKFGSF() } else { TAS = _airspeed_sample_delayed.true_airspeed; } - yawEstimator.update(_imu_sample_delayed.delta_ang, _imu_sample_delayed.delta_vel, _imu_sample_delayed.delta_ang_dt, _imu_sample_delayed.delta_vel_dt, _control_status.flags.in_air, TAS); + + yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS); // basic sanity check on GPS velocity data if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON && isfinite(_gps_sample_delayed.vel(0)) && isfinite(_gps_sample_delayed.vel(1))) { - Vector2f vel_NE = Vector2f(_gps_sample_delayed.vel(0),_gps_sample_delayed.vel(1)); - yawEstimator.setVelocity(vel_NE, _gps_sample_delayed.vacc); + yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc); } } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 252befe662..2803421459 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -417,7 +417,7 @@ public: // request the EKF reset the yaw to the estimate from the internal EKF-GSF filter // argment should be incremented only when a new reset is required - virtual void requestEmergencyNavReset(uint8_t counter) = 0; + virtual void requestEmergencyNavReset() = 0; // get ekf-gsf debug data virtual bool getDataEKFGSF(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]) = 0;