fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 11:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: access state covariance using helper functions

Browse Source
This commit is contained in:
bresch
2023-09-26 06:41:04 +02:00
committed by Mathieu Bresciani
parent 619616b9f0
commit 99197919d7
11 changed files with 36 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+3 -8
View File
@@ -541,15 +541,10 @@ void Ekf::resetQuatCov(const float yaw_noise)
void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
{
// clear existing quaternion covariance
// 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, State::quat_nominal.dof> q_cov;
sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov);
q_cov.copyLowerToUpperTriangle();
P.slice<State::quat_nominal.dof, State::quat_nominal.dof>(State::quat_nominal.idx, State::quat_nominal.idx) = q_cov;
resetStateCovariance<State::quat_nominal>(q_cov);
}
void Ekf::resetMagCov()
@@ -565,8 +560,8 @@ void Ekf::resetMagCov()
saveMagCovData();
#else
P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.f);
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.f);
#endif
}
src/modules/ekf2/EKF/ekf.h
+18 -17
View File
@@ -308,9 +308,13 @@ public:
// get the wind velocity in m/s
const Vector2f &getWindVelocity() const { return _state.wind_vel; };
Vector2f getWindVelocityVariance() const { return getStateVariance<State::wind_vel>(); }
// get the wind velocity var
Vector2f getWindVelocityVariance() const { return P.slice<State::wind_vel.dof, State::wind_vel.dof>(State::wind_vel.idx, State::wind_vel.idx).diag(); }
template <const IdxDof &S>
matrix::Vector<float, S.dof>getStateVariance() const { return P.slice<S.dof, S.dof>(S.idx, S.idx).diag(); } // calling getStateCovariance().diag() uses more flash space
template <const IdxDof &S>
matrix::SquareMatrix<float, S.dof>getStateCovariance() const { return P.slice<S.dof, S.dof>(S.idx, S.idx); }
// get the full covariance matrix
const matrix::SquareMatrix<float, State::size> &covariances() const { return P; }
@@ -318,14 +322,10 @@ public:
// get the diagonal elements of the covariance matrix
matrix::Vector<float, State::size> covariances_diagonal() const { return P.diag(); }
// get the orientation (quaterion) covariances
matrix::SquareMatrix<float, 4> orientation_covariances() const { return P.slice<State::quat_nominal.dof, State::quat_nominal.dof>(State::quat_nominal.idx, State::quat_nominal.idx); }
matrix::Vector<float, State::quat_nominal.dof> getQuaternionVariance() const { return getStateVariance<State::quat_nominal>(); }
Vector3f getVelocityVariance() const { return getStateVariance<State::vel>(); };
Vector3f getPositionVariance() const { return getStateVariance<State::pos>(); }
// get the linear velocity covariances
matrix::SquareMatrix<float, 3> velocity_covariances() const { return P.slice<State::vel.dof, State::vel.dof>(State::vel.idx, State::vel.idx); }
// get the position covariances
matrix::SquareMatrix<float, 3> position_covariances() const { return P.slice<State::pos.dof, State::pos.dof>(State::pos.idx, State::pos.idx); }
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
bool collect_gps(const gpsMessage &gps) override;
@@ -356,10 +356,6 @@ public:
void resetGyroBias();
void resetAccelBias();
Vector3f getVelocityVariance() const { return velocity_covariances().diag(); };
Vector3f getPositionVariance() const { return position_covariances().diag(); }
// First argument returns GPS drift metrics in the following array locations
// 0 : Horizontal position drift rate (m/s)
// 1 : Vertical position drift rate (m/s)
@@ -408,23 +404,22 @@ public:
// gyro bias
const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias 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
Vector3f getGyroBiasVariance() const { return getStateVariance<State::gyro_bias>(); } // get the gyroscope bias variance in rad/s
float getGyroBiasLimit() const { return _params.gyro_bias_lim; }
// accel bias
const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias 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
Vector3f getAccelBiasVariance() const { return getStateVariance<State::accel_bias>(); } // get the accelerometer bias variance in m/s**2
float getAccelBiasLimit() const { return _params.acc_bias_lim; }
#if defined(CONFIG_EKF2_MAGNETOMETER)
const Vector3f &getMagEarthField() const { return _state.mag_I; }
// mag bias (states 19, 20, 21)
const Vector3f &getMagBias() const { return _state.mag_B; }
Vector3f getMagBiasVariance() const
{
if (_control_status.flags.mag) {
return P.slice<State::mag_B.dof, State::mag_B.dof>(State::mag_B.idx, State::mag_B.idx).diag();
return getStateVariance<State::mag_B>();
}
return _saved_mag_bf_covmat.diag();
@@ -805,6 +800,12 @@ private:
// predict ekf covariance
void predictCovariance(const imuSample &imu_delayed);
template <const IdxDof &S>
void resetStateCovariance(const matrix::SquareMatrix<float, S.dof> &cov) {
P.uncorrelateCovarianceSetVariance<S.dof>(S.idx, 0.0f);
P.slice<S.dof, S.dof>(S.idx, S.idx) = cov;
}
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
bool fuseYaw(estimator_aid_source1d_s &aid_src_status);
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
src/modules/ekf2/EKF/ekf_helper.cpp
+2 -2
View File
@@ -625,7 +625,7 @@ void Ekf::resetGyroBias()
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<State::gyro_bias.dof, State::gyro_bias.dof>(State::gyro_bias.idx, State::gyro_bias.idx).diag();
_prev_gyro_bias_var = getStateVariance<State::gyro_bias>();
}
void Ekf::resetAccelBias()
@@ -638,7 +638,7 @@ void Ekf::resetAccelBias()
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<State::accel_bias.dof, State::accel_bias.dof>(State::accel_bias.idx, State::accel_bias.idx).diag();
_prev_accel_bias_var = getStateVariance<State::accel_bias>();
}
// get EKF innovation consistency check status information comprising of:
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 + P.slice<2, 2>(State::pos.idx, State::pos.idx).diag());
_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(getStateVariance<State::pos>()));
}
if (!measurement_valid) {
src/modules/ekf2/EKF/mag_3d_control.cpp
+6 -8
View File
@@ -167,8 +167,8 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
|| wmm_updated
|| !_mag_decl_cov_reset
|| !_state.mag_I.longerThan(0.f)
|| (P.slice<3, 3>(16, 16).diag().min() < sq(0.0001f)) // mag_I
|| (P.slice<3, 3>(19, 19).diag().min() < sq(0.0001f)) // mag_B
|| (getStateVariance<State::mag_I>().min() < sq(0.0001f))
|| (getStateVariance<State::mag_B>().min() < sq(0.0001f))
) {
ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
@@ -220,19 +220,17 @@ void Ekf::stopMagFusion()
void Ekf::saveMagCovData()
{
// save the NED axis covariance sub-matrix
_saved_mag_ef_covmat = P.slice<State::mag_I.dof, State::mag_I.dof>(State::mag_I.idx, State::mag_I.idx);
_saved_mag_ef_covmat = getStateCovariance<State::mag_I>();
// save the XYZ body covariance sub-matrix
_saved_mag_bf_covmat = P.slice<State::mag_B.dof, State::mag_B.dof>(State::mag_B.idx, State::mag_B.idx);
_saved_mag_bf_covmat = getStateCovariance<State::mag_B>();
}
void Ekf::loadMagCovData()
{
// re-instate the NED axis covariance sub-matrix
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;
resetStateCovariance<State::mag_I>(_saved_mag_ef_covmat);
// re-instate the XYZ body axis covariance sub-matrix
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;
resetStateCovariance<State::mag_B>(_saved_mag_bf_covmat);
}
src/modules/ekf2/EKF/zero_velocity_update.cpp
+1 -1
View File
@@ -55,7 +55,7 @@ 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.slice<3, 3>(State::vel.idx, State::vel.idx).diag() + obs_var;
Vector3f innov_var = getVelocityVariance() + obs_var;
for (unsigned i = 0; i < 3; i++) {
fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i);