Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 14:24:21 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

ekf2: access state covariance using enum

Browse Source
This commit is contained in:
bresch
2023-09-01 17:28:58 +02:00
committed by Daniel Agar
parent 9c41c06325
commit 779ea3f4d1
15 changed files with 256 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/baro_height_control.cpp
+1 -1
View File
@@ -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
src/modules/ekf2/EKF/covariance.cpp
+114 -97
View File
@@ -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.dof>(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.dof>(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.dof>(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.dof>(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.dof>(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<int32_t>(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.dof>(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.dof>(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.dof>(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.dof>(State::quat_nominal.idx);
P.makeRowColSymmetric<State::vel.dof>(State::vel.idx);
P.makeRowColSymmetric<State::pos.dof>(State::pos.idx);
P.makeRowColSymmetric<State::gyro_bias.dof>(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.dof>(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.dof>(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.dof>(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.dof>(State::mag_I.idx);
P.makeRowColSymmetric<State::mag_B.dof>(State::mag_B.idx);
}
}
// wind velocity states
if (!_control_status.flags.wind) {
P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(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.dof>(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<float, 4> q_cov;
matrix::SquareMatrix<float, State::quat_nominal.dof> 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.dof, State::quat_nominal.dof>(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.dof>(State::mag_I.idx, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(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);
}
src/modules/ekf2/EKF/ekf.h
+6 -5
View File
@@ -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 <uORB/topics/estimator_aid_source1d.h>
#include <uORB/topics/estimator_aid_source2d.h>
@@ -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.dof, State::gyro_bias.dof>(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.dof, State::accel_bias.dof>(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.dof, State::mag_B.dof>(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)
src/modules/ekf2/EKF/ekf_helper.cpp
+34 -35
View File
@@ -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<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
{
matrix::Vector<float, 24> 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.dof, 1>(State::quat_nominal.idx, 0) = _state.quat_nominal;
state.slice<State::vel.dof, 1>(State::vel.idx, 0) = _state.vel;
state.slice<State::pos.dof, 1>(State::pos.idx, 0) = _state.pos;
state.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) = _state.gyro_bias;
state.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) = _state.accel_bias;
state.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = _state.mag_I;
state.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = _state.mag_B;
state.slice<State::wind_vel.dof, 1>(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.dof>(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.dof, State::gyro_bias.dof>(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.dof>(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.dof, State::accel_bias.dof>(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.dof, 1>(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.dof, 1>(State::vel.idx, 0) * innovation;
_state.pos -= K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation;
_state.gyro_bias -= K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) * innovation;
_state.accel_bias -= K.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) * innovation;
_state.mag_I -= K.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) * innovation;
_state.mag_B -= K.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) * innovation;
_state.wind_vel -= K.slice<State::wind_vel.dof, 1>(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.dof>(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.dof>(State::wind_vel.idx, _params.initial_wind_uncertainty);
}
src/modules/ekf2/EKF/ev_height_control.cpp
+1 -1
View File
@@ -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<int32_t>(EvCtrl::VPOS))
src/modules/ekf2/EKF/ev_pos_control.cpp
+1 -1
View File
@@ -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) {
src/modules/ekf2/EKF/gnss_height_control.cpp
+1 -1
View File
@@ -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
src/modules/ekf2/EKF/mag_3d_control.cpp
+6 -6
View File
@@ -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.dof, State::mag_I.dof>(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.dof, State::mag_B.dof>(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.dof>(State::mag_I.idx, 0.f);
P.slice<State::mag_I.dof, State::mag_I.dof>(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.dof>(State::mag_B.idx, 0.f);
P.slice<State::mag_B.dof, State::mag_B.dof>(State::mag_B.idx, State::mag_B.idx) = _saved_mag_bf_covmat;
}
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+9
View File
@@ -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})
src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py
+28
View File
@@ -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}")
src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h
+22
View File
@@ -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
src/modules/ekf2/EKF/range_height_control.cpp
+3 -3
View File
@@ -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
src/modules/ekf2/EKF/terrain_estimator.cpp
+1 -1
View File
@@ -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());
}
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+25 -23
View File
@@ -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;
src/modules/ekf2/EKF/zero_velocity_update.cpp
+4 -7
View File
@@ -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;
}