fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: update more hardcoded indexes

Browse Source
This commit is contained in:
bresch 2023-09-20 10:55:06 +02:00 committed by Daniel Agar
parent ebf962bf68
commit 9e962f3efa
4 changed files with 32 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf2/EKF/airspeed_fusion.cpp
View File

@ -200,9 +200,9 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K);
if (update_wind_only) {
for (unsigned row = 0; row <= 21; row++) {
K(row) = 0.f;
}
const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);
K.setZero();
K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
}
const bool is_fused = measurementUpdate(K, aid_src.innovation_variance, aid_src.innovation);
@ -258,7 +258,7 @@ void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample
const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw;
// it is safer to remove all existing correlations to other states at this time
P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, 0.0f);
P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -

50
src/modules/ekf2/EKF/ekf.h
View File

@ -310,22 +310,22 @@ public:
const Vector2f &getWindVelocity() const { return _state.wind_vel; };
// get the wind velocity var
Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); }
Vector2f getWindVelocityVariance() const { return P.slice<State::wind_vel.dof, State::wind_vel.dof>(State::wind_vel.idx, State::wind_vel.idx).diag(); }
// get the full covariance matrix
const matrix::SquareMatrix<float, 24> &covariances() const { return P; }
const matrix::SquareMatrix<float, State::size> &covariances() const { return P; }
// get the diagonal elements of the covariance matrix
matrix::Vector<float, 24> covariances_diagonal() const { return P.diag(); }
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<4, 4>(0, 0); }
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); }
// get the linear velocity covariances
matrix::SquareMatrix<float, 3> velocity_covariances() const { return P.slice<3, 3>(4, 4); }
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<3, 3>(7, 7); }
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,9 +356,9 @@ public:
void resetGyroBias();
void resetAccelBias();
Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); };
Vector3f getVelocityVariance() const { return velocity_covariances().diag(); };
Vector3f getPositionVariance() const { return P.slice<3, 3>(7, 7).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)
@ -406,12 +406,12 @@ public:
#endif
}
// gyro bias (states 10, 11, 12)
// 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
float getGyroBiasLimit() const { return _params.gyro_bias_lim; }
// accel bias (states 13, 14, 15)
// 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
float getAccelBiasLimit() const { return _params.acc_bias_lim; }
@ -974,38 +974,34 @@ private:
void clearInhibitedStateKalmanGains(VectorState &K) const
{
// gyro bias: states 10, 11, 12
for (unsigned i = 0; i < 3; i++) {
for (unsigned i = 0; i < State::gyro_bias.dof; i++) {
if (_gyro_bias_inhibit[i]) {
K(10 + i) = 0.f;
K(State::gyro_bias.idx + i) = 0.f;
}
}
// accel bias: states 13, 14, 15
for (unsigned i = 0; i < 3; i++) {
for (unsigned i = 0; i < State::accel_bias.dof; i++) {
if (_accel_bias_inhibit[i]) {
K(13 + i) = 0.f;
K(State::accel_bias.idx + i) = 0.f;
}
}
// mag I: states 16, 17, 18
if (!_control_status.flags.mag) {
K(16) = 0.f;
K(17) = 0.f;
K(18) = 0.f;
for (unsigned i = 0; i < State::mag_I.dof; i++) {
K(State::mag_I.idx + i) = 0.f;
}
}
// mag B: states 19, 20, 21
if (!_control_status.flags.mag) {
K(19) = 0.f;
K(20) = 0.f;
K(21) = 0.f;
for (unsigned i = 0; i < State::mag_B.dof; i++) {
K(State::mag_B.idx + i) = 0.f;
}
}
// wind: states 22, 23
if (!_control_status.flags.wind) {
K(22) = 0.f;
K(23) = 0.f;
for (unsigned i = 0; i < State::wind_vel.dof; i++) {
K(State::wind_vel.idx + i) = 0.f;
}
}
}

6
src/modules/ekf2/EKF/sideslip_fusion.cpp
View File

@ -141,9 +141,9 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K);
if (update_wind_only) {
for (unsigned row = 0; row <= 21; row++) {
K(row) = 0.f;
}
const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);
K.setZero();
K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
}
const bool is_fused = measurementUpdate(K, sideslip.innovation_variance, sideslip.innovation);

7
src/modules/ekf2/EKF/zero_gyro_update.cpp
View File

@ -59,10 +59,7 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed)
const float obs_var = sq(math::constrain(_params.gyro_noise, 0.f, 1.f));
Vector3f innov_var{
P(10, 10) + obs_var,
P(11, 11) + obs_var,
P(12, 12) + obs_var};
const Vector3f innov_var = getGyroBiasVariance() + obs_var;
for (int i = 0; i < 3; i++) {
fuseDeltaAngBias(innovation(i), innov_var(i), i);
@ -83,7 +80,7 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed)
void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int obs_index)
{
VectorState K; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = obs_index + 10;
const unsigned state_index = obs_index + State::gyro_bias.idx;
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row < State::size; row++) {