diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/baro_height_control.cpp index 9eee1c0e1a..16cfa3ef53 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/baro_height_control.cpp @@ -108,7 +108,7 @@ void Ekf::controlBaroHeightFusion() if (measurement_valid) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9)); + bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 4409241108..55c679eac0 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -57,41 +57,39 @@ void Ekf::initialiseCovariance() resetQuatCov(); // velocity - P(4,4) = sq(fmaxf(_params.gps_vel_noise, 0.01f)); - P(5,5) = P(4,4); - P(6,6) = sq(1.5f) * P(4,4); + const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f)); + P.uncorrelateCovarianceSetVariance(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var)); // position - P(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f)); - P(8,8) = P(7,7); - P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f)); + const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f)); + float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f)); if (_control_status.flags.gps_hgt) { - P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); + z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); } #if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { - P(9,9) = sq(fmaxf(_params.range_noise, 0.01f)); + z_pos_var = sq(fmaxf(_params.range_noise, 0.01f)); } #endif // CONFIG_EKF2_RANGE_FINDER + P.uncorrelateCovarianceSetVariance(State::pos.idx, Vector3f(xy_pos_var, xy_pos_var, z_pos_var)); + // gyro bias - _prev_gyro_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias); - _prev_gyro_bias_var(1) = P(11,11) = P(10,10); - _prev_gyro_bias_var(2) = P(12,12) = P(10,10); + const float gyro_bias_var = sq(_params.switch_on_gyro_bias); + P.uncorrelateCovarianceSetVariance(State::gyro_bias.idx, gyro_bias_var); + _prev_gyro_bias_var.setAll(gyro_bias_var); // accel bias - _prev_accel_bias_var(0) = P(13,13) = sq(_params.switch_on_accel_bias); - _prev_accel_bias_var(1) = P(14,14) = P(13,13); - _prev_accel_bias_var(2) = P(15,15) = P(13,13); + const float accel_bias_var = sq(_params.switch_on_accel_bias); + P.uncorrelateCovarianceSetVariance(State::accel_bias.idx, accel_bias_var); + _prev_accel_bias_var.setAll(accel_bias_var); resetMagCov(); // wind - P(22,22) = sq(_params.initial_wind_uncertainty); - P(23,23) = P(22,22); - + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); } void Ekf::predictCovariance(const imuSample &imu_delayed) @@ -113,8 +111,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // gyro bias inhibit const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)); - for (unsigned stateIndex = 10; stateIndex <= 12; stateIndex++) { - const unsigned index = stateIndex - 10; + for (unsigned index = 0; index < State::gyro_bias.dof; index++) { + const unsigned stateIndex = State::gyro_bias.idx + index; bool is_bias_observable = true; @@ -143,8 +141,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) || is_manoeuvre_level_high || _fault_status.flags.bad_acc_vertical; - for (unsigned stateIndex = 13; stateIndex <= 15; stateIndex++) { - const unsigned index = stateIndex - 13; + for (unsigned index = 0; index < State::accel_bias.dof; index++) { + const unsigned stateIndex = State::accel_bias.idx + index; bool is_bias_observable = true; @@ -180,7 +178,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // 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 = 0.0f; - if (_control_status.flags.mag && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) { + if (_control_status.flags.mag && P.trace(State::mag_I.idx) < 0.1f) { #if defined(CONFIG_EKF2_MAGNETOMETER) mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f); #endif // CONFIG_EKF2_MAGNETOMETER @@ -189,7 +187,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // 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 = 0.0f; - if (_control_status.flags.mag && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) { + if (_control_status.flags.mag && P.trace(State::mag_B.idx) < 0.1f) { #if defined(CONFIG_EKF2_MAGNETOMETER) mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f); #endif // CONFIG_EKF2_MAGNETOMETER @@ -202,14 +200,13 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; // Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned - if (_control_status.flags.wind && (P(22,22) + P(23,23)) < sq(_params.initial_wind_uncertainty)) { + if (_control_status.flags.wind && P.trace(State::wind_vel.idx) < sq(_params.initial_wind_uncertainty)) { wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.0f, 1.0f) * (1.0f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); } else { wind_vel_nsd_scaled = 0.0f; } - // assign IMU noise variances // inputs to the system are 3 delta angles and 3 delta velocities float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f); @@ -219,7 +216,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) Vector3f d_vel_var; - for (int i = 0; i <= 2; i++) { + for (unsigned i = 0; i < 3; i++) { if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) { // Increase accelerometer process noise if bad accel data is detected d_vel_var(i) = sq(imu_delayed.delta_vel_dt * BADACC_BIAS_PNOISE); @@ -238,54 +235,87 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var, &nextP); - // compute noise variance for stationary processes - Vector24f process_noise; - // Construct the process noise variance diagonal for those states with a stationary process model // These are kinematic states and their error growth is controlled separately by the IMU noise variances - // earth frame magnetic field states - 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); - // wind velocity states - process_noise.slice<2, 1>(22, 0) = sq(wind_vel_nsd_scaled) * dt; - - // add process noise that is not from the IMU - for (unsigned i = 16; i <= 23; i++) { - nextP(i, i) += process_noise(i); - } - // gyro bias: add process noise, or restore previous gyro bias var if state inhibited const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f); const float gyro_bias_process_noise = sq(gyro_bias_sig); - for (unsigned i = 10; i <= 12; i++) { - const int axis_index = i - 10; + for (unsigned index = 0; index < State::gyro_bias.dof; index++) { + const unsigned i = State::gyro_bias.idx + index; - if (!_gyro_bias_inhibit[axis_index]) { + if (!_gyro_bias_inhibit[index]) { nextP(i, i) += gyro_bias_process_noise; } else { - nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(axis_index)); + nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(index)); } } // accel bias: add process noise, or restore previous accel bias var if state inhibited const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f); const float accel_bias_process_noise = sq(accel_bias_sig); - for (int i = 13; i <= 15; i++) { - const int axis_index = i - 13; + for (unsigned index = 0; index < State::accel_bias.dof; index++) { + const unsigned i = State::accel_bias.idx + index; - if (!_accel_bias_inhibit[axis_index]) { + if (!_accel_bias_inhibit[index]) { nextP(i, i) += accel_bias_process_noise; } else { - nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(axis_index)); + nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(index)); + } + } + + if (_control_status.flags.mag) { + const float mag_I_process_noise = sq(mag_I_sig); + for (unsigned index = 0; index < State::mag_I.dof; index++) { + unsigned i = State::mag_I.idx + index; + nextP(i, i) += mag_I_process_noise; + } + + const float mag_B_process_noise = sq(mag_B_sig); + for (unsigned index = 0; index < State::mag_B.dof; index++) { + unsigned i = State::mag_B.idx + index; + nextP(i, i) += mag_B_process_noise; + } + + } else { + // keep previous covariance + for (unsigned i = 0; i < State::mag_I.dof; i++) { + unsigned row = State::mag_I.idx + i; + for (unsigned col = 0; col < _k_num_states; col++) { + nextP(row, col) = nextP(col, row) = P(row, col); + } + } + + for (unsigned i = 0; i < State::mag_B.dof; i++) { + unsigned row = State::mag_B.idx + i; + for (unsigned col = 0; col < _k_num_states; col++) { + nextP(row, col) = nextP(col, row) = P(row, col); + } + } + } + + if (_control_status.flags.wind) { + const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; +; + for (unsigned index = 0; index < State::wind_vel.dof; index++) { + unsigned i = State::wind_vel.idx + index; + nextP(i, i) += wind_vel_process_noise; + } + + } else { + // keep previous covariance + for (unsigned i = 0; i < State::wind_vel.dof; i++) { + unsigned row = State::wind_vel.idx + i; + for (unsigned col = 0 ; col < _k_num_states; col++) { + nextP(row, col) = nextP(col, row) = P(row, col); + } } } // covariance matrix is symmetrical, so copy upper half to lower half - for (unsigned row = 0; row <= 15; row++) { + for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0 ; column < row; column++) { P(row, column) = P(column, row) = nextP(column, row); } @@ -293,30 +323,9 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) P(row, row) = nextP(row, row); } - if (_control_status.flags.mag) { - for (unsigned row = 16; row <= 21; row++) { - for (unsigned column = 0 ; column < row; column++) { - P(row, column) = P(column, row) = nextP(column, row); - } - - P(row, row) = nextP(row, row); - } - } - - if (_control_status.flags.wind) { - for (unsigned row = 22; row <= 23; row++) { - for (unsigned column = 0 ; column < row; column++) { - P(row, column) = P(column, row) = nextP(column, row); - } - - P(row, row) = nextP(row, row); - } - } - // fix gross errors in the covariance matrix and ensure rows and // columns for un-used states are zero fixCovarianceErrors(false); - } void Ekf::fixCovarianceErrors(bool force_symmetry) @@ -336,29 +345,32 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) P_lim[6] = 1.0f; // body mag field max var P_lim[7] = 1e6f; // wind max var - for (int i = 0; i <= 3; i++) { + for (unsigned i = State::quat_nominal.idx; i < (State::quat_nominal.idx + State::quat_nominal.dof); i++) { // quaternion states P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[0]); } - for (int i = 4; i <= 6; i++) { + for (unsigned i = State::vel.idx; i < (State::vel.idx + State::vel.dof); i++) { // NED velocity states P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[1]); } - for (int i = 7; i <= 9; i++) { + for (unsigned i = State::pos.idx; i < (State::pos.idx + State::pos.dof); i++) { // NED position states P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[2]); } - for (int i = 10; i <= 12; i++) { + for (unsigned i = State::gyro_bias.idx; i < (State::gyro_bias.idx + State::gyro_bias.dof); i++) { // gyro bias states P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[3]); } // force symmetry on the quaternion, velocity and position state covariances if (force_symmetry) { - P.makeRowColSymmetric<13>(0); + P.makeRowColSymmetric(State::quat_nominal.idx); + P.makeRowColSymmetric(State::vel.idx); + P.makeRowColSymmetric(State::pos.idx); + P.makeRowColSymmetric(State::gyro_bias.idx); //TODO: needed? } // the following states are optional and are deactivated when not required @@ -371,8 +383,10 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) float maxStateVar = minSafeStateVar; bool resetRequired = false; - for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { - if (_accel_bias_inhibit[stateIndex - 13]) { + for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) { + const unsigned stateIndex = State::accel_bias.idx + axis; + + if (_accel_bias_inhibit[axis]) { // Skip the check for the inhibited axis continue; } @@ -391,8 +405,10 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) const float minStateVarTarget = 5E-8f / sq(_dt_ekf_avg); float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); - for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { - if (_accel_bias_inhibit[stateIndex - 13]) { + for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) { + const unsigned stateIndex = State::accel_bias.idx + axis; + + if (_accel_bias_inhibit[axis]) { // Skip the check for the inhibited axis continue; } @@ -402,7 +418,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero if (resetRequired) { - P.uncorrelateCovariance<3>(13); + P.uncorrelateCovariance(State::accel_bias.idx); } // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong @@ -461,7 +477,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue if (isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) { - P.uncorrelateCovariance<3>(13); + P.uncorrelateCovariance(State::accel_bias.idx); _time_acc_bias_check = _time_delayed_us; _fault_status.flags.bad_acc_bias = false; @@ -470,7 +486,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) } else if (force_symmetry) { // ensure the covariance values are symmetrical - P.makeRowColSymmetric<3>(13); + P.makeRowColSymmetric(State::accel_bias.idx); } } @@ -482,35 +498,35 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) } else { // constrain variances - for (int i = 16; i <= 18; i++) { + for (unsigned i = State::mag_I.idx; i < (State::mag_I.idx + State::mag_I.dof); i++) { P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[5]); } - for (int i = 19; i <= 21; i++) { + for (unsigned i = State::mag_B.idx; i < (State::mag_B.idx + State::mag_B.dof); i++) { P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[6]); } // force symmetry if (force_symmetry) { - P.makeRowColSymmetric<3>(16); - P.makeRowColSymmetric<3>(19); + P.makeRowColSymmetric(State::mag_I.idx); + P.makeRowColSymmetric(State::mag_B.idx); } } // wind velocity states if (!_control_status.flags.wind) { - P.uncorrelateCovarianceSetVariance<2>(22, 0.0f); + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, 0.0f); } else { // constrain variances - for (int i = 22; i <= 23; i++) { + for (unsigned i = State::wind_vel.idx; i < (State::wind_vel.idx + State::wind_vel.dof); i++) { P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[7]); } // force symmetry if (force_symmetry) { - P.makeRowColSymmetric<2>(22); + P.makeRowColSymmetric(State::wind_vel.idx); } } } @@ -543,13 +559,14 @@ void Ekf::resetQuatCov(const float yaw_noise) } // clear existing quaternion covariance - P.uncorrelateCovarianceSetVariance<2>(0, 0.0f); - P.uncorrelateCovarianceSetVariance<2>(2, 0.0f); + // Optimization: avoid the creation of a <4> function + P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx, 0.0f); + P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx + 2, 0.0f); - matrix::SquareMatrix q_cov; + matrix::SquareMatrix q_cov; sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov); q_cov.copyLowerToUpperTriangle(); - P.slice<4, 4>(0, 0) = q_cov; + P.slice(State::quat_nominal.idx, State::quat_nominal.idx) = q_cov; } void Ekf::resetMagCov() @@ -560,8 +577,8 @@ void Ekf::resetMagCov() _mag_decl_cov_reset = false; } - P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise)); - P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise)); + P.uncorrelateCovarianceSetVariance(State::mag_I.idx, sq(_params.mag_noise)); + P.uncorrelateCovarianceSetVariance(State::mag_B.idx, sq(_params.mag_noise)); saveMagCovData(); #else @@ -575,5 +592,5 @@ void Ekf::resetGyroBiasZCov() { const float init_gyro_bias_var = sq(_params.switch_on_gyro_bias); - P.uncorrelateCovarianceSetVariance<1>(12, init_gyro_bias_var); + P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, init_gyro_bias_var); } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index aa286ca03d..a28402c7c4 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -49,6 +49,7 @@ #include "bias_estimator.hpp" #include "height_bias_estimator.hpp" #include "position_bias_estimator.hpp" +#include "python/ekf_derivation/generated/state.h" #include #include @@ -407,12 +408,12 @@ public: // gyro bias (states 10, 11, 12) const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s - Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)}; } // get the gyroscope bias variance in rad/s + Vector3f getGyroBiasVariance() const { return P.slice(State::gyro_bias.idx, State::gyro_bias.idx).diag(); } // get the gyroscope bias variance in rad/s float getGyroBiasLimit() const { return _params.gyro_bias_lim; } // accel bias (states 13, 14, 15) const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2 - Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)}; } // get the accelerometer bias variance in m/s**2 + Vector3f getAccelBiasVariance() const { return P.slice(State::accel_bias.idx, State::accel_bias.idx).diag(); } // get the accelerometer bias variance in m/s**2 float getAccelBiasLimit() const { return _params.acc_bias_lim; } #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -423,7 +424,7 @@ public: Vector3f getMagBiasVariance() const { if (_control_status.flags.mag) { - return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; + return P.slice(State::mag_B.idx, State::mag_B.idx).diag(); } return _saved_mag_bf_covmat.diag(); @@ -870,7 +871,7 @@ private: #endif // CONFIG_EKF2_DRAG_FUSION // fuse single velocity and position measurement - bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index); + bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index); void resetVelocityTo(const Vector3f &vel, const Vector3f &new_vel_var); @@ -1197,7 +1198,7 @@ private: void resetFakePosFusion(); void stopFakePosFusion(); - void setVelPosStatus(const int index, const bool healthy); + void setVelPosStatus(const int state_index, const bool healthy); // reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch // yaw : Euler yaw angle (rad) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 7ae756e430..b12527447e 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -67,11 +67,11 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f _state.vel.xy() = new_horz_vel; if (PX4_ISFINITE(new_horz_vel_var(0))) { - P.uncorrelateCovarianceSetVariance<1>(4, math::max(sq(0.01f), new_horz_vel_var(0))); + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0))); } if (PX4_ISFINITE(new_horz_vel_var(1))) { - P.uncorrelateCovarianceSetVariance<1>(5, math::max(sq(0.01f), new_horz_vel_var(1))); + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1))); } _output_predictor.resetHorizontalVelocityTo(delta_horz_vel); @@ -97,7 +97,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) _state.vel(2) = new_vert_vel; if (PX4_ISFINITE(new_vert_vel_var)) { - P.uncorrelateCovarianceSetVariance<1>(6, math::max(sq(0.01f), new_vert_vel_var)); + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var)); } _output_predictor.resetVerticalVelocityTo(delta_vert_vel); @@ -133,11 +133,11 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f _state.pos.xy() = new_horz_pos; if (PX4_ISFINITE(new_horz_pos_var(0))) { - P.uncorrelateCovarianceSetVariance<1>(7, math::max(sq(0.01f), new_horz_pos_var(0))); + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0))); } if (PX4_ISFINITE(new_horz_pos_var(1))) { - P.uncorrelateCovarianceSetVariance<1>(8, math::max(sq(0.01f), new_horz_pos_var(1))); + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1))); } _output_predictor.resetHorizontalPositionTo(delta_horz_pos); @@ -180,7 +180,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v if (PX4_ISFINITE(new_vert_pos_var)) { // the state variance is the same as the observation - P.uncorrelateCovarianceSetVariance<1>(9, math::max(sq(0.01f), new_vert_pos_var)); + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var)); } const float delta_z = new_vert_pos - old_vert_pos; @@ -369,14 +369,14 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const { matrix::Vector state; - state.slice<4, 1>(0, 0) = _state.quat_nominal; - state.slice<3, 1>(4, 0) = _state.vel; - state.slice<3, 1>(7, 0) = _state.pos; - state.slice<3, 1>(10, 0) = _state.gyro_bias; - state.slice<3, 1>(13, 0) = _state.accel_bias; - state.slice<3, 1>(16, 0) = _state.mag_I; - state.slice<3, 1>(19, 0) = _state.mag_B; - state.slice<2, 1>(22, 0) = _state.wind_vel; + state.slice(State::quat_nominal.idx, 0) = _state.quat_nominal; + state.slice(State::vel.idx, 0) = _state.vel; + state.slice(State::pos.idx, 0) = _state.pos; + state.slice(State::gyro_bias.idx, 0) = _state.gyro_bias; + state.slice(State::accel_bias.idx, 0) = _state.accel_bias; + state.slice(State::mag_I.idx, 0) = _state.mag_I; + state.slice(State::mag_B.idx, 0) = _state.mag_B; + state.slice(State::wind_vel.idx, 0) = _state.wind_vel; return state; } @@ -466,7 +466,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const // report absolute accuracy taking into account the uncertainty in location of the origin // If not aiding, return 0 for horizontal position estimate as no estimate is available // TODO - allow for baro drift in vertical position error - float hpos_err = sqrtf(P(7, 7) + P(8, 8) + sq(_gpos_origin_eph)); + float hpos_err = sqrtf(P.trace<2>(State::pos.idx) + sq(_gpos_origin_eph)); // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors @@ -484,14 +484,14 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const } *ekf_eph = hpos_err; - *ekf_epv = sqrtf(P(9, 9) + sq(_gpos_origin_epv)); + *ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2) + sq(_gpos_origin_epv)); } // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const { // TODO - allow for baro drift in vertical position error - float hpos_err = sqrtf(P(7, 7) + P(8, 8)); + float hpos_err = sqrtf(P.trace<2>(State::pos.idx)); // If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors @@ -509,13 +509,13 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const } *ekf_eph = hpos_err; - *ekf_epv = sqrtf(P(9, 9)); + *ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2)); } // get the 1-sigma horizontal and vertical velocity uncertainty void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const { - float hvel_err = sqrtf(P(4, 4) + P(5, 5)); + float hvel_err = sqrtf(P.trace<2>(State::vel.idx)); // If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal velocity error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors @@ -548,7 +548,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const } *ekf_evh = hvel_err; - *ekf_evv = sqrtf(P(6, 6)); + *ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2)); } /* @@ -618,10 +618,10 @@ void Ekf::resetGyroBias() // Zero the corresponding covariances and set // variances to the values use for initial alignment - P.uncorrelateCovarianceSetVariance<3>(10, sq(_params.switch_on_gyro_bias)); + P.uncorrelateCovarianceSetVariance(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias)); // Set previous frame values - _prev_gyro_bias_var = P.slice<3, 3>(10, 10).diag(); + _prev_gyro_bias_var = P.slice(State::gyro_bias.idx, State::gyro_bias.idx).diag(); } void Ekf::resetAccelBias() @@ -631,10 +631,10 @@ void Ekf::resetAccelBias() // Zero the corresponding covariances and set // variances to the values use for initial alignment - P.uncorrelateCovarianceSetVariance<3>(13, sq(_params.switch_on_accel_bias)); + P.uncorrelateCovarianceSetVariance(State::accel_bias.idx, sq(_params.switch_on_accel_bias)); // Set previous frame values - _prev_accel_bias_var = P.slice<3, 3>(13, 13).diag(); + _prev_accel_bias_var = P.slice(State::accel_bias.idx, State::accel_bias.idx).diag(); } // get EKF innovation consistency check status information comprising of: @@ -812,23 +812,22 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const void Ekf::fuse(const Vector24f &K, float innovation) { - _state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation; + _state.quat_nominal -= K.slice(State::quat_nominal.idx, 0) * innovation; _state.quat_nominal.normalize(); _R_to_earth = Dcmf(_state.quat_nominal); - _state.vel -= K.slice<3, 1>(4, 0) * innovation; - _state.pos -= K.slice<3, 1>(7, 0) * innovation; - _state.gyro_bias -= K.slice<3, 1>(10, 0) * innovation; - _state.accel_bias -= K.slice<3, 1>(13, 0) * innovation; - _state.mag_I -= K.slice<3, 1>(16, 0) * innovation; - _state.mag_B -= K.slice<3, 1>(19, 0) * innovation; - _state.wind_vel -= K.slice<2, 1>(22, 0) * innovation; + _state.vel -= K.slice(State::vel.idx, 0) * innovation; + _state.pos -= K.slice(State::pos.idx, 0) * innovation; + _state.gyro_bias -= K.slice(State::gyro_bias.idx, 0) * innovation; + _state.accel_bias -= K.slice(State::accel_bias.idx, 0) * innovation; + _state.mag_I -= K.slice(State::mag_I.idx, 0) * innovation; + _state.mag_B -= K.slice(State::mag_B.idx, 0) * innovation; + _state.wind_vel -= K.slice(State::wind_vel.idx, 0) * innovation; } void Ekf::uncorrelateQuatFromOtherStates() { - P.slice<_k_num_states - 4, 4>(4, 0) = 0.f; - P.slice<4, _k_num_states - 4>(0, 4) = 0.f; + P.uncorrelateCovarianceBlock(State::quat_nominal.idx); } void Ekf::updateDeadReckoningStatus() @@ -1070,5 +1069,5 @@ void Ekf::resetWindToZero() _state.wind_vel.setZero(); // start with a small initial uncertainty to improve the initial estimate - P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty); + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, _params.initial_wind_uncertainty); } diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index 9dffdc8e38..d4b7f8fa16 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -94,7 +94,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com if (measurement_valid && quality_sufficient) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); - bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9)); + bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VPOS)) diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index fc4e2db633..fd055175bb 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -164,7 +164,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common if (measurement_valid && quality_sufficient) { _ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1)))); _ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO - _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(P(7, 7), P(8, 8))); + _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + P.slice<2, 2>(State::pos.idx, State::pos.idx).diag()); } if (!measurement_valid) { diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index 56b75a054b..011118a7e6 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -82,7 +82,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) if (measurement_valid && gps_checks_passing && !gps_checks_failing) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9)); + bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding diff --git a/src/modules/ekf2/EKF/mag_3d_control.cpp b/src/modules/ekf2/EKF/mag_3d_control.cpp index 5b8f483555..b45621697f 100644 --- a/src/modules/ekf2/EKF/mag_3d_control.cpp +++ b/src/modules/ekf2/EKF/mag_3d_control.cpp @@ -220,19 +220,19 @@ void Ekf::stopMagFusion() void Ekf::saveMagCovData() { // save the NED axis covariance sub-matrix - _saved_mag_ef_covmat = P.slice<3, 3>(16, 16); + _saved_mag_ef_covmat = P.slice(State::mag_I.idx, State::mag_I.idx); // save the XYZ body covariance sub-matrix - _saved_mag_bf_covmat = P.slice<3, 3>(19, 19); + _saved_mag_bf_covmat = P.slice(State::mag_B.idx, State::mag_B.idx); } void Ekf::loadMagCovData() { // re-instate the NED axis covariance sub-matrix - P.uncorrelateCovarianceSetVariance<3>(16, 0.f); - P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat; + P.uncorrelateCovarianceSetVariance(State::mag_I.idx, 0.f); + P.slice(State::mag_I.idx, State::mag_I.idx) = _saved_mag_ef_covmat; // re-instate the XYZ body axis covariance sub-matrix - P.uncorrelateCovarianceSetVariance<3>(19, 0.f); - P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat; + P.uncorrelateCovarianceSetVariance(State::mag_B.idx, 0.f); + P.slice(State::mag_B.idx, State::mag_B.idx) = _saved_mag_bf_covmat; } diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 3fdef210c2..d23b9a33e4 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -558,3 +558,12 @@ generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"]) generate_px4_function(quat_var_to_rot_var, output_names=["rot_var"]) generate_px4_function(rot_var_ned_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"]) + +generate_px4_state({"quat_nominal": sf.V4, + "vel": sf.V3, + "pos": sf.V3, + "gyro_bias": sf.V3, + "accel_bias": sf.V3, + "mag_I": sf.V3, + "mag_B": sf.V3, + "wind_vel": sf.V2}) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py index ca2987e00f..9213c70f0a 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py @@ -87,3 +87,31 @@ def generate_python_function(function_name, output_names): metadata = codegen.generate_function( output_dir="generated", skip_directory_nesting=True) + +def generate_px4_state(states): + print("Generate EKF state definition") + filename = "state.h" + f = open(f"./generated/{filename}", "w") + header = ["// --------------------------------------------------\n", + "// This file was autogenerated, do NOT modify by hand\n", + "// --------------------------------------------------\n", + "\n#ifndef EKF_STATE_H", + "\n#define EKF_STATE_H\n\n", + "namespace estimator\n{\n"] + f.writelines(header) + + f.write("struct IdxDof { unsigned idx; unsigned dof; };\n"); + + f.write("namespace State {\n"); + + start_index = 0 + for (state_name, state_type) in states.items(): + tangent_dim = state_type.tangent_dim() + f.write(f"\tstatic constexpr IdxDof {state_name}{{{start_index}, {tangent_dim}}};\n") + start_index += tangent_dim + + f.write("};\n") # namespace State + f.write("}\n") # namespace estimator + f.write("#endif // !EKF_STATE_H\n") + f.close() + print(f" |- {filename}") diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h new file mode 100644 index 0000000000..5f3a0759f2 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h @@ -0,0 +1,22 @@ +// -------------------------------------------------- +// This file was autogenerated, do NOT modify by hand +// -------------------------------------------------- + +#ifndef EKF_STATE_H +#define EKF_STATE_H + +namespace estimator +{ +struct IdxDof { unsigned idx; unsigned dof; }; +namespace State { + static constexpr IdxDof quat_nominal{0, 4}; + static constexpr IdxDof vel{4, 3}; + static constexpr IdxDof pos{7, 3}; + static constexpr IdxDof gyro_bias{10, 3}; + static constexpr IdxDof accel_bias{13, 3}; + static constexpr IdxDof mag_I{16, 3}; + static constexpr IdxDof mag_B{19, 3}; + static constexpr IdxDof wind_vel{22, 2}; +}; +} +#endif // !EKF_STATE_H diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index aafcfce3f6..be3c26db7b 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -64,13 +64,13 @@ void Ekf::controlRangeHeightFusion() // Run the kinematic consistency check when not moving horizontally if (_control_status.flags.in_air && !_control_status.flags.fixed_wing - && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) { + && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P.trace<2>(State::vel.idx), 0.1f))) { const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); const float var = sq(_params.range_noise) + dist_dependant_var; _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _time_delayed_us); + _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), _time_delayed_us); } } else { @@ -117,7 +117,7 @@ void Ekf::controlRangeHeightFusion() if (measurement_valid && _range_sensor.isDataHealthy()) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9)); + bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index a33f1cae6e..c8fd3dc88c 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -182,7 +182,7 @@ void Ekf::controlHaglRngFusion() float Ekf::getRngVar() const { - return fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f) + return fmaxf(P(State::pos.idx + 2, State::pos.idx + 2) * _params.vehicle_variance_scaler, 0.0f) + sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getRange()); } diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index e2822a90fe..e596571bae 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -53,7 +53,8 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &ob aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - aid_src.innovation_variance[i] = P(4 + i, 4 + i) + aid_src.observation_variance[i]; + const int state_index = State::vel.idx + i; + aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; } setEstimatorAidStatusTestRatio(aid_src, innov_gate); @@ -71,7 +72,8 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - aid_src.innovation_variance[i] = P(4 + i, 4 + i) + aid_src.observation_variance[i]; + const int state_index = State::vel.idx + i; + aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; } setEstimatorAidStatusTestRatio(aid_src, innov_gate); @@ -96,7 +98,7 @@ void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const floa aid_src.innovation = _state.pos(2) - aid_src.observation; aid_src.observation_variance = math::max(sq(0.01f), obs_var); - aid_src.innovation_variance = P(9, 9) + aid_src.observation_variance; + aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance; setEstimatorAidStatusTestRatio(aid_src, innov_gate); @@ -121,7 +123,8 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i]; aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - aid_src.innovation_variance[i] = P(7 + i, 7 + i) + aid_src.observation_variance[i]; + const int state_index = State::pos.idx + i; + aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; } setEstimatorAidStatusTestRatio(aid_src, innov_gate); @@ -133,8 +136,8 @@ void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) { if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { // vx, vy - if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 0) - && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 1) + if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx) + && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -149,9 +152,9 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) { if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { // vx, vy, vz - if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 0) - && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 1) - && fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], 2) + if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx) + && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) + && fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -166,8 +169,8 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) { // x & y if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { - if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 3) - && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 4) + if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx) + && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -182,7 +185,7 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) { // z if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { - if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, 5)) { + if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; } @@ -190,10 +193,9 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) } // Helper function that fuses a single velocity or position measurement -bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) +bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int state_index) { Vector24f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used. - const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state // calculate kalman gain K = PHS, where S = 1/innovation variance for (int row = 0; row < _k_num_states; row++) { @@ -212,7 +214,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o const bool healthy = checkAndFixCovarianceUpdate(KHP); - setVelPosStatus(obs_index, healthy); + setVelPosStatus(state_index, healthy); if (healthy) { // apply the covariance corrections @@ -229,10 +231,10 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o return false; } -void Ekf::setVelPosStatus(const int index, const bool healthy) +void Ekf::setVelPosStatus(const int state_index, const bool healthy) { - switch (index) { - case 0: + switch (state_index) { + case State::vel.idx: if (healthy) { _fault_status.flags.bad_vel_N = false; _time_last_hor_vel_fuse = _time_delayed_us; @@ -243,7 +245,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 1: + case State::vel.idx + 1: if (healthy) { _fault_status.flags.bad_vel_E = false; _time_last_hor_vel_fuse = _time_delayed_us; @@ -254,7 +256,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 2: + case State::vel.idx + 2: if (healthy) { _fault_status.flags.bad_vel_D = false; _time_last_ver_vel_fuse = _time_delayed_us; @@ -265,7 +267,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 3: + case State::pos.idx: if (healthy) { _fault_status.flags.bad_pos_N = false; _time_last_hor_pos_fuse = _time_delayed_us; @@ -276,7 +278,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 4: + case State::pos.idx + 1: if (healthy) { _fault_status.flags.bad_pos_E = false; _time_last_hor_pos_fuse = _time_delayed_us; @@ -287,7 +289,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 5: + case State::pos.idx + 2: if (healthy) { _fault_status.flags.bad_pos_D = false; _time_last_hgt_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/zero_velocity_update.cpp b/src/modules/ekf2/EKF/zero_velocity_update.cpp index 4863918a13..2e7a69a22c 100644 --- a/src/modules/ekf2/EKF/zero_velocity_update.cpp +++ b/src/modules/ekf2/EKF/zero_velocity_update.cpp @@ -55,14 +55,11 @@ void Ekf::controlZeroVelocityUpdate() // Set a low variance initially for faster leveling and higher // later to let the states follow the measurements const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f); - Vector3f innov_var{ - P(4, 4) + obs_var, - P(5, 5) + obs_var, - P(6, 6) + obs_var}; + Vector3f innov_var = P.slice<3, 3>(State::vel.idx, State::vel.idx).diag() + obs_var; - fuseVelPosHeight(innovation(0), innov_var(0), 0); - fuseVelPosHeight(innovation(1), innov_var(1), 1); - fuseVelPosHeight(innovation(2), innov_var(2), 2); + for (unsigned i = 0; i < 3; i++) { + fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i); + } _time_last_zero_velocity_fuse = _time_delayed_us; }