diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp index c282c01ed4..859796726a 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp @@ -36,14 +36,7 @@ EKFGSF_yaw::EKFGSF_yaw() { - // this flag must be false when we start - _ahrs_ekf_gsf_tilt_aligned = false; - - // these objects are initialised in initialise() before being used internally, but can be reported for logging before then - memset(&_ahrs_ekf_gsf, 0, sizeof(_ahrs_ekf_gsf)); - memset(&_ekf_gsf, 0, sizeof(_ekf_gsf)); - _gsf_yaw = 0.0f; - _ahrs_accel.zero(); + initialiseEKFGSF(); } void EKFGSF_yaw::update(const imuSample &imu_sample, @@ -52,16 +45,12 @@ void EKFGSF_yaw::update(const imuSample &imu_sample, const Vector3f &imu_gyro_bias) // estimated rate gyro bias (rad/sec) { // copy to class variables - _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; _true_airspeed = airspeed; // 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.0f * _delta_vel_dt * _tilt_gain, 1.0f); - const Vector3f accel = _delta_vel / fmaxf(_delta_vel_dt, 0.001f); - _ahrs_accel = _ahrs_accel * (1.0f - filter_coef) + accel * filter_coef; + 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; // Initialise states first time if (!_ahrs_ekf_gsf_tilt_aligned) { @@ -73,8 +62,7 @@ void EKFGSF_yaw::update(const imuSample &imu_sample, 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(); + ahrsAlignTilt(imu_sample.delta_vel); _ahrs_ekf_gsf_tilt_aligned = true; } @@ -88,20 +76,17 @@ void EKFGSF_yaw::update(const imuSample &imu_sample, _ahrs_accel_fusion_gain = ahrsCalcAccelGain(); for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - predictEKF(model_index); + predictEKF(model_index, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt); } // 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(); - ahrsAlignYaw(); + 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 - for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - _ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias; - } + ahrsAlignYaw(imu_gyro_bias); _ekf_gsf_vel_fuse_started = true; @@ -110,7 +95,7 @@ void EKFGSF_yaw::update(const imuSample &imu_sample, 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)) { + if (!updateEKF(model_index, _vel_NE, _vel_accuracy)) { bad_update = true; } } @@ -138,7 +123,7 @@ void EKFGSF_yaw::update(const imuSample &imu_sample, } else { // all weights have collapsed due to excessive innovation variances so reset filters - initialiseEKFGSF(); + initialiseEKFGSF(_vel_NE, _vel_accuracy); } } } @@ -173,11 +158,10 @@ void EKFGSF_yaw::update(const imuSample &imu_sample, _vel_data_updated = false; } -void EKFGSF_yaw::ahrsPredict(const uint8_t model_index) +void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt) { // generate attitude solution using simple complementary filter for the selected model - - const 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; const Dcmf R_to_body = _ahrs_ekf_gsf[model_index].R.transpose(); const Vector3f gravity_direction_bf = R_to_body.col(2); @@ -200,29 +184,27 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index) } tilt_correction = (gravity_direction_bf % accel) * _ahrs_accel_fusion_gain / _ahrs_accel_norm; - } // Gyro bias estimation constexpr float gyro_bias_limit = 0.05f; - const float spinRate = ang_rate.length(); + const float spin_rate = ang_rate.length(); - if (spinRate < 0.175f) { - _ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * _delta_ang_dt); + 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); } // 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); - } -void EKFGSF_yaw::ahrsAlignTilt() +void EKFGSF_yaw::ahrsAlignTilt(const Vector3f &delta_vel) { // Rotation matrix is constructed directly from acceleration measurement and will be the same for // all models so only need to calculate it once. Assumptions are: @@ -230,10 +212,10 @@ 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 - const Vector3f down_in_bf = -_delta_vel.normalized(); + 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.f, 0.f, 0.f); Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf.dot(down_in_bf)); north_in_bf.normalize(); @@ -254,7 +236,7 @@ void EKFGSF_yaw::ahrsAlignTilt() } } -void EKFGSF_yaw::ahrsAlignYaw() +void EKFGSF_yaw::ahrsAlignYaw(const Vector3f &imu_gyro_bias) { // Align yaw angle for each model for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { @@ -263,13 +245,16 @@ void EKFGSF_yaw::ahrsAlignYaw() R = updateYawInRotMat(yaw, R); _ahrs_ekf_gsf[model_index].aligned = true; + + _ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias; } } -void EKFGSF_yaw::predictEKF(const uint8_t model_index) +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) { // generate an attitude reference using IMU data - ahrsPredict(model_index); + ahrsPredict(model_index, delta_ang, delta_ang_dt); // we don't start running the EKF part of the algorithm until there are regular velocity observations if (!_ekf_gsf_vel_fuse_started) { @@ -280,7 +265,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index) _ekf_gsf[model_index].X(2) = getEulerYaw(_ahrs_ekf_gsf[model_index].R); // calculate delta velocity in a horizontal front-right frame - const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel; + const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * delta_vel; const float cos_yaw = cosf(_ekf_gsf[model_index].X(2)); const float sin_yaw = sinf(_ekf_gsf[model_index].X(2)); const float dvx = del_vel_NED(0) * cos_yaw + del_vel_NED(1) * sin_yaw; @@ -293,42 +278,42 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index) // predict covariance - equations generated using EKF/python/gsf_ekf_yaw_estimator/main.py // Local short variable name copies required for readability - const float P00 = _ekf_gsf[model_index].P(0,0); - const float P01 = _ekf_gsf[model_index].P(0,1); - const float P02 = _ekf_gsf[model_index].P(0,2); - const float P11 = _ekf_gsf[model_index].P(1,1); - const float P12 = _ekf_gsf[model_index].P(1,2); - const float P22 = _ekf_gsf[model_index].P(2,2); + const float P00 = _ekf_gsf[model_index].P(0, 0); + const float P01 = _ekf_gsf[model_index].P(0, 1); + const float P02 = _ekf_gsf[model_index].P(0, 2); + const float P11 = _ekf_gsf[model_index].P(1, 1); + const float P12 = _ekf_gsf[model_index].P(1, 2); + const float P22 = _ekf_gsf[model_index].P(2, 2); const float psi = _ekf_gsf[model_index].X(2); // Use fixed values for delta velocity and delta angle process noise variances - const float dvxVar = sq(_accel_noise * _delta_vel_dt); // variance of forward delta velocity - (m/s)^2 + const float dvxVar = sq(_accel_noise * delta_vel_dt); // variance of forward delta velocity - (m/s)^2 const float dvyVar = dvxVar; // variance of right delta velocity - (m/s)^2 - const float dazVar = sq(_gyro_noise * _delta_ang_dt); // variance of yaw delta angle - rad^2 + const float dazVar = sq(_gyro_noise * delta_ang_dt); // variance of yaw delta angle - rad^2 // optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py const float S0 = cosf(psi); const float S1 = ecl::powf(S0, 2); const float S2 = sinf(psi); const float S3 = ecl::powf(S2, 2); - const float S4 = S0*dvy + S2*dvx; - const float S5 = P02 - P22*S4; - const float S6 = S0*dvx - S2*dvy; - const float S7 = S0*S2; - const float S8 = P01 + S7*dvxVar - S7*dvyVar; - const float S9 = P12 + P22*S6; + const float S4 = S0 * dvy + S2 * dvx; + const float S5 = P02 - P22 * S4; + const float S6 = S0 * dvx - S2 * dvy; + const float S7 = S0 * S2; + const float S8 = P01 + S7 * dvxVar - S7 * dvyVar; + const float S9 = P12 + P22 * S6; - _ekf_gsf[model_index].P(0,0) = P00 - P02*S4 + S1*dvxVar + S3*dvyVar - S4*S5; - _ekf_gsf[model_index].P(0,1) = -P12*S4 + S5*S6 + S8; - _ekf_gsf[model_index].P(1,1) = P11 + P12*S6 + S1*dvyVar + S3*dvxVar + S6*S9; - _ekf_gsf[model_index].P(0,2) = S5; - _ekf_gsf[model_index].P(1,2) = S9; - _ekf_gsf[model_index].P(2,2) = P22 + dazVar; + _ekf_gsf[model_index].P(0, 0) = P00 - P02 * S4 + S1 * dvxVar + S3 * dvyVar - S4 * S5; + _ekf_gsf[model_index].P(0, 1) = -P12 * S4 + S5 * S6 + S8; + _ekf_gsf[model_index].P(1, 1) = P11 + P12 * S6 + S1 * dvyVar + S3 * dvxVar + S6 * S9; + _ekf_gsf[model_index].P(0, 2) = S5; + _ekf_gsf[model_index].P(1, 2) = S9; + _ekf_gsf[model_index].P(2, 2) = P22 + dazVar; // 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); - _ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2); - _ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2); + _ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1); + _ekf_gsf[model_index].P(2, 0) = _ekf_gsf[model_index].P(0, 2); + _ekf_gsf[model_index].P(2, 1) = _ekf_gsf[model_index].P(1, 2); // constrain variances const float min_var = 1e-6f; @@ -339,87 +324,89 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index) } // Update EKF states and covariance for specified model index using velocity measurement -bool EKFGSF_yaw::updateEKF(const uint8_t model_index) +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 - const float velObsVar = sq(fmaxf(_vel_accuracy, 0.01f)); + const float velObsVar = sq(fmaxf(vel_accuracy, 0.01f)); // calculate velocity observation innovations - _ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - _vel_NE(0); - _ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - _vel_NE(1); + _ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - vel_NE(0); + _ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - vel_NE(1); // Use temporary variables for covariance elements to reduce verbosity of auto-code expressions - const float P00 = _ekf_gsf[model_index].P(0,0); - const float P01 = _ekf_gsf[model_index].P(0,1); - const float P02 = _ekf_gsf[model_index].P(0,2); - const float P11 = _ekf_gsf[model_index].P(1,1); - const float P12 = _ekf_gsf[model_index].P(1,2); - const float P22 = _ekf_gsf[model_index].P(2,2); + const float P00 = _ekf_gsf[model_index].P(0, 0); + const float P01 = _ekf_gsf[model_index].P(0, 1); + const float P02 = _ekf_gsf[model_index].P(0, 2); + const float P11 = _ekf_gsf[model_index].P(1, 1); + const float P12 = _ekf_gsf[model_index].P(1, 2); + const float P22 = _ekf_gsf[model_index].P(2, 2); // optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py const float t0 = ecl::powf(P01, 2); const float t1 = -t0; - const float t2 = P00*P11 + P00*velObsVar + P11*velObsVar + t1 + ecl::powf(velObsVar, 2); + const float t2 = P00 * P11 + P00 * velObsVar + P11 * velObsVar + t1 + ecl::powf(velObsVar, 2); + if (fabsf(t2) < 1e-6f) { return false; } - const float t3 = 1.0F/t2; + + const float t3 = 1.0F / t2; const float t4 = P11 + velObsVar; - const float t5 = P01*t3; + const float t5 = P01 * t3; const float t6 = -t5; const float t7 = P00 + velObsVar; - const float t8 = P00*t4 + t1; - const float t9 = t5*velObsVar; - const float t10 = P11*t7; + const float t8 = P00 * t4 + t1; + const float t9 = t5 * velObsVar; + const float t10 = P11 * t7; const float t11 = t1 + t10; - const float t12 = P01*P12; - const float t13 = P02*t4; - const float t14 = P01*P02; - const float t15 = P12*t7; - const float t16 = t0*velObsVar; + const float t12 = P01 * P12; + const float t13 = P02 * t4; + const float t14 = P01 * P02; + const float t15 = P12 * t7; + const float t16 = t0 * velObsVar; const float t17 = ecl::powf(t2, -2); - const float t18 = t4*velObsVar + t8; - const float t19 = t17*t18; - const float t20 = t17*(t16 + t7*t8); + const float t18 = t4 * velObsVar + t8; + const float t19 = t17 * t18; + const float t20 = t17 * (t16 + t7 * t8); const float t21 = t0 - t10; - const float t22 = t17*t21; + const float t22 = t17 * t21; const float t23 = t14 - t15; - const float t24 = P01*t23; + const float t24 = P01 * t23; const float t25 = t12 - t13; - const float t26 = t16 - t21*t4; - const float t27 = t17*t26; - const float t28 = t11 + t7*velObsVar; - const float t30 = t17*t28; - const float t31 = P01*t25; - const float t32 = t23*t4 + t31; - const float t33 = t17*t32; - const float t35 = t24 + t25*t7; - const float t36 = t17*t35; + const float t26 = t16 - t21 * t4; + const float t27 = t17 * t26; + const float t28 = t11 + t7 * velObsVar; + const float t30 = t17 * t28; + const float t31 = P01 * t25; + const float t32 = t23 * t4 + t31; + const float t33 = t17 * t32; + const float t35 = t24 + t25 * t7; + const float t36 = t17 * t35; _ekf_gsf[model_index].S_det_inverse = t3; - _ekf_gsf[model_index].S_inverse(0,0) = t3*t4; - _ekf_gsf[model_index].S_inverse(0,1) = t6; - _ekf_gsf[model_index].S_inverse(1,1) = t3*t7; - _ekf_gsf[model_index].S_inverse(1,0) = _ekf_gsf[model_index].S_inverse(0,1); + _ekf_gsf[model_index].S_inverse(0, 0) = t3 * t4; + _ekf_gsf[model_index].S_inverse(0, 1) = t6; + _ekf_gsf[model_index].S_inverse(1, 1) = t3 * t7; + _ekf_gsf[model_index].S_inverse(1, 0) = _ekf_gsf[model_index].S_inverse(0, 1); matrix::Matrix K; - K(0,0) = t3*t8; - K(1,0) = t9; - K(2,0) = t3*(-t12 + t13); - K(0,1) = t9; - K(1,1) = t11*t3; - K(2,1) = t3*(-t14 + t15); + K(0, 0) = t3 * t8; + K(1, 0) = t9; + K(2, 0) = t3 * (-t12 + t13); + K(0, 1) = t9; + K(1, 1) = t11 * t3; + K(2, 1) = t3 * (-t14 + t15); - _ekf_gsf[model_index].P(0,0) = P00 - t16*t19 - t20*t8; - _ekf_gsf[model_index].P(0,1) = P01*(t18*t22 - t20*velObsVar + 1); - _ekf_gsf[model_index].P(1,1) = P11 - t16*t30 + t22*t26; - _ekf_gsf[model_index].P(0,2) = P02 + t19*t24 + t20*t25; - _ekf_gsf[model_index].P(1,2) = P12 + t23*t27 + t30*t31; - _ekf_gsf[model_index].P(2,2) = P22 - t23*t33 - t25*t36; - _ekf_gsf[model_index].P(1,0) = _ekf_gsf[model_index].P(0,1); - _ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2); - _ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2); + _ekf_gsf[model_index].P(0, 0) = P00 - t16 * t19 - t20 * t8; + _ekf_gsf[model_index].P(0, 1) = P01 * (t18 * t22 - t20 * velObsVar + 1); + _ekf_gsf[model_index].P(1, 1) = P11 - t16 * t30 + t22 * t26; + _ekf_gsf[model_index].P(0, 2) = P02 + t19 * t24 + t20 * t25; + _ekf_gsf[model_index].P(1, 2) = P12 + t23 * t27 + t30 * t31; + _ekf_gsf[model_index].P(2, 2) = P22 - t23 * t33 - t25 * t36; + _ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1); + _ekf_gsf[model_index].P(2, 0) = _ekf_gsf[model_index].P(0, 2); + _ekf_gsf[model_index].P(2, 1) = _ekf_gsf[model_index].P(1, 2); // constrain variances const float min_var = 1e-6f; @@ -462,24 +449,24 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index) return true; } -void EKFGSF_yaw::initialiseEKFGSF() +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 = _m_pi2 * _m_pi2; + _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 / (float)N_MODELS_EKFGSF; + const float yaw_increment = 2.0f * M_PI_F / (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); + _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(_vel_accuracy); + _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); // use half yaw interval for yaw uncertainty @@ -492,7 +479,7 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const // calculate transpose(innovation) * inv(S) * innovation const float normDist = _ekf_gsf[model_index].innov.dot(_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov); - return _m_2pi_inv * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist); + return (1.f / (2.f * M_PI_F)) * 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], @@ -538,15 +525,15 @@ float EKFGSF_yaw::ahrsCalcAccelGain() const 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); - ret(0,1) += R(0,2) * g(0) - R(0,0) * g(2); - ret(0,2) += R(0,0) * g(1) - R(0,1) * g(0); - ret(1,0) += R(1,1) * g(2) - R(1,2) * g(1); - ret(1,1) += R(1,2) * g(0) - R(1,0) * g(2); - ret(1,2) += R(1,0) * g(1) - R(1,1) * g(0); - ret(2,0) += R(2,1) * g(2) - R(2,2) * g(1); - ret(2,1) += R(2,2) * g(0) - R(2,0) * g(2); - ret(2,2) += R(2,0) * g(1) - R(2,1) * g(0); + ret(0, 0) += R(0, 1) * g(2) - R(0, 2) * g(1); + ret(0, 1) += R(0, 2) * g(0) - R(0, 0) * g(2); + ret(0, 2) += R(0, 0) * g(1) - R(0, 1) * g(0); + ret(1, 0) += R(1, 1) * g(2) - R(1, 2) * g(1); + ret(1, 1) += R(1, 2) * g(0) - R(1, 0) * g(2); + ret(1, 2) += R(1, 0) * g(1) - R(1, 1) * g(0); + ret(2, 0) += R(2, 1) * g(2) - R(2, 2) * g(1); + ret(2, 1) += R(2, 2) * g(0) - R(2, 0) * g(2); + ret(2, 2) += R(2, 0) * g(1) - R(2, 1) * g(0); // Renormalise rows for (uint8_t r = 0; r < 3; r++) { diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.h b/src/modules/ekf2/EKF/EKFGSF_yaw.h index ee22eeccb6..d8073c4492 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.h @@ -52,11 +52,6 @@ using matrix::wrap_pi; static constexpr uint8_t N_MODELS_EKFGSF = 5; -// 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; - using namespace estimator; class EKFGSF_yaw @@ -94,38 +89,30 @@ private: const float _gyro_bias_gain{0.04f}; // gain applied to integral of gyro correction for complementary filter (1/sec) // Declarations used by the bank of N_MODELS_EKFGSF AHRS complementary filters - - Vector3f _delta_ang{}; // IMU delta angle (rad) - Vector3f _delta_vel{}; // IMU delta velocity (m/s) - float _delta_ang_dt{}; // _delta_ang integration time interval (sec) - 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 { - 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) + struct { + Dcmf R{matrix::eye()}; // 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{}; // 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 - Vector3f _ahrs_accel{}; // low pass filtered body frame specific force vector used by AHRS calculation (m/s/s) - float _ahrs_accel_norm{}; // length of _ahrs_accel specific force vector used by AHRS calculation (m/s/s) + 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; // update specified AHRS rotation matrix using IMU and optionally true airspeed data - void ahrsPredict(const uint8_t model_index); + void ahrsPredict(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt); // align all AHRS roll and pitch orientations using IMU delta velocity vector - void ahrsAlignTilt(); + void ahrsAlignTilt(const Vector3f &delta_vel); // align all AHRS yaw orientations to initial values - void ahrsAlignYaw(); + void ahrsAlignYaw(const Vector3f &imu_gyro_bias = {}); // 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); @@ -133,11 +120,11 @@ private: // 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 - 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) + matrix::Vector3f X{}; // Vel North (m/s), Vel East (m/s), yaw (rad)s + matrix::SquareMatrix P{}; // 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) } _ekf_gsf[N_MODELS_EKFGSF] {}; bool _vel_data_updated{}; // true when velocity data has been updated @@ -146,14 +133,15 @@ private: 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(); + void initialiseEKFGSF(const Vector2f &vel_NE = {}, const float vel_accuracy = 0.f); // predict state and covariance for the specified EKF using inertial data - void predictEKF(const uint8_t model_index); + 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); // update state and covariance for the specified EKF using a NE velocity measurement // return false if update failed - bool updateEKF(const uint8_t model_index); + bool updateEKF(const uint8_t model_index, const Vector2f &vel_NE, const float vel_accuracy); inline float sq(float x) const { return x * x; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index fd7f9442fd..5acba0d7d4 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -121,7 +121,12 @@ void Ekf::controlFusionModes() _gps_sample_delayed.pos -= pos_offset_earth.xy(); _gps_sample_delayed.hgt += pos_offset_earth(2); - _gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise); + // update GSF yaw estimator velocity (basic sanity check on GNSS velocity data) + if ((_gps_sample_delayed.sacc > 0.f) && (_gps_sample_delayed.sacc < _params.req_sacc) + && _gps_sample_delayed.vel.isAllFinite() + ) { + _yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), math::max(_gps_sample_delayed.sacc, _params.gps_vel_noise)); + } } } diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index 2c7c55e4c3..15e3669cf7 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -117,7 +117,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) // reset vertical velocity if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & GnssCtrl::VEL)) { // use 1.5 as a typical ratio of vacc/hacc - resetVerticalVelocityTo(gps_sample.vel(2), sq(1.5f * gps_sample.sacc)); + resetVerticalVelocityTo(gps_sample.vel(2), sq(math::max(1.5f * gps_sample.sacc, _params.gps_vel_noise))); } else { resetVerticalVelocityToZero(); diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index e45d08123c..be8102d02d 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -87,11 +87,6 @@ void Ekf::controlGpsFusion() _aid_src_gnss_pos); _aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); - // update GSF yaw estimator velocity (basic sanity check on GNSS velocity data) - if (gps_checks_passing && !gps_checks_failing) { - _yawEstimator.setVelocity(gps_sample.vel.xy(), gps_sample.sacc); - } - // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently const bool mandatory_conditions_passing = ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))