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

ekf2: variable to parameter name consistency (#25042)

Browse Source 
Rename various EKF2 variable names to match the PX4 parameter names
This commit is contained in:
Jacob Dahl
2025-06-24 09:15:50 +02:00
committed by GitHub
parent 9e90fd193f
commit 95119027a9
38 changed files with 449 additions and 446 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp
+7 -7
View File
@@ -58,7 +58,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
const bool sideslip_timed_out = isTimedOut(_aid_src_sideslip.time_last_fuse, (uint64_t)10e6);
if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.drag_ctrl == 0))) {
if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.ekf2_drag_ctrl == 0))) {
_control_status.flags.wind = false;
}
@@ -72,7 +72,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
if (_control_status.flags.fixed_wing) {
if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
if (!_control_status.flags.fuse_aspd) {
_yawEstimator.setTrueAirspeed(_params.EKFGSF_tas_default);
_yawEstimator.setTrueAirspeed(_params.ekf2_gsf_tas);
}
} else {
@@ -82,7 +82,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
#endif // CONFIG_EKF2_GNSS
if (_params.arsp_thr <= 0.f) {
if (_params.ekf2_arsp_thr <= 0.f) {
stopAirspeedFusion();
return;
}
@@ -99,7 +99,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
const bool continuing_conditions_passing = _control_status.flags.in_air
&& !_control_status.flags.fake_pos;
const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr;
const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.ekf2_arsp_thr;
const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f);
const bool starting_conditions_passing = continuing_conditions_passing
&& is_airspeed_significant
@@ -151,7 +151,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const
{
// Variance for true airspeed measurement - (m/sec)^2
const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
const float R = sq(math::constrain(_params.ekf2_eas_noise, 0.5f, 5.0f) *
math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
float innov = 0.f;
@@ -165,7 +165,7 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
R, // observation variance
innov, // innovation
innov_var, // innovation variance
math::max(_params.tas_innov_gate, 1.f)); // innovation gate
math::max(_params.ekf2_tas_gate, 1.f)); // innovation gate
}
void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src)
@@ -243,7 +243,7 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
constexpr float sideslip_var = sq(math::radians(15.0f));
const float euler_yaw = getEulerYaw(_R_to_earth);
const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f)
const float airspeed_var = sq(math::constrain(_params.ekf2_eas_noise, 0.5f, 5.0f)
* math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
matrix::SquareMatrix<float, State::wind_vel.dof> P_wind;
src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
+9 -9
View File
@@ -57,7 +57,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
const float measurement = baro_sample.hgt;
#endif
const float measurement_var = sq(_params.baro_noise);
const float measurement_var = sq(_params.ekf2_baro_noise);
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
@@ -83,14 +83,14 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
baro_sample.time_us,
-(measurement - bias_est.getBias()), // observation
measurement_var + bias_est.getBiasVar(), // observation variance
math::max(_params.baro_innov_gate, 1.f)); // innovation gate
math::max(_params.ekf2_baro_gate, 1.f)); // innovation gate
// Compensate for positive static pressure transients (negative vertical position innovations)
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
if (_control_status.flags.gnd_effect && (_params.gnd_effect_deadzone > 0.f)) {
if (_control_status.flags.gnd_effect && (_params.ekf2_gnd_eff_dz > 0.f)) {
const float deadzone_start = 0.0f;
const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
const float deadzone_end = deadzone_start + _params.ekf2_gnd_eff_dz;
if (aid_src.innovation < -deadzone_start) {
if (aid_src.innovation <= -deadzone_end) {
@@ -111,7 +111,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
}
// determine if we should use height aiding
const bool continuing_conditions_passing = (_params.baro_ctrl == 1)
const bool continuing_conditions_passing = (_params.ekf2_baro_ctrl == 1)
&& measurement_valid
&& (_baro_counter > _obs_buffer_length)
&& !_control_status.flags.baro_fault;
@@ -222,11 +222,11 @@ float Ekf::compensateBaroForDynamicPressure(const imuSample &imu_sample, const f
const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);
const Vector3f K_pstatic_coef(
airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn,
airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
_params.static_pressure_coef_z);
airspeed_body(0) >= 0.f ? _params.ekf2_pcoef_xp : _params.ekf2_pcoef_xn,
airspeed_body(1) >= 0.f ? _params.ekf2_pcoef_yp : _params.ekf2_pcoef_yn,
_params.ekf2_pcoef_z);
const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.ekf2_aspd_max));
const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));
src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp
+10 -8
View File
@@ -45,7 +45,7 @@
void Ekf::controlDragFusion(const imuSample &imu_delayed)
{
if ((_params.drag_ctrl > 0) && _drag_buffer) {
if ((_params.ekf2_drag_ctrl > 0) && _drag_buffer) {
if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) {
_control_status.flags.wind = true;
@@ -65,17 +65,19 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed)
void Ekf::fuseDrag(const dragSample &drag_sample)
{
const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2
const float R_ACC = fmaxf(_params.ekf2_drag_noise,
0.5f); // observation noise variance in specific force drag (m/sec**2)**2
const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3)
// correct rotor momentum drag for increase in required rotor mass flow with altitude
// obtained from momentum disc theory
const float mcoef_corrrected = fmaxf(_params.mcoef * sqrtf(rho / atmosphere::kAirDensitySeaLevelStandardAtmos), 0.f);
const float mcoef_corrrected = fmaxf(_params.ekf2_mcoef * sqrtf(rho / atmosphere::kAirDensitySeaLevelStandardAtmos),
0.f);
// drag model parameters
const bool using_bcoef_x = _params.bcoef_x > 1.0f;
const bool using_bcoef_y = _params.bcoef_y > 1.0f;
const bool using_mcoef = _params.mcoef > 0.001f;
const bool using_bcoef_x = _params.ekf2_bcoef_x > 1.0f;
const bool using_bcoef_y = _params.ekf2_bcoef_y > 1.0f;
const bool using_mcoef = _params.ekf2_mcoef > 0.001f;
if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) {
return;
@@ -92,11 +94,11 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
Vector2f bcoef_inv{0.f, 0.f};
if (using_bcoef_x) {
bcoef_inv(0) = 1.f / _params.bcoef_x;
bcoef_inv(0) = 1.f / _params.ekf2_bcoef_x;
}
if (using_bcoef_y) {
bcoef_inv(1) = 1.f / _params.bcoef_y;
bcoef_inv(1) = 1.f / _params.ekf2_bcoef_y;
}
if (using_bcoef_x && using_bcoef_y) {
src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp
+8 -8
View File
@@ -52,14 +52,14 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);
// determine if we should use the horizontal position observations
bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (ev_sample.quality >= _params.ev_quality_minimum);
bool quality_sufficient = (_params.ekf2_ev_qmin <= 0) || (ev_sample.quality >= _params.ekf2_ev_qmin);
const bool starting_conditions_passing = quality_sufficient
&& ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL)
&& ((_params.ev_quality_minimum <= 0)
|| (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
&& ((_params.ev_quality_minimum <= 0)
|| (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
&& ((_params.ekf2_ev_qmin <= 0)
|| (_ev_sample_prev.quality >= _params.ekf2_ev_qmin)) // previous quality sufficient
&& ((_params.ekf2_ev_qmin <= 0)
|| (_ext_vision_buffer->get_newest().quality >= _params.ekf2_ev_qmin)) // newest quality sufficient
&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
updateEvAttitudeErrorFilter(ev_sample, ev_reset);
@@ -68,19 +68,19 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
switch (ev_sample.vel_frame) {
case VelocityFrame::BODY_FRAME_FRD: {
EvVelBodyFrameFrd ev_vel_body(*this, ev_sample, _params.ev_vel_noise, imu_sample);
EvVelBodyFrameFrd ev_vel_body(*this, ev_sample, _params.ekf2_evv_noise, imu_sample);
controlEvVelFusion(ev_vel_body, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
break;
}
case VelocityFrame::LOCAL_FRAME_NED: {
EvVelLocalFrameNed ev_vel_ned(*this, ev_sample, _params.ev_vel_noise, imu_sample);
EvVelLocalFrameNed ev_vel_ned(*this, ev_sample, _params.ekf2_evv_noise, imu_sample);
controlEvVelFusion(ev_vel_ned, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
break;
}
case VelocityFrame::LOCAL_FRAME_FRD: {
EvVelLocalFrameFrd ev_vel_frd(*this, ev_sample, _params.ev_vel_noise, imu_sample);
EvVelLocalFrameFrd ev_vel_frd(*this, ev_sample, _params.ekf2_evv_noise, imu_sample);
controlEvVelFusion(ev_vel_frd, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
break;
}
src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp
+4 -4
View File
@@ -75,13 +75,13 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
}
const float measurement = pos(2) - pos_offset_earth(2);
float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f));
float measurement_var = math::max(pos_cov(2, 2), sq(_params.ekf2_evp_noise), sq(0.01f));
#if defined(CONFIG_EKF2_GNSS)
// increase minimum variance if GPS active
if (_control_status.flags.gps_hgt) {
measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise));
measurement_var = math::max(measurement_var, sq(_params.ekf2_gps_p_noise));
}
#endif // CONFIG_EKF2_GNSS
@@ -92,7 +92,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
ev_sample.time_us,
measurement - bias_est.getBias(), // observation
measurement_var + bias_est.getBiasVar(), // observation variance
math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate
math::max(_params.ekf2_evp_gate, 1.f)); // innovation gate
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
@@ -102,7 +102,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
bias_est.fuseBias(measurement + _gpos.altitude(), 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))
const bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
&& measurement_valid;
const bool starting_conditions_passing = common_starting_conditions_passing
src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp
+5 -5
View File
@@ -49,7 +49,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
// determine if we should use EV position aiding
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
&& _control_status.flags.tilt_align
&& PX4_ISFINITE(ev_sample.pos(0))
&& PX4_ISFINITE(ev_sample.pos(1));
@@ -131,7 +131,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
// increase minimum variance if GNSS is active (position reference)
if (_control_status.flags.gnss_pos) {
for (int i = 0; i < 2; i++) {
pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.ekf2_gps_p_noise));
}
}
@@ -142,8 +142,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
const Vector2f measurement{pos(0), pos(1)};
const Vector2f measurement_var{
math::max(pos_cov(0, 0), sq(_params.ev_pos_noise), sq(0.01f)),
math::max(pos_cov(1, 1), sq(_params.ev_pos_noise), sq(0.01f))
math::max(pos_cov(0, 0), sq(_params.ekf2_evp_noise), sq(0.01f)),
math::max(pos_cov(1, 1), sq(_params.ekf2_evp_noise), sq(0.01f))
};
const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
@@ -169,7 +169,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
pos_obs_var, // observation variance
position_estimate - position, // innovation
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate
math::max(_params.ekf2_evp_gate, 1.f)); // innovation gate
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h
+8 -8
View File
@@ -45,10 +45,10 @@
class ExternalVisionVel
{
public:
ExternalVisionVel(Ekf &ekf_instance, const extVisionSample &vision_sample, const float ev_vel_noise,
ExternalVisionVel(Ekf &ekf_instance, const extVisionSample &vision_sample, const float ekf2_evv_noise,
const imuSample &imu_sample) : _ekf(ekf_instance), _sample(vision_sample)
{
_min_variance = sq(ev_vel_noise);
_min_variance = sq(ekf2_evv_noise);
const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _ekf._state.gyro_bias;
Vector3f position_offset_body = _ekf._params.ev_pos_body - _ekf._params.imu_pos_body;
_velocity_offset_body = angular_velocity % position_offset_body;
@@ -93,9 +93,9 @@ public:
class EvVelBodyFrameFrd : public ExternalVisionVel
{
public:
EvVelBodyFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
EvVelBodyFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ekf2_evv_noise,
const imuSample &imu_sample) :
ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample)
ExternalVisionVel(ekf_instance, vision_sample, ekf2_evv_noise, imu_sample)
{
_measurement = _sample.vel - _velocity_offset_body;
_measurement_var = _sample.velocity_var;
@@ -132,9 +132,9 @@ public:
class EvVelLocalFrameNed : public ExternalVisionVel
{
public:
EvVelLocalFrameNed(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
EvVelLocalFrameNed(Ekf &ekf_instance, extVisionSample &vision_sample, const float ekf2_evv_noise,
const imuSample &imu_sample) :
ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample)
ExternalVisionVel(ekf_instance, vision_sample, ekf2_evv_noise, imu_sample)
{
const Vector3f velocity_offset_earth = _ekf._R_to_earth * _velocity_offset_body;
@@ -149,9 +149,9 @@ public:
class EvVelLocalFrameFrd : public ExternalVisionVel
{
public:
EvVelLocalFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
EvVelLocalFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ekf2_evv_noise,
const imuSample &imu_sample) :
ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample)
ExternalVisionVel(ekf_instance, vision_sample, ekf2_evv_noise, imu_sample)
{
const Vector3f velocity_offset_earth = _ekf._R_to_earth * _velocity_offset_body;
src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp
+3 -3
View File
@@ -51,15 +51,15 @@ void Ekf::controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_c
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
// determine if we should use EV velocity aiding
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
&& _control_status.flags.tilt_align
&& ev._sample.vel.isAllFinite()
&& !ev._sample.vel.longerThan(_params.velocity_limit);
&& !ev._sample.vel.longerThan(_params.ekf2_vel_lim);
continuing_conditions_passing &= ev._measurement.isAllFinite() && ev._measurement_var.isAllFinite();
float gate = math::max(_params.ev_vel_innov_gate, 1.f);
float gate = math::max(_params.ekf2_evv_gate, 1.f);
if (_control_status.flags.ev_vel) {
if (continuing_conditions_passing) {
src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp
+3 -3
View File
@@ -45,7 +45,7 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
static constexpr const char *AID_SRC_NAME = "EV yaw";
float obs = getEulerYaw(ev_sample.quat);
float obs_var = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
float obs_var = math::max(ev_sample.orientation_var(2), _params.ekf2_eva_noise, sq(0.01f));
float innov = wrap_pi(getEulerYaw(_R_to_earth) - obs);
float innov_var = 0.f;
@@ -59,14 +59,14 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
obs_var, // observation variance
innov, // innovation
innov_var, // innovation variance
math::max(_params.heading_innov_gate, 1.f)); // innovation gate
math::max(_params.ekf2_hdg_gate, 1.f)); // innovation gate
if (ev_reset) {
_control_status.flags.ev_yaw_fault = false;
}
// determine if we should use EV yaw aiding
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
&& _control_status.flags.tilt_align
&& !_control_status.flags.ev_yaw_fault
&& PX4_ISFINITE(aid_src.observation)
src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp
+2 -2
View File
@@ -48,7 +48,7 @@ void Ekf::controlFakeHgtFusion()
if (fake_hgt_data_ready) {
const float obs_var = sq(_params.pos_noaid_noise);
const float obs_var = sq(_params.ekf2_noaid_noise);
const float innov_gate = 3.f;
updateVerticalPositionAidStatus(aid_src, _time_delayed_us, -_last_known_gpos.altitude(), obs_var, innov_gate);
@@ -110,7 +110,7 @@ void Ekf::resetHeightToLastKnown()
{
_information_events.flags.reset_pos_to_last_known = true;
ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude());
resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise));
resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.ekf2_noaid_noise));
}
void Ekf::stopFakeHgtFusion()
src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp
+1 -1
View File
@@ -52,7 +52,7 @@ void Ekf::controlFakePosFusion()
Vector2f obs_var;
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, 1.f));
obs_var(0) = obs_var(1) = sq(fmaxf(_params.ekf2_noaid_noise, 1.f));
} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
// Accelerate tilt fine alignment by fusing more
src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp
+15 -15
View File
@@ -113,29 +113,29 @@ bool GnssChecks::runInitialFixChecks(const gnssSample &gnss)
_check_fail_status.flags.fix = (gnss.fix_type < 3);
// Check the number of satellites
_check_fail_status.flags.nsats = (gnss.nsats < _params.req_nsats);
_check_fail_status.flags.nsats = (gnss.nsats < _params.ekf2_req_nsats);
// Check the position dilution of precision
_check_fail_status.flags.pdop = (gnss.pdop > _params.req_pdop);
_check_fail_status.flags.pdop = (gnss.pdop > _params.ekf2_req_pdop);
// Check the reported horizontal and vertical position accuracy
_check_fail_status.flags.hacc = (gnss.hacc > _params.req_hacc);
_check_fail_status.flags.vacc = (gnss.vacc > _params.req_vacc);
_check_fail_status.flags.hacc = (gnss.hacc > _params.ekf2_req_eph);
_check_fail_status.flags.vacc = (gnss.vacc > _params.ekf2_req_epv);
// Check the reported speed accuracy
_check_fail_status.flags.sacc = (gnss.sacc > _params.req_sacc);
_check_fail_status.flags.sacc = (gnss.sacc > _params.ekf2_req_sacc);
_check_fail_status.flags.spoofed = gnss.spoofed;
runOnGroundGnssChecks(gnss);
// force horizontal speed failure if above the limit
if (gnss.vel.xy().longerThan(_params.velocity_limit)) {
if (gnss.vel.xy().longerThan(_params.ekf2_vel_lim)) {
_check_fail_status.flags.hspeed = true;
}
// force vertical speed failure if above the limit
if (fabsf(gnss.vel(2)) > _params.velocity_limit) {
if (fabsf(gnss.vel(2)) > _params.ekf2_vel_lim) {
_check_fail_status.flags.vspeed = true;
}
@@ -198,7 +198,7 @@ void GnssChecks::runOnGroundGnssChecks(const gnssSample &gnss)
}
// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
const Vector3f vel_limit(_params.ekf2_req_hdrift, _params.ekf2_req_hdrift, _params.ekf2_req_vdrift);
Vector3f delta_pos(delta_pos_n, delta_pos_e, (_alt_prev - gnss.alt));
// Apply a low pass filter
@@ -209,26 +209,26 @@ void GnssChecks::runOnGroundGnssChecks(const gnssSample &gnss)
// hdrift: calculate the horizontal drift speed and fail if too high
_horizontal_position_drift_rate_m_s = Vector2f(_lat_lon_alt_deriv_filt.xy()).norm();
_check_fail_status.flags.hdrift = (_horizontal_position_drift_rate_m_s > _params.req_hdrift);
_check_fail_status.flags.hdrift = (_horizontal_position_drift_rate_m_s > _params.ekf2_req_hdrift);
// vdrift: fail if the vertical drift speed is too high
_vertical_position_drift_rate_m_s = fabsf(_lat_lon_alt_deriv_filt(2));
_check_fail_status.flags.vdrift = (_vertical_position_drift_rate_m_s > _params.req_vdrift);
_check_fail_status.flags.vdrift = (_vertical_position_drift_rate_m_s > _params.ekf2_req_vdrift);
// hspeed: check the magnitude of the filtered horizontal GNSS velocity
const Vector2f vel_ne = matrix::constrain(Vector2f(gnss.vel.xy()),
-10.0f * _params.req_hdrift,
10.0f * _params.req_hdrift);
-10.0f * _params.ekf2_req_hdrift,
10.0f * _params.ekf2_req_hdrift);
_vel_ne_filt = vel_ne * filter_coef + _vel_ne_filt * (1.0f - filter_coef);
_filtered_horizontal_velocity_m_s = _vel_ne_filt.norm();
_check_fail_status.flags.hspeed = (_filtered_horizontal_velocity_m_s > _params.req_hdrift);
_check_fail_status.flags.hspeed = (_filtered_horizontal_velocity_m_s > _params.ekf2_req_hdrift);
// vspeed: check the magnitude of the filtered vertical GNSS velocity
const float gnss_vz_limit = 10.f * _params.req_vdrift;
const float gnss_vz_limit = 10.f * _params.ekf2_req_vdrift;
const float gnss_vz = math::constrain(gnss.vel(2), -gnss_vz_limit, gnss_vz_limit);
_vel_d_filt = gnss_vz * filter_coef + _vel_d_filt * (1.f - filter_coef);
_check_fail_status.flags.vspeed = (fabsf(_vel_d_filt) > _params.req_vdrift);
_check_fail_status.flags.vspeed = (fabsf(_vel_d_filt) > _params.ekf2_req_vdrift);
} else {
// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation
src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp
+12 -11
View File
@@ -43,10 +43,11 @@ namespace estimator
class GnssChecks final
{
public:
GnssChecks(int32_t &check_mask, int32_t &req_nsats, float &req_pdop, float &req_hacc, float &req_vacc, float &req_sacc,
float &req_hdrift, float &req_vdrift, float &velocity_limit, uint32_t &min_health_time_us,
GnssChecks(int32_t &check_mask, int32_t &ekf2_req_nsats, float &ekf2_req_pdop, float &ekf2_req_eph, float &ekf2_req_epv,
float &ekf2_req_sacc,
float &ekf2_req_hdrift, float &ekf2_req_vdrift, float &ekf2_vel_lim, uint32_t &min_health_time_us,
filter_control_status_u &control_status):
_params{check_mask, req_nsats, req_pdop, req_hacc, req_vacc, req_sacc, req_hdrift, req_vdrift, velocity_limit, min_health_time_us},
_params{check_mask, ekf2_req_nsats, ekf2_req_pdop, ekf2_req_eph, ekf2_req_epv, ekf2_req_sacc, ekf2_req_hdrift, ekf2_req_vdrift, ekf2_vel_lim, min_health_time_us},
_control_status(control_status)
{};
@@ -143,14 +144,14 @@ private:
struct Params {
const int32_t &check_mask;
const int32_t &req_nsats;
const float &req_pdop;
const float &req_hacc;
const float &req_vacc;
const float &req_sacc;
const float &req_hdrift;
const float &req_vdrift;
const float &velocity_limit;
const int32_t &ekf2_req_nsats;
const float &ekf2_req_pdop;
const float &ekf2_req_eph;
const float &ekf2_req_epv;
const float &ekf2_req_sacc;
const float &ekf2_req_hdrift;
const float &ekf2_req_vdrift;
const float &ekf2_vel_lim;
const uint32_t &min_health_time_us;
};
src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
+5 -5
View File
@@ -50,13 +50,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
if (_gps_data_ready) {
// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
float noise = math::max(gps_sample.vacc, 1.5f * _params.gps_pos_noise); // use 1.5 as a typical ratio of vacc/hacc
float noise = math::max(gps_sample.vacc, 1.5f * _params.ekf2_gps_p_noise); // use 1.5 as a typical ratio of vacc/hacc
if (!isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.gps_hgt)) {
// if we are not using another source of aiding, then we are reliant on the GPS
// observations to constrain attitude errors and must limit the observation noise value.
if (noise > _params.pos_noaid_noise) {
noise = _params.pos_noaid_noise;
if (noise > _params.ekf2_noaid_noise) {
noise = _params.ekf2_noaid_noise;
}
}
@@ -74,7 +74,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
gps_sample.time_us,
-(measurement - bias_est.getBias()),
measurement_var + bias_est.getBiasVar(),
math::max(_params.gps_pos_innov_gate, 1.f));
math::max(_params.ekf2_gps_p_gate, 1.f));
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
@@ -89,7 +89,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
&& _local_origin_lat_lon.isInitialized()
&& _gnss_checks.passed();
const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
&& common_conditions_passing;
const bool starting_conditions_passing = continuing_conditions_passing
src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp
+2 -2
View File
@@ -47,7 +47,7 @@
void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
{
if (!(_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
if (!(_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
|| _control_status.flags.gnss_yaw_fault) {
stopGnssYawFusion();
@@ -151,7 +151,7 @@ void Ekf::updateGnssYaw(const gnssSample &gnss_sample)
R_YAW, // observation variance
wrap_pi(heading_pred - measured_hdg), // innovation
heading_innov_var, // innovation variance
math::max(_params.heading_innov_gate, 1.f)); // innovation gate
math::max(_params.ekf2_hdg_gate, 1.f)); // innovation gate
}
void Ekf::fuseGnssYaw(float antenna_yaw_offset)
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
+14 -14
View File
@@ -41,7 +41,7 @@
void Ekf::controlGpsFusion(const imuSample &imu_delayed)
{
if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
if (!_gps_buffer || (_params.ekf2_gps_ctrl == 0)) {
stopGnssFusion();
return;
}
@@ -124,7 +124,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool force_reset)
{
const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
&& _control_status.flags.tilt_align
&& _control_status.flags.yaw_align;
const bool starting_conditions_passing = continuing_conditions_passing && _gnss_checks.passed();
@@ -169,7 +169,7 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for
void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset)
{
const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
const bool gnss_pos_enabled = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
const bool continuing_conditions_passing = gnss_pos_enabled
&& _control_status.flags.tilt_align
@@ -215,10 +215,10 @@ void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_samp
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f));
const float vel_var = sq(math::max(gnss_sample.sacc, _params.ekf2_gps_v_noise, 0.01f));
const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f);
const float innovation_gate = math::max(_params.ekf2_gps_v_gate, 1.f);
updateAidSourceStatus(aid_src,
gnss_sample.time_us, // sample timestamp
@@ -235,7 +235,7 @@ void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_samp
&& (aid_src.test_ratio[0] < 1.f) && (aid_src.test_ratio[1] < 1.f); // vx & vy accepted
if (bad_acc_vz_rejected
&& (gnss_sample.sacc < _params.req_sacc)
&& (gnss_sample.sacc < _params.ekf2_req_sacc)
) {
const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]);
aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
@@ -253,13 +253,13 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
const Vector2f innovation = (_gpos - measurement_corrected).xy();
// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);
float pos_noise = math::max(gnss_sample.hacc, _params.ekf2_gps_p_noise);
if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_pos)) {
// if we are not using another source of aiding, then we are reliant on the GNSS
// observations to constrain attitude errors and must limit the observation noise value.
if (pos_noise > _params.pos_noaid_noise) {
pos_noise = _params.pos_noaid_noise;
if (pos_noise > _params.ekf2_noaid_noise) {
pos_noise = _params.ekf2_noaid_noise;
}
}
@@ -273,7 +273,7 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
pos_obs_var, // observation variance
innovation, // innovation
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate
math::max(_params.ekf2_gps_p_gate, 1.f)); // innovation gate
}
void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
@@ -283,14 +283,14 @@ void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
const Vector2f vel_xy(aid_src_vel.observation);
if ((vel_var > 0.f)
&& (vel_var < _params.req_sacc)
&& (vel_var < _params.ekf2_req_sacc)
&& vel_xy.isAllFinite()) {
_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))
|| (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)))
if (((_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
|| (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)))
&& !_control_status.flags.yaw_align
&& _control_status.flags.tilt_align) {
if (resetYawToEKFGSF()) {
@@ -379,7 +379,7 @@ bool Ekf::shouldResetGpsFusion() const
const bool is_reset_required = has_horizontal_aiding_timed_out
|| (isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max)
&& (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)));
&& (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)));
const bool is_inflight_nav_failure = _control_status.flags.in_air
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp
+1 -1
View File
@@ -50,7 +50,7 @@ void Ekf::controlGravityFusion(const imuSample &imu)
{
// get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian)
const Vector3f measurement = Vector3f(imu.delta_vel / imu.delta_vel_dt - _state.accel_bias).unit();
const float measurement_var = math::max(sq(_params.gravity_noise), sq(0.01f));
const float measurement_var = math::max(sq(_params.ekf2_grav_noise), sq(0.01f));
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
+30 -30
View File
@@ -54,7 +54,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
_control_status.flags.mag_aligned_in_flight = false;
}
if (_params.mag_fusion_type == MagFuseType::NONE) {
if (_params.ekf2_mag_type == MagFuseType::NONE) {
stopMagFusion();
return;
}
@@ -114,7 +114,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
// this is useful if there is a lot of interference on the sensor measurement.
if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
if (_params.ekf2_synt_mag_z && (_params.ekf2_decl_type & GeoDeclinationMask::USE_GEO_DECL)
&& (_wmm_earth_field_gauss.isAllFinite() && _wmm_earth_field_gauss.longerThan(0.f))
) {
mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, _wmm_earth_field_gauss);
@@ -131,7 +131,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
_fault_status.flags.bad_mag_z = false;
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
const float R_MAG = math::max(sq(_params.ekf2_mag_noise), sq(0.01f));
// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
Vector3f mag_innov;
@@ -147,7 +147,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
Vector3f(R_MAG, R_MAG, R_MAG), // observation variance
mag_innov, // innovation
innov_var, // innovation variance
math::max(_params.mag_innov_gate, 1.f)); // innovation gate
math::max(_params.ekf2_mag_gate, 1.f)); // innovation gate
// Perform an innovation consistency check and report the result
_innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f);
@@ -155,9 +155,9 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
_innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f);
// determine if we should use mag fusion
bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::INIT)
|| (_params.mag_fusion_type == MagFuseType::AUTO)
|| (_params.mag_fusion_type == MagFuseType::HEADING))
bool continuing_conditions_passing = ((_params.ekf2_mag_type == MagFuseType::INIT)
|| (_params.ekf2_mag_type == MagFuseType::AUTO)
|| (_params.ekf2_mag_type == MagFuseType::HEADING))
&& _control_status.flags.tilt_align
&& (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
&& mag_sample.mag.longerThan(0.f)
@@ -184,12 +184,12 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
&& !_control_status.flags.gnss_yaw;
_control_status.flags.mag_3D = common_conditions_passing
&& (_params.mag_fusion_type == MagFuseType::AUTO)
&& (_params.ekf2_mag_type == MagFuseType::AUTO)
&& _control_status.flags.mag_aligned_in_flight;
_control_status.flags.mag_hdg = common_conditions_passing
&& ((_params.mag_fusion_type == MagFuseType::HEADING)
|| (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
&& ((_params.ekf2_mag_type == MagFuseType::HEADING)
|| (_params.ekf2_mag_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
}
// TODO: allow clearing mag_fault if mag_3d is good?
@@ -237,17 +237,17 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
// observation variance (rad**2)
const float R_DECL = sq(0.5f);
if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
if ((_params.ekf2_decl_type & GeoDeclinationMask::USE_GEO_DECL)
&& PX4_ISFINITE(_wmm_declination_rad)
) {
// using declination from the world magnetic model
fuseDeclination(_wmm_declination_rad, 0.5f, update_all_states, update_tilt);
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
} else if ((_params.ekf2_decl_type & GeoDeclinationMask::SAVE_GEO_DECL)
&& PX4_ISFINITE(_params.ekf2_mag_decl) && (fabsf(_params.ekf2_mag_decl) > 0.f)
) {
// using previously saved declination
fuseDeclination(math::radians(_params.mag_declination_deg), R_DECL, update_all_states, update_tilt);
fuseDeclination(math::radians(_params.ekf2_mag_decl), R_DECL, update_all_states, update_tilt);
} else {
// if there is no aiding coming from an inertial frame we need to fuse some declination
@@ -460,7 +460,7 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
Vector3f mag_bias{0.f, 0.f, 0.f};
const Vector3f mag_bias_var = getMagBiasVariance();
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.ekf2_mag_noise))) {
mag_bias = _state.mag_B;
}
@@ -482,10 +482,10 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
_mag_heading_innov_lpf.reset(0.f);
}
if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
if (fabsf(_mag_heading_innov_lpf.getState()) < _params.ekf2_head_noise) {
// Check if there has been enough change in horizontal velocity to make yaw observable
if (isNorthEastAidingActive() && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) {
if (isNorthEastAidingActive() && (_accel_horiz_lpf.getState().longerThan(_params.ekf2_mag_acclim))) {
// yaw angle must be observable to consider consistency
_control_status.flags.mag_heading_consistent = true;
}
@@ -499,7 +499,7 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
{
_control_status.flags.mag_field_disturbed = false;
if (_params.mag_check == 0) {
if (_params.ekf2_mag_check == 0) {
// skip all checks
return true;
}
@@ -507,14 +507,14 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
bool is_check_failing = false;
_mag_strength = mag_sample.length();
if (_params.mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
if (_params.ekf2_mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
if (PX4_ISFINITE(_wmm_field_strength_gauss)) {
if (!isMeasuredMatchingExpected(_mag_strength, _wmm_field_strength_gauss, _params.mag_check_strength_tolerance_gs)) {
if (!isMeasuredMatchingExpected(_mag_strength, _wmm_field_strength_gauss, _params.ekf2_mag_chk_str)) {
_control_status.flags.mag_field_disturbed = true;
is_check_failing = true;
}
} else if (_params.mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
} else if (_params.ekf2_mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
is_check_failing = true;
} else {
@@ -531,9 +531,9 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
const Vector3f mag_earth = _R_to_earth * mag_sample;
_mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));
if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
if (_params.ekf2_mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
if (PX4_ISFINITE(_wmm_inclination_rad)) {
const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
const float inc_tol_rad = radians(_params.ekf2_mag_chk_inc);
const float inc_error_rad = wrap_pi(_mag_inclination - _wmm_inclination_rad);
if (fabsf(inc_error_rad) > inc_tol_rad) {
@@ -541,7 +541,7 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
is_check_failing = true;
}
} else if (_params.mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
} else if (_params.ekf2_mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
is_check_failing = true;
} else {
@@ -569,7 +569,7 @@ void Ekf::resetMagHeading(const Vector3f &mag)
Vector3f mag_bias{0.f, 0.f, 0.f};
const Vector3f mag_bias_var = getMagBiasVariance();
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.ekf2_mag_noise))) {
mag_bias = _state.mag_B;
}
@@ -583,7 +583,7 @@ void Ekf::resetMagHeading(const Vector3f &mag)
// calculate the observed yaw angle and yaw variance
const float declination = getMagDeclination();
float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;
float yaw_new_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f));
float yaw_new_variance = math::max(sq(_params.ekf2_head_noise), sq(0.01f));
ECL_INFO("reset mag heading %.3f -> %.3f rad (bias:[%.3f, %.3f, %.3f], declination:%.1f)",
(double)getEulerYaw(_R_to_earth), (double)yaw_new,
@@ -606,17 +606,17 @@ float Ekf::getMagDeclination()
// Use value consistent with earth field state
return atan2f(_state.mag_I(1), _state.mag_I(0));
} else if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
} else if ((_params.ekf2_decl_type & GeoDeclinationMask::USE_GEO_DECL)
&& PX4_ISFINITE(_wmm_declination_rad)
) {
// if available use value returned by geo library
return _wmm_declination_rad;
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
} else if ((_params.ekf2_decl_type & GeoDeclinationMask::SAVE_GEO_DECL)
&& PX4_ISFINITE(_params.ekf2_mag_decl) && (fabsf(_params.ekf2_mag_decl) > 0.f)
) {
// using saved mag declination
return math::radians(_params.mag_declination_deg);
return math::radians(_params.ekf2_mag_decl);
}
// otherwise unavailable
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
+1 -1
View File
@@ -93,7 +93,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
// we need to re-initialise covariances and abort this fusion step
if (update_all_states) {
resetQuatCov(_params.mag_heading_noise);
resetQuatCov(_params.ekf2_head_noise);
}
resetMagEarthCov();
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+6 -6
View File
@@ -42,7 +42,7 @@
void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
{
if (!_flow_buffer || (_params.flow_ctrl != 1)) {
if (!_flow_buffer || (_params.ekf2_of_ctrl != 1)) {
stopFlowFusion();
return;
}
@@ -56,7 +56,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
_ref_body_rate = -(imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias());
// ensure valid flow sample gyro rate before proceeding
switch (static_cast<FlowGyroSource>(_params.flow_gyro_src)) {
switch (static_cast<FlowGyroSource>(_params.ekf2_of_gyr_src)) {
default:
/* FALLTHROUGH */
@@ -80,8 +80,8 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
const flowSample &flow_sample = _flow_sample_delayed;
const int32_t min_quality = _control_status.flags.in_air
? _params.flow_qual_min
: _params.flow_qual_min_gnd;
? _params.ekf2_of_qmin
: _params.ekf2_of_qmin_gnd;
const bool is_quality_good = (flow_sample.quality >= min_quality);
@@ -113,7 +113,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
Vector2f{R_LOS, R_LOS}, // observation variance
predictFlow(flow_gyro_corrected) - flow_compensated, // innovation
innov_var, // innovation variance
math::max(_params.flow_innov_gate, 1.f)); // innovation gate
math::max(_params.ekf2_of_gate, 1.f)); // innovation gate
// logging
_flow_rate_compensated = flow_compensated;
@@ -144,7 +144,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
&& !flow_sample.flow_rate.longerThan(_flow_max_rate)
&& !flow_compensated.longerThan(_flow_max_rate);
const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
const bool continuing_conditions_passing = (_params.ekf2_of_ctrl == 1)
&& _control_status.flags.tilt_align
&& is_within_sensor_dist;
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
+5 -5
View File
@@ -73,7 +73,7 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
// when close to the ground (singularity at 0) and the innovation can suddenly become really
// large and destabilize the filter
_aid_src_optical_flow.test_ratio[1] = sq(_aid_src_optical_flow.innovation[1]) / (sq(
_params.flow_innov_gate) * _aid_src_optical_flow.innovation_variance[1]);
_params.ekf2_of_gate) * _aid_src_optical_flow.innovation_variance[1]);
if (_aid_src_optical_flow.test_ratio[1] > 1.f) {
continue;
@@ -164,14 +164,14 @@ Vector2f Ekf::predictFlow(const Vector3f &flow_gyro) const
float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample) const
{
// calculate the observation noise variance - scaling noise linearly across flow quality range
const float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
const float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
const float R_LOS_best = fmaxf(_params.ekf2_of_n_min, 0.05f);
const float R_LOS_worst = fmaxf(_params.ekf2_of_n_max, 0.05f);
// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
float weighting = (255.f - (float)_params.flow_qual_min);
float weighting = (255.f - (float)_params.ekf2_of_qmin);
if (weighting >= 1.f) {
weighting = math::constrain((float)(flow_sample.quality - _params.flow_qual_min) / weighting, 0.f, 1.f);
weighting = math::constrain((float)(flow_sample.quality - _params.ekf2_of_qmin) / weighting, 0.f, 1.f);
} else {
weighting = 0.0f;
src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp
+3 -3
View File
@@ -49,7 +49,7 @@
void Ekf::controlBetaFusion(const imuSample &imu_delayed)
{
_control_status.flags.fuse_beta = _params.beta_fusion_enabled
_control_status.flags.fuse_beta = _params.ekf2_fuse_beta
&& (_control_status.flags.fixed_wing || _control_status.flags.fuse_aspd)
&& _control_status.flags.in_air
&& !_control_status.flags.fake_pos;
@@ -77,7 +77,7 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed)
void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
{
float observation = 0.f;
const float R = math::max(sq(_params.beta_noise), sq(0.01f)); // observation noise variance
const float R = math::max(sq(_params.ekf2_beta_noise), sq(0.01f)); // observation noise variance
const float epsilon = 1e-3f;
float innov;
float innov_var;
@@ -89,7 +89,7 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
R, // observation variance
innov, // innovation
innov_var, // innovation variance
math::max(_params.beta_innov_gate, 1.f)); // innovation gate
math::max(_params.ekf2_beta_gate, 1.f)); // innovation gate
}
bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp
+1 -1
View File
@@ -61,7 +61,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
if (!_control_status.flags.tilt_align
|| (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
|| (aid_src_status.innovation_variance - obs_var) > sq(_params.ekf2_head_noise)) {
// The yaw variance is too large, fuse fake measurement
fuseYaw(aid_src_status, H_YAW);
}
src/modules/ekf2/EKF/common.h
+92 -92
View File
@@ -100,13 +100,13 @@ enum class VelocityFrame : uint8_t {
#if defined(CONFIG_EKF2_MAGNETOMETER)
enum GeoDeclinationMask : uint8_t {
// Bit locations for mag_declination_source
// Bit locations for ekf2_decl_type
USE_GEO_DECL = (1 << 0), ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
SAVE_GEO_DECL = (1 << 1) ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
};
enum MagFuseType : uint8_t {
// Integer definitions for mag_fusion_type
// Integer definitions for ekf2_mag_type
AUTO = 0, ///< The selection of either heading or 3D magnetometer fusion will be automatic
HEADING = 1, ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
NONE = 5, ///< Do not use magnetometer under any circumstance.
@@ -266,83 +266,83 @@ struct systemFlagUpdate {
struct parameters {
int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
int32_t ekf2_predict_us{10000}; ///< filter update interval in microseconds
int32_t imu_ctrl{static_cast<int32_t>(ImuCtrl::GyroBias) | static_cast<int32_t>(ImuCtrl::AccelBias)};
float velocity_limit{100.f}; ///< velocity state limit (m/s)
float ekf2_vel_lim{100.f}; ///< velocity state limit (m/s)
// measurement source control
int32_t height_sensor_ref{static_cast<int32_t>(HeightSensor::BARO)};
int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
float delay_max_ms{110.f}; ///< maximum time delay of all the aiding sensors. Sets the size of the observation buffers. (mSec)
float ekf2_delay_max{110.f}; ///< maximum time delay of all the aiding sensors. Sets the size of the observation buffers. (mSec)
// input noise
float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec)
float accel_noise{3.5e-1f}; ///< IMU acceleration noise use for covariance prediction (m/sec**2)
float ekf2_gyr_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec)
float ekf2_acc_noise{3.5e-1f}; ///< IMU acceleration noise use for covariance prediction (m/sec**2)
// process noise
float gyro_bias_p_noise{1.0e-3f}; ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
float accel_bias_p_noise{1.0e-2f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3)
float ekf2_gyr_b_noise{1.0e-3f}; ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
float ekf2_acc_b_noise{1.0e-2f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3)
#if defined(CONFIG_EKF2_WIND)
const float initial_wind_uncertainty {1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec)
float wind_vel_nsd{1.0e-2f}; ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
float ekf2_wind_nsd{1.0e-2f}; ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
const float wind_vel_nsd_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity
#endif // CONFIG_EKF2_WIND
// initialization errors
float switch_on_gyro_bias{0.1f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
float switch_on_accel_bias{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
float initial_tilt_err{0.1f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
float ekf2_gbias_init{0.1f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
float ekf2_abias_init{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
float ekf2_angerr_init{0.1f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
#if defined(CONFIG_EKF2_BAROMETER)
int32_t baro_ctrl {1};
float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec)
float baro_noise{2.0f}; ///< observation noise for barometric height fusion (m)
int32_t ekf2_baro_ctrl {1};
float ekf2_baro_delay{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec)
float ekf2_baro_noise{2.0f}; ///< observation noise for barometric height fusion (m)
float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz))
float baro_innov_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD)
float ekf2_baro_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD)
float gnd_effect_deadzone{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
float gnd_effect_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m)
float ekf2_gnd_eff_dz{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
float ekf2_gnd_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m)
# if defined(CONFIG_EKF2_BARO_COMPENSATION)
// static barometer pressure position error coefficient along body axes
float static_pressure_coef_xp{0.0f}; // (-)
float static_pressure_coef_xn{0.0f}; // (-)
float static_pressure_coef_yp{0.0f}; // (-)
float static_pressure_coef_yn{0.0f}; // (-)
float static_pressure_coef_z{0.0f}; // (-)
float ekf2_pcoef_xp{0.0f}; // (-)
float ekf2_pcoef_xn{0.0f}; // (-)
float ekf2_pcoef_yp{0.0f}; // (-)
float ekf2_pcoef_yn{0.0f}; // (-)
float ekf2_pcoef_z{0.0f}; // (-)
// upper limit on airspeed used for correction (m/s**2)
float max_correction_airspeed{20.0f};
float ekf2_aspd_max{20.0f};
# endif // CONFIG_EKF2_BARO_COMPENSATION
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GNSS)
int32_t gnss_ctrl {static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL)};
float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
int32_t ekf2_gps_ctrl {static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL)};
float ekf2_gps_delay{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m)
// position and velocity fusion
float gps_vel_noise{0.5f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec)
float gps_pos_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m)
float ekf2_gps_v_noise{0.5f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec)
float ekf2_gps_p_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m)
float gps_hgt_bias_nsd{0.13f}; ///< process noise for gnss height bias estimation (m/s/sqrt(Hz))
float gps_pos_innov_gate{5.0f}; ///< GPS horizontal position innovation consistency gate size (STD)
float gps_vel_innov_gate{5.0f}; ///< GPS velocity innovation consistency gate size (STD)
float ekf2_gps_p_gate{5.0f}; ///< GPS horizontal position innovation consistency gate size (STD)
float ekf2_gps_v_gate{5.0f}; ///< GPS velocity innovation consistency gate size (STD)
// these parameters control the strictness of GPS quality checks used to determine if the GPS is
// good enough to set a local origin and commence aiding
int32_t gps_check_mask{21}; ///< bitmask used to control which GPS quality checks are used
float req_hacc{5.0f}; ///< maximum acceptable horizontal position error (m)
float req_vacc{8.0f}; ///< maximum acceptable vertical position error (m)
float req_sacc{1.0f}; ///< maximum acceptable speed error (m/s)
int32_t req_nsats{6}; ///< minimum acceptable satellite count
float req_pdop{2.0f}; ///< maximum acceptable position dilution of precision
float req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s)
float req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s)
int32_t ekf2_gps_check{21}; ///< bitmask used to control which GPS quality checks are used
float ekf2_req_eph{5.0f}; ///< maximum acceptable horizontal position error (m)
float ekf2_req_epv{8.0f}; ///< maximum acceptable vertical position error (m)
float ekf2_req_sacc{1.0f}; ///< maximum acceptable speed error (m/s)
int32_t ekf2_req_nsats{6}; ///< minimum acceptable satellite count
float ekf2_req_pdop{2.0f}; ///< maximum acceptable position dilution of precision
float ekf2_req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s)
float ekf2_req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s)
# if defined(CONFIG_EKF2_GNSS_YAW)
// GNSS heading fusion
@@ -350,51 +350,51 @@ struct parameters {
# endif // CONFIG_EKF2_GNSS_YAW
// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
float ekf2_gsf_tas{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
const unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
const float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
#endif // CONFIG_EKF2_GNSS
float pos_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m)
float ekf2_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m)
float heading_innov_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD)
float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad)
float ekf2_hdg_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD)
float ekf2_head_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad)
#if defined(CONFIG_EKF2_MAGNETOMETER)
float mag_delay_ms {0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec)
float ekf2_mag_delay {0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec)
float mage_p_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec)
float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec)
float ekf2_mag_e_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec)
float ekf2_mag_b_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec)
// magnetometer fusion
float mag_noise{5.0e-2f}; ///< measurement noise used for 3-axis magnetometer fusion (Gauss)
float mag_declination_deg{0.0f}; ///< magnetic declination (degrees)
float mag_innov_gate{3.0f}; ///< magnetometer fusion innovation consistency gate size (STD)
int32_t mag_declination_source{3}; ///< bitmask used to control the handling of declination data
int32_t mag_fusion_type{0}; ///< integer used to specify the type of magnetometer fusion used
float mag_acc_gate{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
float ekf2_mag_noise{5.0e-2f}; ///< measurement noise used for 3-axis magnetometer fusion (Gauss)
float ekf2_mag_decl{0.0f}; ///< magnetic declination (degrees)
float ekf2_mag_gate{3.0f}; ///< magnetometer fusion innovation consistency gate size (STD)
int32_t ekf2_decl_type{3}; ///< bitmask used to control the handling of declination data
int32_t ekf2_mag_type{0}; ///< integer used to specify the type of magnetometer fusion used
float ekf2_mag_acclim{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
// compute synthetic magnetomter Z value if possible
int32_t synthesize_mag_z{0};
int32_t mag_check{0};
float mag_check_strength_tolerance_gs{0.2f};
float mag_check_inclination_tolerance_deg{20.f};
int32_t ekf2_synt_mag_z{0};
int32_t ekf2_mag_check{0};
float ekf2_mag_chk_str{0.2f};
float ekf2_mag_chk_inc{20.f};
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_AIRSPEED)
// airspeed fusion
float airspeed_delay_ms{100.0f}; ///< airspeed measurement delay relative to the IMU (mSec)
float tas_innov_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD)
float eas_noise{1.4f}; ///< EAS measurement noise standard deviation used for airspeed fusion (m/s)
float arsp_thr{2.0f}; ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
float ekf2_asp_delay{100.0f}; ///< airspeed measurement delay relative to the IMU (mSec)
float ekf2_tas_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD)
float ekf2_eas_noise{1.4f}; ///< EAS measurement noise standard deviation used for airspeed fusion (m/s)
float ekf2_arsp_thr{2.0f}; ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
// synthetic sideslip fusion
int32_t beta_fusion_enabled{0};
float beta_innov_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
float beta_noise{0.3f}; ///< synthetic sideslip noise (rad)
int32_t ekf2_fuse_beta{0};
float ekf2_beta_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
float ekf2_beta_noise{0.3f}; ///< synthetic sideslip noise (rad)
const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec)
#endif // CONFIG_EKF2_SIDESLIP
@@ -430,15 +430,15 @@ struct parameters {
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
// vision position fusion
int32_t ev_ctrl{0};
float ev_delay_ms{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec)
int32_t ekf2_ev_ctrl{0};
float ekf2_ev_delay{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec)
float ev_vel_noise{0.1f}; ///< minimum allowed observation noise for EV velocity fusion (m/sec)
float ev_pos_noise{0.1f}; ///< minimum allowed observation noise for EV position fusion (m)
float ev_att_noise{0.1f}; ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
int32_t ev_quality_minimum{0}; ///< vision minimum acceptable quality integer
float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD)
float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD)
float ekf2_evv_noise{0.1f}; ///< minimum allowed observation noise for EV velocity fusion (m/sec)
float ekf2_evp_noise{0.1f}; ///< minimum allowed observation noise for EV position fusion (m)
float ekf2_eva_noise{0.1f}; ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
int32_t ekf2_ev_qmin{0}; ///< vision minimum acceptable quality integer
float ekf2_evv_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD)
float ekf2_evp_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD)
float ev_hgt_bias_nsd{0.13f}; ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m)
@@ -446,20 +446,20 @@ struct parameters {
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
// gravity fusion
float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2)
float ekf2_grav_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2)
#endif // CONFIG_EKF2_GRAVITY_FUSION
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
int32_t flow_ctrl {0};
int32_t flow_gyro_src {static_cast<int32_t>(FlowGyroSource::Auto)};
float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
int32_t ekf2_of_ctrl {0};
int32_t ekf2_of_gyr_src {static_cast<int32_t>(FlowGyroSource::Auto)};
float ekf2_of_delay{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
// optical flow fusion
float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec)
float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor
int32_t flow_qual_min_gnd{0}; ///< minimum acceptable quality integer from the flow sensor when on ground
float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD)
float ekf2_of_n_min{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec)
float ekf2_of_n_max{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
int32_t ekf2_of_qmin{1}; ///< minimum acceptable quality integer from the flow sensor
int32_t ekf2_of_qmin_gnd{0}; ///< minimum acceptable quality integer from the flow sensor when on ground
float ekf2_of_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD)
Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m)
#endif // CONFIG_EKF2_OPTICAL_FLOW
@@ -468,26 +468,26 @@ struct parameters {
Vector3f imu_pos_body{}; ///< xyz position of IMU in body frame (m)
// accel bias learning control
float acc_bias_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2)
float acc_bias_learn_acc_lim{25.0f}; ///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2)
float acc_bias_learn_gyr_lim{3.0f}; ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
float acc_bias_learn_tc{0.5f}; ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
float ekf2_abl_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2)
float ekf2_abl_acclim{25.0f}; ///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2)
float ekf2_abl_gyrlim{3.0f}; ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
float ekf2_abl_tau{0.5f}; ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
float gyro_bias_lim{0.4f}; ///< maximum gyro bias magnitude (rad/sec)
float ekf2_gyr_b_lim{0.4f}; ///< maximum gyro bias magnitude (rad/sec)
const unsigned reset_timeout_max{7'000'000}; ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
const unsigned no_aid_timeout_max{1'000'000}; ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
const unsigned hgt_fusion_timeout_max{5'000'000}; ///< maximum time we allow height fusion to fail before attempting a reset or stopping the fusion aiding (uSec)
int32_t valid_timeout_max{5'000'000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
int32_t ekf2_noaid_tout{5'000'000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
#if defined(CONFIG_EKF2_DRAG_FUSION)
// multi-rotor drag specific force fusion
int32_t drag_ctrl{0};
float drag_noise{2.5f}; ///< observation noise variance for drag specific force measurements (m/sec**2)**2
float bcoef_x{100.0f}; ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2)
float bcoef_y{100.0f}; ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
float mcoef{0.1f}; ///< rotor momentum drag coefficient for the X and Y axes (1/s)
int32_t ekf2_drag_ctrl{0};
float ekf2_drag_noise{2.5f}; ///< observation noise variance for drag specific force measurements (m/sec**2)**2
float ekf2_bcoef_x{100.0f}; ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2)
float ekf2_bcoef_y{100.0f}; ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
float ekf2_mcoef{0.1f}; ///< rotor momentum drag coefficient for the X and Y axes (1/s)
#endif // CONFIG_EKF2_DRAG_FUSION
// control of accel error detection and mitigation (IMU clipping)
@@ -497,7 +497,7 @@ struct parameters {
#if defined(CONFIG_EKF2_AUXVEL)
// auxiliary velocity fusion
float auxvel_delay_ms{5.0f}; ///< auxiliary velocity measurement delay relative to the IMU (mSec)
float ekf2_avel_delay{5.0f}; ///< auxiliary velocity measurement delay relative to the IMU (mSec)
const float auxvel_noise{0.5f}; ///< minimum observation noise, uses reported noise if greater (m/s)
const float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD)
#endif // CONFIG_EKF2_AUXVEL
src/modules/ekf2/EKF/covariance.cpp
+20 -20
View File
@@ -57,7 +57,7 @@ void Ekf::initialiseCovariance()
// velocity
#if defined(CONFIG_EKF2_GNSS)
const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f));
const float vel_var = sq(fmaxf(_params.ekf2_gps_v_noise, 0.01f));
#else
const float vel_var = sq(0.5f);
#endif
@@ -65,20 +65,20 @@ void Ekf::initialiseCovariance()
// position
#if defined(CONFIG_EKF2_BAROMETER)
float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f));
float z_pos_var = sq(fmaxf(_params.ekf2_baro_noise, 0.01f));
#else
float z_pos_var = sq(1.f);
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GNSS)
const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
const float xy_pos_var = sq(fmaxf(_params.ekf2_gps_p_noise, 0.01f));
if (_control_status.flags.gps_hgt) {
z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
z_pos_var = sq(fmaxf(1.5f * _params.ekf2_gps_p_noise, 0.01f));
}
#else
const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f));
const float xy_pos_var = sq(fmaxf(_params.ekf2_noaid_noise, 0.01f));
#endif
#if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -116,11 +116,11 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
const float dt = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt);
// gyro noise variance
float gyro_noise = _params.gyro_noise;
float gyro_noise = _params.ekf2_gyr_noise;
const float gyro_var = sq(gyro_noise);
// accel noise variance
float accel_noise = _params.accel_noise;
float accel_noise = _params.ekf2_acc_noise;
Vector3f accel_var;
for (unsigned i = 0; i < 3; i++) {
@@ -144,7 +144,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// gyro bias: add process noise
{
const float gyro_bias_sig = dt * _params.gyro_bias_p_noise;
const float gyro_bias_sig = dt * _params.ekf2_gyr_b_noise;
const float gyro_bias_process_noise = sq(gyro_bias_sig);
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
@@ -158,7 +158,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// accel bias: add process noise
{
const float accel_bias_sig = dt * _params.accel_bias_p_noise;
const float accel_bias_sig = dt * _params.ekf2_acc_b_noise;
const float accel_bias_process_noise = sq(accel_bias_sig);
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
@@ -173,25 +173,25 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
#if defined(CONFIG_EKF2_MAGNETOMETER)
// mag_I: add process noise
float mag_I_sig = dt * _params.mage_p_noise;
float mag_I_sig = dt * _params.ekf2_mag_e_noise;
float mag_I_process_noise = sq(mag_I_sig);
for (unsigned index = 0; index < State::mag_I.dof; index++) {
const unsigned i = State::mag_I.idx + index;
if (P(i, i) < sq(_params.mag_noise)) {
if (P(i, i) < sq(_params.ekf2_mag_noise)) {
P(i, i) += mag_I_process_noise;
}
}
// mag_B: add process noise
float mag_B_sig = dt * _params.magb_p_noise;
float mag_B_sig = dt * _params.ekf2_mag_b_noise;
float mag_B_process_noise = sq(mag_B_sig);
for (unsigned index = 0; index < State::mag_B.dof; index++) {
const unsigned i = State::mag_B.idx + index;
if (P(i, i) < sq(_params.mag_noise)) {
if (P(i, i) < sq(_params.ekf2_mag_noise)) {
P(i, i) += mag_B_process_noise;
}
}
@@ -203,7 +203,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// wind vel: add process noise
const float height_rate = _height_rate_lpf.update(_state.vel(2), imu_delayed.delta_vel_dt);
const float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(height_rate));
const float wind_vel_nsd_scaled = _params.ekf2_wind_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(height_rate));
const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
for (unsigned index = 0; index < State::wind_vel.dof; index++) {
@@ -313,7 +313,7 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max,
void Ekf::resetQuatCov(const float yaw_noise)
{
const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f));
const float tilt_var = sq(math::max(_params.ekf2_angerr_init, 0.01f));
float yaw_var = sq(0.01f);
// update the yaw angle variance using the variance of the measurement
@@ -334,19 +334,19 @@ void Ekf::resetGyroBiasCov()
{
// Zero the corresponding covariances and set
// variances to the values use for initial alignment
P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.ekf2_gbias_init));
}
void Ekf::resetGyroBiasZCov()
{
P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.ekf2_gbias_init));
}
void Ekf::resetAccelBiasCov()
{
// Zero the corresponding covariances and set
// variances to the values use for initial alignment
P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.ekf2_abias_init));
}
#if defined(CONFIG_EKF2_MAGNETOMETER)
@@ -354,13 +354,13 @@ void Ekf::resetMagEarthCov()
{
ECL_INFO("reset mag earth covariance");
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, sq(_params.ekf2_mag_noise));
}
void Ekf::resetMagBiasCov()
{
ECL_INFO("reset mag bias covariance");
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.ekf2_mag_noise));
}
#endif // CONFIG_EKF2_MAGNETOMETER
src/modules/ekf2/EKF/ekf.cpp
+9 -9
View File
@@ -151,7 +151,7 @@ bool Ekf::update()
// calculate an average filter update time
// limit input between -50% and +100% of nominal value
const float filter_update_s = 1e-6f * _params.filter_update_interval_us;
const float filter_update_s = 1e-6f * _params.ekf2_predict_us;
const float input = math::constrain(0.5f * (imu_sample_delayed.delta_vel_dt + imu_sample_delayed.delta_ang_dt),
0.5f * filter_update_s,
2.f * filter_update_s);
@@ -270,7 +270,7 @@ void Ekf::predictState(const imuSample &imu_delayed)
_state.pos(2) = -_gpos.altitude();
// constrain states
_state.vel = matrix::constrain(_state.vel, -_params.velocity_limit, _params.velocity_limit);
_state.vel = matrix::constrain(_state.vel, -_params.ekf2_vel_lim, _params.ekf2_vel_lim);
// calculate a filtered horizontal acceleration this are used for manoeuvre detection elsewhere
_accel_horiz_lpf.update(corrected_delta_vel_ef.xy() / imu_delayed.delta_vel_dt, imu_delayed.delta_vel_dt);
@@ -372,19 +372,19 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
void Ekf::updateParameters()
{
_params.gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f);
_params.accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f);
_params.ekf2_gyr_noise = math::constrain(_params.ekf2_gyr_noise, 0.f, 1.f);
_params.ekf2_acc_noise = math::constrain(_params.ekf2_acc_noise, 0.f, 1.f);
_params.gyro_bias_p_noise = math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
_params.accel_bias_p_noise = math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
_params.ekf2_gyr_b_noise = math::constrain(_params.ekf2_gyr_b_noise, 0.f, 1.f);
_params.ekf2_acc_b_noise = math::constrain(_params.ekf2_acc_b_noise, 0.f, 1.f);
#if defined(CONFIG_EKF2_MAGNETOMETER)
_params.mage_p_noise = math::constrain(_params.mage_p_noise, 0.f, 1.f);
_params.magb_p_noise = math::constrain(_params.magb_p_noise, 0.f, 1.f);
_params.ekf2_mag_e_noise = math::constrain(_params.ekf2_mag_e_noise, 0.f, 1.f);
_params.ekf2_mag_b_noise = math::constrain(_params.ekf2_mag_b_noise, 0.f, 1.f);
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
_params.wind_vel_nsd = math::constrain(_params.wind_vel_nsd, 0.f, 1.f);
_params.ekf2_wind_nsd = math::constrain(_params.ekf2_wind_nsd, 0.f, 1.f);
#endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
src/modules/ekf2/EKF/ekf.h
+3 -3
View File
@@ -266,13 +266,13 @@ public:
// gyro bias
const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias 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; }
float getGyroNoise() const { return _params.gyro_noise; }
float getGyroBiasLimit() const { return _params.ekf2_gyr_b_lim; }
float getGyroNoise() const { return _params.ekf2_gyr_noise; }
// accel bias
const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias 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; }
float getAccelBiasLimit() const { return _params.ekf2_abl_lim; }
#if defined(CONFIG_EKF2_MAGNETOMETER)
const Vector3f &getMagEarthField() const { return _state.mag_I; }
src/modules/ekf2/EKF/ekf_helper.cpp
+11 -11
View File
@@ -825,7 +825,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
inertial_dead_reckoning = false;
} else {
if (!_control_status.flags.in_air && (_params.flow_ctrl == 1)
if (!_control_status.flags.in_air && (_params.ekf2_of_ctrl == 1)
&& isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max)
) {
// currently landed, but optical flow aiding should be possible once in air
@@ -851,8 +851,8 @@ void Ekf::updateHorizontalDeadReckoningstatus()
_control_status.flags.wind_dead_reckoning = false;
if (!_control_status.flags.in_air && _control_status.flags.fixed_wing
&& (_params.beta_fusion_enabled == 1)
&& (_params.arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max)
&& (_params.ekf2_fuse_beta == 1)
&& (_params.ekf2_arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max)
) {
// currently landed, but air data aiding should be possible once in air
aiding_expected_in_air = true;
@@ -877,7 +877,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
}
if (inertial_dead_reckoning) {
if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) {
if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.ekf2_noaid_tout)) {
// deadreckon time exceeded
if (!_horizontal_deadreckon_time_exceeded) {
ECL_WARN("horizontal dead reckon time exceeded");
@@ -903,7 +903,7 @@ void Ekf::updateVerticalDeadReckoningStatus()
_time_last_v_pos_aiding = _time_last_hgt_fuse;
_vertical_position_deadreckon_time_exceeded = false;
} else if (isTimedOut(_time_last_v_pos_aiding, (uint64_t)_params.valid_timeout_max)) {
} else if (isTimedOut(_time_last_v_pos_aiding, (uint64_t)_params.ekf2_noaid_tout)) {
_vertical_position_deadreckon_time_exceeded = true;
}
@@ -911,7 +911,7 @@ void Ekf::updateVerticalDeadReckoningStatus()
_time_last_v_vel_aiding = _time_last_ver_vel_fuse;
_vertical_velocity_deadreckon_time_exceeded = false;
} else if (isTimedOut(_time_last_v_vel_aiding, (uint64_t)_params.valid_timeout_max)
} else if (isTimedOut(_time_last_v_vel_aiding, (uint64_t)_params.ekf2_noaid_tout)
&& _vertical_position_deadreckon_time_exceeded) {
_vertical_velocity_deadreckon_time_exceeded = true;
@@ -950,7 +950,7 @@ void Ekf::updateGroundEffect()
if (isTerrainEstimateValid()) {
// automatically set ground effect if terrain is valid
float height = getHagl();
_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
_control_status.flags.gnd_effect = (height < _params.ekf2_gnd_max_hgt);
} else
#endif // CONFIG_EKF2_TERRAIN
@@ -975,7 +975,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
{
const Vector3f gyro_corrected = imu_delayed.delta_ang / imu_delayed.delta_ang_dt - _state.gyro_bias;
const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f);
const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.ekf2_abl_tau), 0.f, 1.f);
const float beta = 1.f - alpha;
_ang_rate_magnitude_filt = fmaxf(gyro_corrected.norm(), beta * _ang_rate_magnitude_filt);
@@ -984,15 +984,15 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
{
const Vector3f accel_corrected = imu_delayed.delta_vel / imu_delayed.delta_vel_dt - _state.accel_bias;
const float alpha = math::constrain((imu_delayed.delta_vel_dt / _params.acc_bias_learn_tc), 0.f, 1.f);
const float alpha = math::constrain((imu_delayed.delta_vel_dt / _params.ekf2_abl_tau), 0.f, 1.f);
const float beta = 1.f - alpha;
_accel_magnitude_filt = fmaxf(accel_corrected.norm(), beta * _accel_magnitude_filt);
}
const bool is_manoeuvre_level_high = (_ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim)
|| (_accel_magnitude_filt > _params.acc_bias_learn_acc_lim);
const bool is_manoeuvre_level_high = (_ang_rate_magnitude_filt > _params.ekf2_abl_gyrlim)
|| (_accel_magnitude_filt > _params.ekf2_abl_acclim);
// gyro bias inhibit
src/modules/ekf2/EKF/estimator_interface.cpp
+12 -12
View File
@@ -97,7 +97,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
imuSample imu_downsampled = _imu_down_sampler.getDownSampledImuAndTriggerReset();
// as a precaution constrain the integration delta time to prevent numerical problems
const float filter_update_period_s = _params.filter_update_interval_us * 1e-6f;
const float filter_update_period_s = _params.ekf2_predict_us * 1e-6f;
const float imu_min_dt = 0.5f * filter_update_period_s;
const float imu_max_dt = 2.0f * filter_update_period_s;
@@ -139,7 +139,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
}
const int64_t time_us = mag_sample.time_us
- static_cast<int64_t>(_params.mag_delay_ms * 1000)
- static_cast<int64_t>(_params.ekf2_mag_delay * 1000)
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
// limit data rate to prevent data being lost
@@ -178,7 +178,7 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
}
const int64_t time_us = gnss_sample.time_us
- static_cast<int64_t>(_params.gps_delay_ms * 1000)
- static_cast<int64_t>(_params.ekf2_gps_delay * 1000)
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
if (time_us >= static_cast<int64_t>(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) {
@@ -225,7 +225,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
}
const int64_t time_us = baro_sample.time_us
- static_cast<int64_t>(_params.baro_delay_ms * 1000)
- static_cast<int64_t>(_params.ekf2_baro_delay * 1000)
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
// limit data rate to prevent data being lost
@@ -264,7 +264,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
}
const int64_t time_us = airspeed_sample.time_us
- static_cast<int64_t>(_params.airspeed_delay_ms * 1000)
- static_cast<int64_t>(_params.ekf2_asp_delay * 1000)
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
// limit data rate to prevent data being lost
@@ -341,7 +341,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
}
const int64_t time_us = flow.time_us
- static_cast<int64_t>(_params.flow_delay_ms * 1000)
- static_cast<int64_t>(_params.ekf2_of_delay * 1000)
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
// limit data rate to prevent data being lost
@@ -380,7 +380,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
// calculate the system time-stamp for the mid point of the integration period
const int64_t time_us = evdata.time_us
- static_cast<int64_t>(_params.ev_delay_ms * 1000)
- static_cast<int64_t>(_params.ekf2_ev_delay * 1000)
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
// limit data rate to prevent data being lost
@@ -419,7 +419,7 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
}
const int64_t time_us = auxvel_sample.time_us
- static_cast<int64_t>(_params.auxvel_delay_ms * 1000)
- static_cast<int64_t>(_params.ekf2_avel_delay * 1000)
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
// limit data rate to prevent data being lost
@@ -477,7 +477,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
{
// down-sample the drag specific force data by accumulating and calculating the mean when
// sufficient samples have been collected
if (_params.drag_ctrl > 0) {
if (_params.ekf2_drag_ctrl > 0) {
// Allocate the required buffer size if not previously done
if (_drag_buffer == nullptr) {
@@ -538,15 +538,15 @@ void EstimatorInterface::setDragData(const imuSample &imu)
bool EstimatorInterface::initialise_interface(uint64_t timestamp)
{
const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
const float filter_update_period_ms = _params.ekf2_predict_us / 1000.f;
// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
_imu_buffer_length = math::max(2, (int)ceilf(_params.delay_max_ms / filter_update_period_ms));
_imu_buffer_length = math::max(2, (int)ceilf(_params.ekf2_delay_max / filter_update_period_ms));
// set the observation buffer length to handle the minimum time of arrival between observations in combination
// with the worst case delay from current time to ekf fusion time
// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
const float ekf_delay_ms = _params.delay_max_ms * 1.5f;
const float ekf_delay_ms = _params.ekf2_delay_max * 1.5f;
_obs_buffer_length = roundf(ekf_delay_ms / filter_update_period_ms);
// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
src/modules/ekf2/EKF/estimator_interface.h
+13 -13
View File
@@ -201,7 +201,7 @@ public:
void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
// set flag if static pressure rise due to ground effect is expected
// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
// use _params.ekf2_gnd_eff_dz to adjust for expected rise in static pressure
// flag will clear after GNDEFFECT_TIMEOUT uSec
void set_gnd_effect()
{
@@ -262,10 +262,10 @@ public:
#if defined(CONFIG_EKF2_MAGNETOMETER)
// Get the value of magnetic declination in degrees to be saved for use at the next startup
// Returns true when the declination can be saved
// At the next startup, set param.mag_declination_deg to the value saved
// At the next startup, set param.ekf2_mag_decl to the value saved
bool get_mag_decl_deg(float &val) const
{
if (PX4_ISFINITE(_wmm_declination_rad) && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
if (PX4_ISFINITE(_wmm_declination_rad) && (_params.ekf2_decl_type & GeoDeclinationMask::SAVE_GEO_DECL)) {
val = math::degrees(_wmm_declination_rad);
return true;
@@ -404,15 +404,15 @@ protected:
gnssSample _gps_sample_delayed{};
uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
GnssChecks _gnss_checks{_params.gps_check_mask,
_params.req_nsats,
_params.req_pdop,
_params.req_hacc,
_params.req_vacc,
_params.req_sacc,
_params.req_hdrift,
_params.req_vdrift,
_params.velocity_limit,
GnssChecks _gnss_checks{_params.ekf2_gps_check,
_params.ekf2_req_nsats,
_params.ekf2_req_pdop,
_params.ekf2_req_eph,
_params.ekf2_req_epv,
_params.ekf2_req_sacc,
_params.ekf2_req_hdrift,
_params.ekf2_req_vdrift,
_params.ekf2_vel_lim,
_min_gps_health_time_us,
_control_status};
@@ -508,6 +508,6 @@ protected:
void printBufferAllocationFailed(const char *buffer_name);
ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us};
ImuDownSampler _imu_down_sampler{_params.ekf2_predict_us};
};
#endif // !EKF_ESTIMATOR_INTERFACE_H
src/modules/ekf2/EKF/position_fusion.cpp
+1 -1
View File
@@ -253,5 +253,5 @@ void Ekf::resetHorizontalPositionToLastKnown()
// Used when falling back to non-aiding mode of operation
resetHorizontalPositionTo(_last_known_gpos.latitude_deg(), _last_known_gpos.longitude_deg(),
sq(_params.pos_noaid_noise));
sq(_params.ekf2_noaid_noise));
}
src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
+2 -2
View File
@@ -207,13 +207,13 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_an
}
// Gyro bias estimation
constexpr float gyro_bias_limit = 0.05f;
constexpr float ekf2_gyr_b_limit = 0.05f;
const float spin_rate = ang_rate.length();
if (spin_rate < math::radians(10.f)) {
_ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * delta_ang_dt);
_ahrs_ekf_gsf[model_index].gyro_bias = matrix::constrain(_ahrs_ekf_gsf[model_index].gyro_bias,
-gyro_bias_limit, gyro_bias_limit);
-ekf2_gyr_b_limit, ekf2_gyr_b_limit);
}
// delta angle from previous to current frame
src/modules/ekf2/EKF/yaw_fusion.cpp
+1 -1
View File
@@ -82,7 +82,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
// constrain the innovation to the maximum set by the gate
// we need to delay this forced fusion to avoid starting it
// immediately after touchdown, when the drone is still armed
const float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
const float gate_sigma = math::max(_params.ekf2_hdg_gate, 1.f);
const float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
src/modules/ekf2/EKF2.cpp
+92 -92
View File
@@ -63,87 +63,87 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)),
#endif // CONFIG_EKF2_WIND
_params(_ekf.getParamHandle()),
_param_ekf2_predict_us(_params->filter_update_interval_us),
_param_ekf2_delay_max(_params->delay_max_ms),
_param_ekf2_predict_us(_params->ekf2_predict_us),
_param_ekf2_delay_max(_params->ekf2_delay_max),
_param_ekf2_imu_ctrl(_params->imu_ctrl),
_param_ekf2_vel_lim(_params->velocity_limit),
_param_ekf2_vel_lim(_params->ekf2_vel_lim),
#if defined(CONFIG_EKF2_AUXVEL)
_param_ekf2_avel_delay(_params->auxvel_delay_ms),
_param_ekf2_avel_delay(_params->ekf2_avel_delay),
#endif // CONFIG_EKF2_AUXVEL
_param_ekf2_gyr_noise(_params->gyro_noise),
_param_ekf2_acc_noise(_params->accel_noise),
_param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise),
_param_ekf2_acc_b_noise(_params->accel_bias_p_noise),
_param_ekf2_gyr_noise(_params->ekf2_gyr_noise),
_param_ekf2_acc_noise(_params->ekf2_acc_noise),
_param_ekf2_gyr_b_noise(_params->ekf2_gyr_b_noise),
_param_ekf2_acc_b_noise(_params->ekf2_acc_b_noise),
#if defined(CONFIG_EKF2_WIND)
_param_ekf2_wind_nsd(_params->wind_vel_nsd),
_param_ekf2_wind_nsd(_params->ekf2_wind_nsd),
#endif // CONFIG_EKF2_WIND
_param_ekf2_noaid_noise(_params->pos_noaid_noise),
_param_ekf2_noaid_noise(_params->ekf2_noaid_noise),
#if defined(CONFIG_EKF2_GNSS)
_param_ekf2_gps_ctrl(_params->gnss_ctrl),
_param_ekf2_gps_delay(_params->gps_delay_ms),
_param_ekf2_gps_ctrl(_params->ekf2_gps_ctrl),
_param_ekf2_gps_delay(_params->ekf2_gps_delay),
_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
_param_ekf2_gps_v_noise(_params->gps_vel_noise),
_param_ekf2_gps_p_noise(_params->gps_pos_noise),
_param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
_param_ekf2_gps_v_gate(_params->gps_vel_innov_gate),
_param_ekf2_gps_check(_params->gps_check_mask),
_param_ekf2_req_eph(_params->req_hacc),
_param_ekf2_req_epv(_params->req_vacc),
_param_ekf2_req_sacc(_params->req_sacc),
_param_ekf2_req_nsats(_params->req_nsats),
_param_ekf2_req_pdop(_params->req_pdop),
_param_ekf2_req_hdrift(_params->req_hdrift),
_param_ekf2_req_vdrift(_params->req_vdrift),
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default),
_param_ekf2_gps_v_noise(_params->ekf2_gps_v_noise),
_param_ekf2_gps_p_noise(_params->ekf2_gps_p_noise),
_param_ekf2_gps_p_gate(_params->ekf2_gps_p_gate),
_param_ekf2_gps_v_gate(_params->ekf2_gps_v_gate),
_param_ekf2_gps_check(_params->ekf2_gps_check),
_param_ekf2_req_eph(_params->ekf2_req_eph),
_param_ekf2_req_epv(_params->ekf2_req_epv),
_param_ekf2_req_sacc(_params->ekf2_req_sacc),
_param_ekf2_req_nsats(_params->ekf2_req_nsats),
_param_ekf2_req_pdop(_params->ekf2_req_pdop),
_param_ekf2_req_hdrift(_params->ekf2_req_hdrift),
_param_ekf2_req_vdrift(_params->ekf2_req_vdrift),
_param_ekf2_gsf_tas(_params->ekf2_gsf_tas),
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_BAROMETER)
_param_ekf2_baro_ctrl(_params->baro_ctrl),
_param_ekf2_baro_delay(_params->baro_delay_ms),
_param_ekf2_baro_noise(_params->baro_noise),
_param_ekf2_baro_gate(_params->baro_innov_gate),
_param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone),
_param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt),
_param_ekf2_baro_ctrl(_params->ekf2_baro_ctrl),
_param_ekf2_baro_delay(_params->ekf2_baro_delay),
_param_ekf2_baro_noise(_params->ekf2_baro_noise),
_param_ekf2_baro_gate(_params->ekf2_baro_gate),
_param_ekf2_gnd_eff_dz(_params->ekf2_gnd_eff_dz),
_param_ekf2_gnd_max_hgt(_params->ekf2_gnd_max_hgt),
# if defined(CONFIG_EKF2_BARO_COMPENSATION)
_param_ekf2_aspd_max(_params->max_correction_airspeed),
_param_ekf2_pcoef_xp(_params->static_pressure_coef_xp),
_param_ekf2_pcoef_xn(_params->static_pressure_coef_xn),
_param_ekf2_pcoef_yp(_params->static_pressure_coef_yp),
_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
_param_ekf2_aspd_max(_params->ekf2_aspd_max),
_param_ekf2_pcoef_xp(_params->ekf2_pcoef_xp),
_param_ekf2_pcoef_xn(_params->ekf2_pcoef_xn),
_param_ekf2_pcoef_yp(_params->ekf2_pcoef_yp),
_param_ekf2_pcoef_yn(_params->ekf2_pcoef_yn),
_param_ekf2_pcoef_z(_params->ekf2_pcoef_z),
# endif // CONFIG_EKF2_BARO_COMPENSATION
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_AIRSPEED)
_param_ekf2_asp_delay(_params->airspeed_delay_ms),
_param_ekf2_tas_gate(_params->tas_innov_gate),
_param_ekf2_eas_noise(_params->eas_noise),
_param_ekf2_arsp_thr(_params->arsp_thr),
_param_ekf2_asp_delay(_params->ekf2_asp_delay),
_param_ekf2_tas_gate(_params->ekf2_tas_gate),
_param_ekf2_eas_noise(_params->ekf2_eas_noise),
_param_ekf2_arsp_thr(_params->ekf2_arsp_thr),
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
_param_ekf2_beta_gate(_params->beta_innov_gate),
_param_ekf2_beta_noise(_params->beta_noise),
_param_ekf2_fuse_beta(_params->beta_fusion_enabled),
_param_ekf2_beta_gate(_params->ekf2_beta_gate),
_param_ekf2_beta_noise(_params->ekf2_beta_noise),
_param_ekf2_fuse_beta(_params->ekf2_fuse_beta),
#endif // CONFIG_EKF2_SIDESLIP
#if defined(CONFIG_EKF2_MAGNETOMETER)
_param_ekf2_mag_delay(_params->mag_delay_ms),
_param_ekf2_mag_e_noise(_params->mage_p_noise),
_param_ekf2_mag_b_noise(_params->magb_p_noise),
_param_ekf2_head_noise(_params->mag_heading_noise),
_param_ekf2_mag_noise(_params->mag_noise),
_param_ekf2_mag_decl(_params->mag_declination_deg),
_param_ekf2_hdg_gate(_params->heading_innov_gate),
_param_ekf2_mag_gate(_params->mag_innov_gate),
_param_ekf2_decl_type(_params->mag_declination_source),
_param_ekf2_mag_type(_params->mag_fusion_type),
_param_ekf2_mag_acclim(_params->mag_acc_gate),
_param_ekf2_mag_check(_params->mag_check),
_param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs),
_param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg),
_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
_param_ekf2_mag_delay(_params->ekf2_mag_delay),
_param_ekf2_mag_e_noise(_params->ekf2_mag_e_noise),
_param_ekf2_mag_b_noise(_params->ekf2_mag_b_noise),
_param_ekf2_head_noise(_params->ekf2_head_noise),
_param_ekf2_mag_noise(_params->ekf2_mag_noise),
_param_ekf2_mag_decl(_params->ekf2_mag_decl),
_param_ekf2_hdg_gate(_params->ekf2_hdg_gate),
_param_ekf2_mag_gate(_params->ekf2_mag_gate),
_param_ekf2_decl_type(_params->ekf2_decl_type),
_param_ekf2_mag_type(_params->ekf2_mag_type),
_param_ekf2_mag_acclim(_params->ekf2_mag_acclim),
_param_ekf2_mag_check(_params->ekf2_mag_check),
_param_ekf2_mag_chk_str(_params->ekf2_mag_chk_str),
_param_ekf2_mag_chk_inc(_params->ekf2_mag_chk_inc),
_param_ekf2_synt_mag_z(_params->ekf2_synt_mag_z),
#endif // CONFIG_EKF2_MAGNETOMETER
_param_ekf2_hgt_ref(_params->height_sensor_ref),
_param_ekf2_noaid_tout(_params->valid_timeout_max),
_param_ekf2_noaid_tout(_params->ekf2_noaid_tout),
#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
_param_ekf2_min_rng(_params->rng_gnd_clearance),
#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
@@ -169,52 +169,52 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_param_ekf2_ev_delay(_params->ev_delay_ms),
_param_ekf2_ev_ctrl(_params->ev_ctrl),
_param_ekf2_ev_qmin(_params->ev_quality_minimum),
_param_ekf2_evp_noise(_params->ev_pos_noise),
_param_ekf2_evv_noise(_params->ev_vel_noise),
_param_ekf2_eva_noise(_params->ev_att_noise),
_param_ekf2_evv_gate(_params->ev_vel_innov_gate),
_param_ekf2_evp_gate(_params->ev_pos_innov_gate),
_param_ekf2_ev_delay(_params->ekf2_ev_delay),
_param_ekf2_ev_ctrl(_params->ekf2_ev_ctrl),
_param_ekf2_ev_qmin(_params->ekf2_ev_qmin),
_param_ekf2_evp_noise(_params->ekf2_evp_noise),
_param_ekf2_evv_noise(_params->ekf2_evv_noise),
_param_ekf2_eva_noise(_params->ekf2_eva_noise),
_param_ekf2_evv_gate(_params->ekf2_evv_gate),
_param_ekf2_evp_gate(_params->ekf2_evp_gate),
_param_ekf2_ev_pos_x(_params->ev_pos_body(0)),
_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_param_ekf2_of_ctrl(_params->flow_ctrl),
_param_ekf2_of_gyr_src(_params->flow_gyro_src),
_param_ekf2_of_delay(_params->flow_delay_ms),
_param_ekf2_of_n_min(_params->flow_noise),
_param_ekf2_of_n_max(_params->flow_noise_qual_min),
_param_ekf2_of_qmin(_params->flow_qual_min),
_param_ekf2_of_qmin_gnd(_params->flow_qual_min_gnd),
_param_ekf2_of_gate(_params->flow_innov_gate),
_param_ekf2_of_ctrl(_params->ekf2_of_ctrl),
_param_ekf2_of_gyr_src(_params->ekf2_of_gyr_src),
_param_ekf2_of_delay(_params->ekf2_of_delay),
_param_ekf2_of_n_min(_params->ekf2_of_n_min),
_param_ekf2_of_n_max(_params->ekf2_of_n_max),
_param_ekf2_of_qmin(_params->ekf2_of_qmin),
_param_ekf2_of_qmin_gnd(_params->ekf2_of_qmin_gnd),
_param_ekf2_of_gate(_params->ekf2_of_gate),
_param_ekf2_of_pos_x(_params->flow_pos_body(0)),
_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
_param_ekf2_of_pos_z(_params->flow_pos_body(2)),
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_DRAG_FUSION)
_param_ekf2_drag_ctrl(_params->drag_ctrl),
_param_ekf2_drag_noise(_params->drag_noise),
_param_ekf2_bcoef_x(_params->bcoef_x),
_param_ekf2_bcoef_y(_params->bcoef_y),
_param_ekf2_mcoef(_params->mcoef),
_param_ekf2_drag_ctrl(_params->ekf2_drag_ctrl),
_param_ekf2_drag_noise(_params->ekf2_drag_noise),
_param_ekf2_bcoef_x(_params->ekf2_bcoef_x),
_param_ekf2_bcoef_y(_params->ekf2_bcoef_y),
_param_ekf2_mcoef(_params->ekf2_mcoef),
#endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
_param_ekf2_grav_noise(_params->gravity_noise),
_param_ekf2_grav_noise(_params->ekf2_grav_noise),
#endif // CONFIG_EKF2_GRAVITY_FUSION
_param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
_param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
_param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
_param_ekf2_abias_init(_params->switch_on_accel_bias),
_param_ekf2_angerr_init(_params->initial_tilt_err),
_param_ekf2_abl_lim(_params->acc_bias_lim),
_param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim),
_param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim),
_param_ekf2_abl_tau(_params->acc_bias_learn_tc),
_param_ekf2_gyr_b_lim(_params->gyro_bias_lim)
_param_ekf2_gbias_init(_params->ekf2_gbias_init),
_param_ekf2_abias_init(_params->ekf2_abias_init),
_param_ekf2_angerr_init(_params->ekf2_angerr_init),
_param_ekf2_abl_lim(_params->ekf2_abl_lim),
_param_ekf2_abl_acclim(_params->ekf2_abl_acclim),
_param_ekf2_abl_gyrlim(_params->ekf2_abl_gyrlim),
_param_ekf2_abl_tau(_params->ekf2_abl_tau),
_param_ekf2_gyr_b_lim(_params->ekf2_gyr_b_lim)
{
AdvertiseTopics();
}
@@ -1790,7 +1790,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
#if defined(CONFIG_EKF2_GNSS)
// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
// the GPS Fix bit, which is always checked)
status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->gps_check_mask << 1) | 1);
status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->ekf2_gps_check << 1) | 1);
#endif // CONFIG_EKF2_GNSS
status.control_mode_flags = _ekf.control_status().value;
@@ -2599,7 +2599,7 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
#if defined(CONFIG_EKF2_SIDESLIP)
if (vehicle_status.is_vtol_tailsitter && _params->beta_fusion_enabled) {
if (vehicle_status.is_vtol_tailsitter && _params->ekf2_fuse_beta) {
PX4_WARN("Disable EKF beta fusion as unsupported for tailsitter");
_param_ekf2_fuse_beta.set(0);
_param_ekf2_fuse_beta.commit_no_notification();
src/modules/ekf2/EKF2.hpp
+2 -2
View File
@@ -536,7 +536,7 @@ private:
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h,
// Used by EKF-GSF experimental yaw estimator
(ParamExtFloat<px4::params::EKF2_GSF_TAS>) _param_ekf2_gsf_tas_default,
(ParamExtFloat<px4::params::EKF2_GSF_TAS>) _param_ekf2_gsf_tas,
(ParamFloat<px4::params::EKF2_GPS_YAW_OFF>) _param_ekf2_gps_yaw_off,
#endif // CONFIG_EKF2_GNSS
@@ -593,7 +593,7 @@ private:
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check,
(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
(ParamExtFloat<px4::params::EKF2_MAG_CHK_INC>) _param_ekf2_mag_chk_inc,
(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>) _param_ekf2_synthetic_mag_z,
(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>) _param_ekf2_synt_mag_z,
#endif // CONFIG_EKF2_MAGNETOMETER
(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref, ///< selects the primary source for height data
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+29 -29
View File
@@ -17,12 +17,12 @@ void EkfWrapper::setBaroHeightRef()
void EkfWrapper::enableBaroHeightFusion()
{
_ekf_params->baro_ctrl = 1;
_ekf_params->ekf2_baro_ctrl = 1;
}
void EkfWrapper::disableBaroHeightFusion()
{
_ekf_params->baro_ctrl = 0;
_ekf_params->ekf2_baro_ctrl = 0;
}
bool EkfWrapper::isIntendingBaroHeightFusion() const
@@ -37,12 +37,12 @@ void EkfWrapper::setGpsHeightRef()
void EkfWrapper::enableGpsHeightFusion()
{
_ekf_params->gnss_ctrl |= static_cast<int32_t>(GnssCtrl::VPOS);
_ekf_params->ekf2_gps_ctrl |= static_cast<int32_t>(GnssCtrl::VPOS);
}
void EkfWrapper::disableGpsHeightFusion()
{
_ekf_params->gnss_ctrl &= ~static_cast<int32_t>(GnssCtrl::VPOS);
_ekf_params->ekf2_gps_ctrl &= ~static_cast<int32_t>(GnssCtrl::VPOS);
}
bool EkfWrapper::isIntendingGpsHeightFusion() const
@@ -77,7 +77,7 @@ void EkfWrapper::setExternalVisionHeightRef()
void EkfWrapper::enableExternalVisionHeightFusion()
{
_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::VPOS);
_ekf_params->ekf2_ev_ctrl |= static_cast<int32_t>(EvCtrl::VPOS);
}
bool EkfWrapper::isIntendingExternalVisionHeightFusion() const
@@ -87,12 +87,12 @@ bool EkfWrapper::isIntendingExternalVisionHeightFusion() const
void EkfWrapper::enableBetaFusion()
{
_ekf_params->beta_fusion_enabled = true;
_ekf_params->ekf2_fuse_beta = true;
}
void EkfWrapper::disableBetaFusion()
{
_ekf_params->beta_fusion_enabled = false;
_ekf_params->ekf2_fuse_beta = false;
}
bool EkfWrapper::isIntendingBetaFusion() const
@@ -107,12 +107,12 @@ bool EkfWrapper::isIntendingAirspeedFusion() const
void EkfWrapper::enableGpsFusion()
{
_ekf_params->gnss_ctrl |= static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL);
_ekf_params->ekf2_gps_ctrl |= static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL);
}
void EkfWrapper::disableGpsFusion()
{
_ekf_params->gnss_ctrl &= ~(static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL));
_ekf_params->ekf2_gps_ctrl &= ~(static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL));
}
bool EkfWrapper::isIntendingGpsFusion() const
@@ -122,12 +122,12 @@ bool EkfWrapper::isIntendingGpsFusion() const
void EkfWrapper::enableGpsHeadingFusion()
{
_ekf_params->gnss_ctrl |= static_cast<int32_t>(GnssCtrl::YAW);
_ekf_params->ekf2_gps_ctrl |= static_cast<int32_t>(GnssCtrl::YAW);
}
void EkfWrapper::disableGpsHeadingFusion()
{
_ekf_params->gnss_ctrl &= ~static_cast<int32_t>(GnssCtrl::YAW);
_ekf_params->ekf2_gps_ctrl &= ~static_cast<int32_t>(GnssCtrl::YAW);
}
bool EkfWrapper::isIntendingGpsHeadingFusion() const
@@ -137,12 +137,12 @@ bool EkfWrapper::isIntendingGpsHeadingFusion() const
void EkfWrapper::enableFlowFusion()
{
_ekf_params->flow_ctrl = 1;
_ekf_params->ekf2_of_ctrl = 1;
}
void EkfWrapper::disableFlowFusion()
{
_ekf_params->flow_ctrl = 0;
_ekf_params->ekf2_of_ctrl = 0;
}
bool EkfWrapper::isIntendingFlowFusion() const
@@ -157,12 +157,12 @@ void EkfWrapper::setFlowOffset(const Vector3f &offset)
void EkfWrapper::enableExternalVisionPositionFusion()
{
_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::HPOS);
_ekf_params->ekf2_ev_ctrl |= static_cast<int32_t>(EvCtrl::HPOS);
}
void EkfWrapper::disableExternalVisionPositionFusion()
{
_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::HPOS);
_ekf_params->ekf2_ev_ctrl &= ~static_cast<int32_t>(EvCtrl::HPOS);
}
bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
@@ -172,12 +172,12 @@ bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
void EkfWrapper::enableExternalVisionVelocityFusion()
{
_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::VEL);
_ekf_params->ekf2_ev_ctrl |= static_cast<int32_t>(EvCtrl::VEL);
}
void EkfWrapper::disableExternalVisionVelocityFusion()
{
_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::VEL);
_ekf_params->ekf2_ev_ctrl &= ~static_cast<int32_t>(EvCtrl::VEL);
}
bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
@@ -187,12 +187,12 @@ bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
void EkfWrapper::enableExternalVisionHeadingFusion()
{
_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::YAW);
_ekf_params->ekf2_ev_ctrl |= static_cast<int32_t>(EvCtrl::YAW);
}
void EkfWrapper::disableExternalVisionHeadingFusion()
{
_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::YAW);
_ekf_params->ekf2_ev_ctrl &= ~static_cast<int32_t>(EvCtrl::YAW);
}
bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
@@ -217,22 +217,22 @@ bool EkfWrapper::isMagHeadingConsistent() const
void EkfWrapper::setMagFuseTypeNone()
{
_ekf_params->mag_fusion_type = MagFuseType::NONE;
_ekf_params->ekf2_mag_type = MagFuseType::NONE;
}
void EkfWrapper::enableMagStrengthCheck()
{
_ekf_params->mag_check |= static_cast<int32_t>(MagCheckMask::STRENGTH);
_ekf_params->ekf2_mag_check |= static_cast<int32_t>(MagCheckMask::STRENGTH);
}
void EkfWrapper::enableMagInclinationCheck()
{
_ekf_params->mag_check |= static_cast<int32_t>(MagCheckMask::INCLINATION);
_ekf_params->ekf2_mag_check |= static_cast<int32_t>(MagCheckMask::INCLINATION);
}
void EkfWrapper::enableMagCheckForceWMM()
{
_ekf_params->mag_check |= static_cast<int32_t>(MagCheckMask::FORCE_WMM);
_ekf_params->ekf2_mag_check |= static_cast<int32_t>(MagCheckMask::FORCE_WMM);
}
bool EkfWrapper::isWindVelocityEstimated() const
@@ -271,24 +271,24 @@ int EkfWrapper::getQuaternionResetCounter() const
void EkfWrapper::enableDragFusion()
{
_ekf_params->drag_ctrl = 1;
_ekf_params->ekf2_drag_ctrl = 1;
}
void EkfWrapper::disableDragFusion()
{
_ekf_params->drag_ctrl = 0;
_ekf_params->ekf2_drag_ctrl = 0;
}
void EkfWrapper::setDragFusionParameters(const float &bcoef_x, const float &bcoef_y, const float &mcoef)
{
_ekf_params->bcoef_x = bcoef_x;
_ekf_params->bcoef_y = bcoef_y;
_ekf_params->mcoef = mcoef;
_ekf_params->ekf2_bcoef_x = bcoef_x;
_ekf_params->ekf2_bcoef_y = bcoef_y;
_ekf_params->ekf2_mcoef = mcoef;
}
float EkfWrapper::getMagHeadingNoise() const
{
return _ekf_params->mag_heading_noise;
return _ekf_params->ekf2_head_noise;
}
void EkfWrapper::enableGyroBiasEstimation()
src/modules/ekf2/test/test_EKF_airspeed.cpp
+5 -5
View File
@@ -115,12 +115,12 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation)
EXPECT_NEAR(height_before_pressure_correction, 0.0f, 1e-5f);
// Apply height correction
const float static_pressure_coef_xp = 1.0f;
const float static_pressure_coef_yp = -1.0f; // not used as wind direction is along x axis
const float ekf2_pcoef_xp = 1.0f;
const float ekf2_pcoef_yp = -1.0f; // not used as wind direction is along x axis
parameters *_params = _ekf->getParamHandle();
_params->static_pressure_coef_xp = static_pressure_coef_xp;
_params->static_pressure_coef_yp = static_pressure_coef_yp;
float expected_height_difference = 0.5f * static_pressure_coef_xp * airspeed_body(0) * airspeed_body(
_params->ekf2_pcoef_xp = ekf2_pcoef_xp;
_params->ekf2_pcoef_yp = ekf2_pcoef_yp;
float expected_height_difference = 0.5f * ekf2_pcoef_xp * airspeed_body(0) * airspeed_body(
0) / CONSTANTS_ONE_G;
_ekf->set_vehicle_at_rest(false);
src/modules/ekf2/test/test_EKF_withReplayData.cpp
+2 -2
View File
@@ -82,8 +82,8 @@ TEST_F(EkfReplayTest, ekfGsfReset)
_sensor_simulator.startGps();
_ekf_wrapper.enableGpsFusion();
auto params = _ekf->getParamHandle();
params->gps_vel_innov_gate = 1.f;
params->gps_pos_innov_gate = 1.f;
params->ekf2_gps_v_gate = 1.f;
params->ekf2_gps_p_gate = 1.f;
uint8_t logging_rate_hz = 10;