dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2-yaw_est: split imu and velocity updates

This commit is contained in:
bresch 2023-12-15 17:03:33 +01:00 committed by Mathieu Bresciani
parent bba30663cc
commit 36eb319834
3 changed files with 50 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
src/modules/ekf2/EKF/EKFGSF_yaw.cpp
View File

@ -44,13 +44,12 @@ EKFGSF_yaw::EKFGSF_yaw()
void EKFGSF_yaw::reset()
{
_vel_data_updated = false;
_ekf_gsf_vel_fuse_started = false;
_gsf_yaw_variance = INFINITY;
}
void EKFGSF_yaw::update(const imuSample &imu_sample, bool in_air)
void EKFGSF_yaw::predict(const imuSample &imu_sample, bool in_air)
{
const Vector3f accel = imu_sample.delta_vel / imu_sample.delta_vel_dt;
@ -85,64 +84,60 @@ void EKFGSF_yaw::update(const imuSample &imu_sample, bool in_air)
}
}
// we don't start running the EKF part of the algorithm until there are regular velocity observations
if (!_ekf_gsf_vel_fuse_started && _vel_data_updated) {
_vel_data_updated = false;
initialiseEKFGSF(_vel_NE, _vel_accuracy);
ahrsAlignYaw();
// don't start until in air or velocity is not negligible
if (in_air || _vel_NE.longerThan(_vel_accuracy)) {
_ekf_gsf_vel_fuse_started = true;
}
}
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
predictEKF(model_index, imu_sample.delta_ang, imu_sample.delta_ang_dt,
imu_sample.delta_vel, imu_sample.delta_vel_dt, in_air);
}
}
if (_ekf_gsf_vel_fuse_started) {
if (_vel_data_updated) {
_vel_data_updated = false;
void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, const bool in_air)
{
// we don't start running the EKF part of the algorithm until there are regular velocity observations
if (!_ekf_gsf_vel_fuse_started) {
bool bad_update = false;
initialiseEKFGSF(vel_NE, vel_accuracy);
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
// subsequent measurements are fused as direct state observations
if (!updateEKF(model_index, _vel_NE, _vel_accuracy)) {
bad_update = true;
ahrsAlignYaw();
// don't start until in air or velocity is not negligible
if (in_air || vel_NE.longerThan(vel_accuracy)) {
_ekf_gsf_vel_fuse_started = true;
}
} else {
bool bad_update = false;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
// subsequent measurements are fused as direct state observations
if (!updateEKF(model_index, vel_NE, vel_accuracy)) {
bad_update = true;
}
}
if (!bad_update) {
float total_weight = 0.0f;
// calculate weighting for each model assuming a normal distribution
const float min_weight = 1e-5f;
uint8_t n_weight_clips = 0;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
_model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index);
if (_model_weights(model_index) < min_weight) {
n_weight_clips++;
_model_weights(model_index) = min_weight;
}
total_weight += _model_weights(model_index);
}
if (!bad_update) {
float total_weight = 0.0f;
// calculate weighting for each model assuming a normal distribution
const float min_weight = 1e-5f;
uint8_t n_weight_clips = 0;
// normalise the weighting function
if (n_weight_clips < N_MODELS_EKFGSF) {
_model_weights /= total_weight;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
_model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index);
if (_model_weights(model_index) < min_weight) {
n_weight_clips++;
_model_weights(model_index) = min_weight;
}
total_weight += _model_weights(model_index);
}
// normalise the weighting function
if (n_weight_clips < N_MODELS_EKFGSF) {
_model_weights /= total_weight;
} else {
// all weights have collapsed due to excessive innovation variances so reset filters
reset();
}
} else {
// all weights have collapsed due to excessive innovation variances so reset filters
reset();
}
}
@ -481,10 +476,3 @@ Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g)
return ret;
}
void EKFGSF_yaw::setVelocity(const Vector2f &velocity, float accuracy)
{
_vel_NE = velocity;
_vel_accuracy = accuracy;
_vel_data_updated = true;
}

10
src/modules/ekf2/EKF/EKFGSF_yaw.h
View File

@ -59,10 +59,11 @@ public:
EKFGSF_yaw();
// Update Filter States - this should be called whenever new IMU data is available
void update(const imuSample &imu_sample, bool in_air = false);
void predict(const imuSample &imu_sample, bool in_air = false);
void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s)
float accuracy); // 1-sigma accuracy of velocity measurement (m/s)
void fuseVelocity(const Vector2f &vel_NE, // NE velocity measurement (m/s)
float vel_accuracy, // 1-sigma accuracy of velocity measurement (m/s)
bool in_air);
void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; }
@ -137,9 +138,6 @@ private:
matrix::Vector2f innov{}; // Velocity N,E innovation (m/s)
} _ekf_gsf[N_MODELS_EKFGSF] {};
bool _vel_data_updated{}; // true when velocity data has been updated
Vector2f _vel_NE{}; // NE velocity observations (m/s)
float _vel_accuracy{}; // 1-sigma accuracy of velocity observations (m/s)
bool _ekf_gsf_vel_fuse_started{}; // true when the EKF's have started fusing velocity data and the prediction and update processing is active
// initialise states and covariance data for the GSF and EKF filters

4
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -51,7 +51,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
}
// run EKF-GSF yaw estimator once per imu_delayed update
_yawEstimator.update(imu_delayed, _control_status.flags.in_air && !_control_status.flags.vehicle_at_rest);
_yawEstimator.predict(imu_delayed, _control_status.flags.in_air && !_control_status.flags.vehicle_at_rest);
_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
@ -224,7 +224,7 @@ void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
&& (vel_var < _params.req_sacc)
&& vel_xy.isAllFinite()) {
_yawEstimator.setVelocity(vel_xy, vel_var);
_yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air);
// Try to align yaw using estimate if available
if (((_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))