mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
ekf2: apply astyle formatting and enforce
This commit is contained in:
Daniel Agar
parent
3fe609f769
commit
c29b4ff87e
@ -18,7 +18,8 @@ exec find boards msg src platforms test \
|
||||
-path src/lib/events/libevents -prune -o \
|
||||
-path src/lib/parameters/uthash -prune -o \
|
||||
-path src/lib/wind_estimator/python/generated -prune -o \
|
||||
-path src/modules/ekf2/EKF -prune -o \
|
||||
-path src/modules/ekf2/EKF/python/ekf_derivation/generated -prune -o \
|
||||
-path src/modules/ekf2/EKF/yaw_estimator/derivation/generated -prune -o \
|
||||
-path src/modules/gyro_fft/CMSIS_5 -prune -o \
|
||||
-path src/modules/mavlink/mavlink -prune -o \
|
||||
-path test/mavsdk_tests/catch2 -prune -o \
|
||||
|
||||
@ -89,7 +89,8 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
|
||||
|
||||
updateAirspeed(airspeed_sample, _aid_src_airspeed);
|
||||
|
||||
_innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag
|
||||
_innov_check_fail_status.flags.reject_airspeed =
|
||||
_aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag
|
||||
|
||||
const bool continuing_conditions_passing = _control_status.flags.in_air
|
||||
&& _control_status.flags.fixed_wing
|
||||
@ -155,12 +156,12 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
|
||||
&innov, &innov_var);
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
airspeed_sample.time_us, // sample timestamp
|
||||
airspeed_sample.true_airspeed, // observation
|
||||
R, // observation variance
|
||||
innov, // innovation
|
||||
innov_var, // innovation variance
|
||||
math::max(_params.tas_innov_gate, 1.f)); // innovation gate
|
||||
airspeed_sample.time_us, // sample timestamp
|
||||
airspeed_sample.true_airspeed, // observation
|
||||
R, // observation variance
|
||||
innov, // innovation
|
||||
innov_var, // innovation variance
|
||||
math::max(_params.tas_innov_gate, 1.f)); // innovation gate
|
||||
}
|
||||
|
||||
void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src)
|
||||
|
||||
@ -85,12 +85,12 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
|
||||
const Vector2f pos_obs_var(pos_var, pos_var);
|
||||
|
||||
ekf.updateAidSourceStatus(aid_src,
|
||||
sample.time_us, // sample timestamp
|
||||
position, // observation
|
||||
pos_obs_var, // observation variance
|
||||
Vector2f(ekf.state().pos) - position, // innovation
|
||||
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
|
||||
math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate
|
||||
sample.time_us, // sample timestamp
|
||||
position, // observation
|
||||
pos_obs_var, // observation variance
|
||||
Vector2f(ekf.state().pos) - position, // innovation
|
||||
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
|
||||
math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate
|
||||
}
|
||||
|
||||
const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
|
||||
|
||||
@ -96,8 +96,8 @@ private:
|
||||
uint64_t _time_last_buffer_push{0};
|
||||
|
||||
enum class Ctrl : uint8_t {
|
||||
HPOS = (1<<0),
|
||||
VPOS = (1<<1)
|
||||
HPOS = (1 << 0),
|
||||
VPOS = (1 << 1)
|
||||
};
|
||||
|
||||
enum class State {
|
||||
|
||||
@ -41,12 +41,12 @@ void Ekf::controlAuxVelFusion()
|
||||
if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) {
|
||||
|
||||
updateAidSourceStatus(_aid_src_aux_vel,
|
||||
sample.time_us, // sample timestamp
|
||||
sample.vel, // observation
|
||||
sample.velVar, // observation variance
|
||||
Vector2f(_state.vel.xy()) - sample.vel, // innovation
|
||||
Vector2f(getStateVariance<State::vel>()) + sample.velVar, // innovation variance
|
||||
math::max(_params.auxvel_gate, 1.f)); // innovation gate
|
||||
sample.time_us, // sample timestamp
|
||||
sample.vel, // observation
|
||||
sample.velVar, // observation variance
|
||||
Vector2f(_state.vel.xy()) - sample.vel, // innovation
|
||||
Vector2f(getStateVariance<State::vel>()) + sample.velVar, // innovation variance
|
||||
math::max(_params.auxvel_gate, 1.f)); // innovation gate
|
||||
|
||||
if (isHorizontalAidingActive()) {
|
||||
fuseHorizontalVelocity(_aid_src_aux_vel);
|
||||
|
||||
@ -169,12 +169,12 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
|
||||
}
|
||||
|
||||
updateAidSourceStatus(_aid_src_drag,
|
||||
drag_sample.time_us, // sample timestamp
|
||||
observation, // observation
|
||||
observation_variance, // observation variance
|
||||
innovation, // innovation
|
||||
innovation_variance, // innovation variance
|
||||
innov_gate); // innovation gate
|
||||
drag_sample.time_us, // sample timestamp
|
||||
observation, // observation
|
||||
observation_variance, // observation variance
|
||||
innovation, // innovation
|
||||
innovation_variance, // innovation variance
|
||||
innov_gate); // innovation gate
|
||||
|
||||
if (fused[0] && fused[1]) {
|
||||
_aid_src_drag.fused = true;
|
||||
|
||||
@ -55,8 +55,10 @@ void Ekf::controlExternalVisionFusion()
|
||||
|
||||
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.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
|
||||
&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
|
||||
|
||||
updateEvAttitudeErrorFilter(ev_sample, ev_reset);
|
||||
|
||||
@ -77,10 +77,12 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
|
||||
float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_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));
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
|
||||
|
||||
@ -161,12 +161,12 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
|
||||
const Vector2f pos_obs_var = measurement_var + _ev_pos_b_est.getBiasVar();
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
ev_sample.time_us, // sample timestamp
|
||||
position, // observation
|
||||
pos_obs_var, // observation variance
|
||||
Vector2f(_state.pos) - position, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
||||
math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate
|
||||
ev_sample.time_us, // sample timestamp
|
||||
position, // observation
|
||||
pos_obs_var, // observation variance
|
||||
Vector2f(_state.pos) - position, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
||||
math::max(_params.ev_pos_innov_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
|
||||
@ -205,7 +205,8 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src)
|
||||
void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var,
|
||||
estimator_aid_source2d_s &aid_src)
|
||||
{
|
||||
// activate fusion
|
||||
// TODO: (_params.position_sensor_ref == PositionSensor::EV)
|
||||
@ -229,7 +230,8 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
|
||||
_control_status.flags.ev_pos = true;
|
||||
}
|
||||
|
||||
void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src)
|
||||
void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient,
|
||||
bool reset, estimator_aid_source2d_s &aid_src)
|
||||
{
|
||||
if (reset) {
|
||||
|
||||
|
||||
@ -53,12 +53,12 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
|
||||
computeYawInnovVarAndH(obs_var, innov_var, H_YAW);
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
ev_sample.time_us, // sample timestamp
|
||||
obs, // observation
|
||||
obs_var, // observation variance
|
||||
innov, // innovation
|
||||
innov_var, // innovation variance
|
||||
math::max(_params.heading_innov_gate, 1.f)); // innovation gate
|
||||
ev_sample.time_us, // sample timestamp
|
||||
obs, // observation
|
||||
obs_var, // observation variance
|
||||
innov, // innovation
|
||||
innov_var, // innovation variance
|
||||
math::max(_params.heading_innov_gate, 1.f)); // innovation gate
|
||||
|
||||
if (ev_reset) {
|
||||
_control_status.flags.ev_yaw_fault = false;
|
||||
@ -191,9 +191,11 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
|
||||
void Ekf::stopEvYawFusion()
|
||||
{
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
|
||||
_control_status.flags.ev_yaw = false;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
}
|
||||
|
||||
@ -68,17 +68,17 @@ void Ekf::controlFakePosFusion()
|
||||
const float innov_gate = 3.f;
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
_time_delayed_us,
|
||||
position, // observation
|
||||
obs_var, // observation variance
|
||||
Vector2f(_state.pos) - position, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
|
||||
innov_gate); // innovation gate
|
||||
_time_delayed_us,
|
||||
position, // observation
|
||||
obs_var, // observation variance
|
||||
Vector2f(_state.pos) - position, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
|
||||
innov_gate); // innovation gate
|
||||
|
||||
const bool enable_conditions_passing = !isHorizontalAidingActive()
|
||||
&& ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
|
||||
&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest)
|
||||
&& _horizontal_deadreckon_time_exceeded;
|
||||
&& ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
|
||||
&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest)
|
||||
&& _horizontal_deadreckon_time_exceeded;
|
||||
|
||||
if (_control_status.flags.fake_pos) {
|
||||
if (enable_conditions_passing) {
|
||||
|
||||
@ -126,6 +126,7 @@ void Ekf::collect_gps(const gnssSample &gps)
|
||||
_wmm_gps_time_last_set = _time_delayed_us;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
|
||||
@ -157,7 +158,8 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
|
||||
|
||||
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
|
||||
constexpr float filt_time_const = 10.0f;
|
||||
const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const);
|
||||
const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp()))
|
||||
* 1e-6f, 0.001f, filt_time_const);
|
||||
const float filter_coef = dt / filt_time_const;
|
||||
|
||||
// The following checks are only valid when the vehicle is at rest
|
||||
|
||||
@ -190,12 +190,12 @@ void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s
|
||||
const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f);
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
gnss_sample.time_us, // sample timestamp
|
||||
velocity, // observation
|
||||
vel_obs_var, // observation variance
|
||||
_state.vel - velocity, // innovation
|
||||
getVelocityVariance() + vel_obs_var, // innovation variance
|
||||
innovation_gate); // innovation gate
|
||||
gnss_sample.time_us, // sample timestamp
|
||||
velocity, // observation
|
||||
vel_obs_var, // observation variance
|
||||
_state.vel - velocity, // innovation
|
||||
getVelocityVariance() + vel_obs_var, // innovation variance
|
||||
innovation_gate); // innovation gate
|
||||
|
||||
// vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
@ -228,12 +228,12 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
|
||||
const Vector2f pos_obs_var(pos_var, pos_var);
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
gnss_sample.time_us, // sample timestamp
|
||||
position, // observation
|
||||
pos_obs_var, // observation variance
|
||||
Vector2f(_state.pos) - position, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
||||
math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate
|
||||
gnss_sample.time_us, // sample timestamp
|
||||
position, // observation
|
||||
pos_obs_var, // observation variance
|
||||
Vector2f(_state.pos) - position, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
||||
math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate
|
||||
}
|
||||
|
||||
void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
|
||||
|
||||
@ -62,12 +62,12 @@ void Ekf::updateGpsYaw(const gnssSample &gps_sample)
|
||||
&heading_pred, &heading_innov_var, &H);
|
||||
|
||||
updateAidSourceStatus(_aid_src_gnss_yaw,
|
||||
gps_sample.time_us, // sample timestamp
|
||||
measured_hdg, // observation
|
||||
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
|
||||
gps_sample.time_us, // sample timestamp
|
||||
measured_hdg, // observation
|
||||
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
|
||||
}
|
||||
|
||||
void Ekf::fuseGpsYaw(float antenna_yaw_offset)
|
||||
|
||||
@ -54,7 +54,8 @@ void Ekf::controlGravityFusion(const imuSample &imu)
|
||||
|
||||
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
|
||||
const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
|
||||
const bool accel_lpf_norm_good = (_accel_magnitude_filt > lower_accel_limit) && (_accel_magnitude_filt < upper_accel_limit);
|
||||
const bool accel_lpf_norm_good = (_accel_magnitude_filt > lower_accel_limit)
|
||||
&& (_accel_magnitude_filt < upper_accel_limit);
|
||||
|
||||
// fuse gravity observation if our overall acceleration isn't too big
|
||||
_control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
|
||||
@ -70,12 +71,12 @@ void Ekf::controlGravityFusion(const imuSample &imu)
|
||||
|
||||
// fill estimator aid source status
|
||||
updateAidSourceStatus(_aid_src_gravity,
|
||||
imu.time_us, // sample timestamp
|
||||
measurement, // observation
|
||||
Vector3f{measurement_var, measurement_var, measurement_var}, // observation variance
|
||||
innovation, // innovation
|
||||
innovation_variance, // innovation variance
|
||||
0.25f); // innovation gate
|
||||
imu.time_us, // sample timestamp
|
||||
measurement, // observation
|
||||
Vector3f{measurement_var, measurement_var, measurement_var}, // observation variance
|
||||
innovation, // innovation
|
||||
innovation_variance, // innovation variance
|
||||
0.25f); // innovation gate
|
||||
|
||||
bool fused[3] {false, false, false};
|
||||
|
||||
@ -90,14 +91,16 @@ void Ekf::controlGravityFusion(const imuSample &imu)
|
||||
sym::ComputeGravityYInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H);
|
||||
|
||||
// recalculate innovation using the updated state
|
||||
_aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index);
|
||||
_aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f,
|
||||
-1.f))(index) - measurement(index);
|
||||
|
||||
} else if (index == 2) {
|
||||
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
|
||||
sym::ComputeGravityZInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H);
|
||||
|
||||
// recalculate innovation using the updated state
|
||||
_aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index);
|
||||
_aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f,
|
||||
-1.f))(index) - measurement(index);
|
||||
}
|
||||
|
||||
VectorState K = P * H / _aid_src_gravity.innovation_variance[index];
|
||||
@ -105,7 +108,8 @@ void Ekf::controlGravityFusion(const imuSample &imu)
|
||||
const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2];
|
||||
|
||||
if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
|
||||
fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]);
|
||||
fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index],
|
||||
_aid_src_gravity.innovation[index]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -109,12 +109,12 @@ void Ekf::controlMagFusion()
|
||||
sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
mag_sample.time_us, // sample timestamp
|
||||
mag_sample.mag, // observation
|
||||
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
|
||||
mag_sample.time_us, // sample timestamp
|
||||
mag_sample.mag, // observation
|
||||
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
|
||||
|
||||
// Perform an innovation consistency check and report the result
|
||||
_innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f);
|
||||
@ -134,7 +134,8 @@ void Ekf::controlMagFusion()
|
||||
&& checkMagField(mag_sample.mag)
|
||||
&& (_mag_counter > 3) // wait until we have more than a few samples through the filter
|
||||
&& (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame
|
||||
&& (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset
|
||||
&& (_state_reset_status.reset_count.quat ==
|
||||
_state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset
|
||||
&& isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL);
|
||||
|
||||
checkMagHeadingConsistency(mag_sample);
|
||||
@ -145,22 +146,22 @@ void Ekf::controlMagFusion()
|
||||
|
||||
|
||||
{
|
||||
const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding;
|
||||
const bool common_conditions_passing = _control_status.flags.mag
|
||||
&& ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding)
|
||||
|| (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
|
||||
&& !_control_status.flags.mag_fault
|
||||
&& !_control_status.flags.mag_field_disturbed
|
||||
&& !_control_status.flags.ev_yaw
|
||||
&& !_control_status.flags.gps_yaw;
|
||||
const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding;
|
||||
const bool common_conditions_passing = _control_status.flags.mag
|
||||
&& ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding)
|
||||
|| (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
|
||||
&& !_control_status.flags.mag_fault
|
||||
&& !_control_status.flags.mag_field_disturbed
|
||||
&& !_control_status.flags.ev_yaw
|
||||
&& !_control_status.flags.gps_yaw;
|
||||
|
||||
_control_status.flags.mag_3D = common_conditions_passing
|
||||
&& (_params.mag_fusion_type == MagFuseType::AUTO)
|
||||
&& _control_status.flags.mag_aligned_in_flight;
|
||||
_control_status.flags.mag_3D = common_conditions_passing
|
||||
&& (_params.mag_fusion_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));
|
||||
_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));
|
||||
}
|
||||
|
||||
// TODO: allow clearing mag_fault if mag_3d is good?
|
||||
|
||||
@ -50,7 +50,8 @@
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states, bool update_tilt)
|
||||
bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src,
|
||||
bool update_all_states, bool update_tilt)
|
||||
{
|
||||
// if any axis failed, abort the mag fusion
|
||||
if (aid_src.innovation_rejected) {
|
||||
@ -72,7 +73,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
|
||||
sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
|
||||
|
||||
// recalculate innovation using the updated state
|
||||
aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
|
||||
aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(
|
||||
index);
|
||||
|
||||
} else if (index == 2) {
|
||||
// we do not fuse synthesized magnetomter measurements when doing 3D fusion
|
||||
@ -85,7 +87,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
|
||||
sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
|
||||
|
||||
// recalculate innovation using the updated state
|
||||
aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
|
||||
aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(
|
||||
index);
|
||||
}
|
||||
|
||||
if (aid_src.innovation_variance[index] < R_MAG) {
|
||||
@ -171,7 +174,8 @@ bool Ekf::fuseDeclination(float decl_sigma)
|
||||
float decl_pred;
|
||||
float innovation_variance;
|
||||
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance,
|
||||
&H);
|
||||
|
||||
const float innovation = wrap_pi(decl_pred - decl_measurement);
|
||||
|
||||
|
||||
@ -85,7 +85,8 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
|
||||
Kfusion(State::terrain.idx) = 0.f;
|
||||
}
|
||||
|
||||
if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) {
|
||||
if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index],
|
||||
_aid_src_optical_flow.innovation[index])) {
|
||||
fused[index] = true;
|
||||
}
|
||||
}
|
||||
|
||||
@ -101,11 +101,11 @@ void Ekf::controlRangeHaglFusion()
|
||||
const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
|
||||
|
||||
const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
|
||||
|| (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
|
||||
&& _control_status.flags.tilt_align
|
||||
&& measurement_valid
|
||||
&& _range_sensor.isDataHealthy()
|
||||
&& _rng_consistency_check.isKinematicallyConsistent();
|
||||
|| (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
|
||||
&& _control_status.flags.tilt_align
|
||||
&& measurement_valid
|
||||
&& _range_sensor.isDataHealthy()
|
||||
&& _rng_consistency_check.isKinematicallyConsistent();
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
|
||||
@ -260,10 +260,10 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
|
||||
float Ekf::getRngVar() const
|
||||
{
|
||||
return fmaxf(
|
||||
P(State::pos.idx + 2, State::pos.idx + 2)
|
||||
+ sq(_params.range_noise)
|
||||
+ sq(_params.range_noise_scaler * _range_sensor.getRange()),
|
||||
0.f);
|
||||
P(State::pos.idx + 2, State::pos.idx + 2)
|
||||
+ sq(_params.range_noise)
|
||||
+ sq(_params.range_noise_scaler * _range_sensor.getRange()),
|
||||
0.f);
|
||||
}
|
||||
|
||||
void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
|
||||
|
||||
@ -56,7 +56,8 @@ struct rangeSample {
|
||||
int8_t quality{}; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
|
||||
};
|
||||
|
||||
static constexpr uint64_t RNG_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between range finder measurements (uSec)
|
||||
static constexpr uint64_t RNG_MAX_INTERVAL =
|
||||
200e3; ///< Maximum allowable time interval between range finder measurements (uSec)
|
||||
|
||||
class SensorRangeFinder : public Sensor
|
||||
{
|
||||
|
||||
@ -88,12 +88,12 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
|
||||
sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, FLT_EPSILON, &innov, &innov_var);
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
_time_delayed_us, // sample timestamp
|
||||
observation, // observation
|
||||
R, // observation variance
|
||||
innov, // innovation
|
||||
innov_var, // innovation variance
|
||||
math::max(_params.beta_innov_gate, 1.f)); // innovation gate
|
||||
_time_delayed_us, // sample timestamp
|
||||
observation, // observation
|
||||
R, // observation variance
|
||||
innov, // innovation
|
||||
innov_var, // innovation variance
|
||||
math::max(_params.beta_innov_gate, 1.f)); // innovation gate
|
||||
}
|
||||
|
||||
void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
|
||||
|
||||
@ -44,7 +44,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
|
||||
|| _control_status.flags.ev_yaw || _control_status.flags.gps_yaw;
|
||||
|
||||
// fuse zero innovation at a limited rate if the yaw variance is too large
|
||||
if(!yaw_aiding
|
||||
if (!yaw_aiding
|
||||
&& isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
|
||||
|
||||
// Use an observation variance larger than usual but small enough
|
||||
@ -60,7 +60,8 @@ 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)) {
|
||||
if (!_control_status.flags.tilt_align
|
||||
|| (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
|
||||
// The yaw variance is too large, fuse fake measurement
|
||||
fuseYaw(aid_src_status, H_YAW);
|
||||
}
|
||||
|
||||
@ -66,18 +66,26 @@ using math::Utilities::sq;
|
||||
using math::Utilities::updateYawInRotMat;
|
||||
|
||||
// maximum sensor intervals in usec
|
||||
static constexpr uint64_t BARO_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between pressure altitude measurements (uSec)
|
||||
static constexpr uint64_t EV_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between external vision system measurements (uSec)
|
||||
static constexpr uint64_t GNSS_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between GNSS measurements (uSec)
|
||||
static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
|
||||
static constexpr uint64_t MAG_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between magnetic field measurements (uSec)
|
||||
static constexpr uint64_t BARO_MAX_INTERVAL =
|
||||
200e3; ///< Maximum allowable time interval between pressure altitude measurements (uSec)
|
||||
static constexpr uint64_t EV_MAX_INTERVAL =
|
||||
200e3; ///< Maximum allowable time interval between external vision system measurements (uSec)
|
||||
static constexpr uint64_t GNSS_MAX_INTERVAL =
|
||||
500e3; ///< Maximum allowable time interval between GNSS measurements (uSec)
|
||||
static constexpr uint64_t GNSS_YAW_MAX_INTERVAL =
|
||||
1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
|
||||
static constexpr uint64_t MAG_MAX_INTERVAL =
|
||||
500e3; ///< Maximum allowable time interval between magnetic field measurements (uSec)
|
||||
|
||||
// bad accelerometer detection and mitigation
|
||||
static constexpr uint64_t BADACC_PROBATION = 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
|
||||
static constexpr float BADACC_BIAS_PNOISE = 4.9f; ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
|
||||
static constexpr uint64_t BADACC_PROBATION =
|
||||
10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
|
||||
static constexpr float BADACC_BIAS_PNOISE =
|
||||
4.9f; ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
|
||||
|
||||
// ground effect compensation
|
||||
static constexpr uint64_t GNDEFFECT_TIMEOUT = 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
|
||||
static constexpr uint64_t GNDEFFECT_TIMEOUT =
|
||||
10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
|
||||
|
||||
enum class PositionFrame : uint8_t {
|
||||
LOCAL_FRAME_NED = 0,
|
||||
@ -93,8 +101,8 @@ enum class VelocityFrame : uint8_t {
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
enum GeoDeclinationMask : uint8_t {
|
||||
// Bit locations for mag_declination_source
|
||||
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
|
||||
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 {
|
||||
@ -128,16 +136,16 @@ enum class PositionSensor : uint8_t {
|
||||
};
|
||||
|
||||
enum class ImuCtrl : uint8_t {
|
||||
GyroBias = (1<<0),
|
||||
AccelBias = (1<<1),
|
||||
GravityVector = (1<<2),
|
||||
GyroBias = (1 << 0),
|
||||
AccelBias = (1 << 1),
|
||||
GravityVector = (1 << 2),
|
||||
};
|
||||
|
||||
enum class GnssCtrl : uint8_t {
|
||||
HPOS = (1<<0),
|
||||
VPOS = (1<<1),
|
||||
VEL = (1<<2),
|
||||
YAW = (1<<3)
|
||||
HPOS = (1 << 0),
|
||||
VPOS = (1 << 1),
|
||||
VEL = (1 << 2),
|
||||
YAW = (1 << 3)
|
||||
};
|
||||
|
||||
enum class RngCtrl : uint8_t {
|
||||
@ -147,10 +155,10 @@ enum class RngCtrl : uint8_t {
|
||||
};
|
||||
|
||||
enum class EvCtrl : uint8_t {
|
||||
HPOS = (1<<0),
|
||||
VPOS = (1<<1),
|
||||
VEL = (1<<2),
|
||||
YAW = (1<<3)
|
||||
HPOS = (1 << 0),
|
||||
VPOS = (1 << 1),
|
||||
VEL = (1 << 2),
|
||||
YAW = (1 << 3)
|
||||
};
|
||||
|
||||
enum class MagCheckMask : uint8_t {
|
||||
@ -271,7 +279,7 @@ struct parameters {
|
||||
float accel_bias_p_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)
|
||||
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))
|
||||
const float wind_vel_nsd_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
@ -282,7 +290,7 @@ struct parameters {
|
||||
float initial_tilt_err{0.1f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
int32_t baro_ctrl{1};
|
||||
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)
|
||||
float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz))
|
||||
@ -305,7 +313,7 @@ struct parameters {
|
||||
#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)};
|
||||
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)
|
||||
|
||||
Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m)
|
||||
@ -346,7 +354,7 @@ struct parameters {
|
||||
float mag_heading_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 mag_delay_ms {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)
|
||||
@ -383,13 +391,13 @@ struct parameters {
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec)
|
||||
float terrain_p_noise {5.0f}; ///< process noise for terrain offset (m/sec)
|
||||
float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m)
|
||||
const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
float rng_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m)
|
||||
float rng_gnd_clearance {0.1f}; ///< minimum valid value for range when on ground (m)
|
||||
#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
@ -433,7 +441,7 @@ struct parameters {
|
||||
#endif // CONFIG_EKF2_GRAVITY_FUSION
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
int32_t flow_ctrl{0};
|
||||
int32_t flow_ctrl {0};
|
||||
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
|
||||
|
||||
// optical flow fusion
|
||||
@ -494,7 +502,8 @@ union fault_status_u {
|
||||
bool bad_hdg : 1; ///< 3 - true if the fusion of the heading angle has encountered a numerical error
|
||||
bool bad_mag_decl : 1; ///< 4 - true if the fusion of the magnetic declination has encountered a numerical error
|
||||
bool bad_airspeed : 1; ///< 5 - true if fusion of the airspeed has encountered a numerical error
|
||||
bool bad_sideslip : 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
|
||||
bool bad_sideslip :
|
||||
1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
|
||||
bool bad_optflow_X : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
|
||||
bool bad_optflow_Y : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
|
||||
bool bad_acc_bias : 1; ///< 9 - true if bad delta velocity bias estimates have been detected
|
||||
@ -555,7 +564,8 @@ union filter_control_status_u {
|
||||
uint64_t in_air : 1; ///< 7 - true when the vehicle is airborne
|
||||
uint64_t wind : 1; ///< 8 - true when wind velocity is being estimated
|
||||
uint64_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference
|
||||
uint64_t rng_hgt : 1; ///< 10 - true when range finder height is being fused as a primary height reference
|
||||
uint64_t rng_hgt :
|
||||
1; ///< 10 - true when range finder height is being fused as a primary height reference
|
||||
uint64_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference
|
||||
uint64_t ev_pos : 1; ///< 12 - true when local position data fusion from external vision is intended
|
||||
uint64_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended
|
||||
@ -563,26 +573,38 @@ union filter_control_status_u {
|
||||
uint64_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused
|
||||
uint64_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength
|
||||
uint64_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
|
||||
uint64_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used
|
||||
uint64_t mag_fault :
|
||||
1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used
|
||||
uint64_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused
|
||||
uint64_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
|
||||
uint64_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
|
||||
uint64_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
|
||||
uint64_t gnd_effect :
|
||||
1; ///< 20 - true when protection from ground effect induced static pressure rise is active
|
||||
uint64_t rng_stuck :
|
||||
1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
|
||||
uint64_t gps_yaw :
|
||||
1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
|
||||
uint64_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed
|
||||
uint64_t ev_vel : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended
|
||||
uint64_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
|
||||
uint64_t ev_vel :
|
||||
1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended
|
||||
uint64_t synthetic_mag_z :
|
||||
1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
|
||||
uint64_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest
|
||||
uint64_t gps_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
|
||||
uint64_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
|
||||
uint64_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
|
||||
uint64_t gps_yaw_fault :
|
||||
1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
|
||||
uint64_t rng_fault :
|
||||
1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
|
||||
uint64_t inertial_dead_reckoning :
|
||||
1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
|
||||
uint64_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
|
||||
uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing
|
||||
uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused
|
||||
uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused
|
||||
uint64_t gravity_vector : 1; ///< 34 - true when gravity vector measurements are being fused
|
||||
uint64_t mag : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
|
||||
uint64_t ev_yaw_fault : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
|
||||
uint64_t mag_heading_consistent : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter
|
||||
uint64_t mag :
|
||||
1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
|
||||
uint64_t ev_yaw_fault :
|
||||
1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
|
||||
uint64_t mag_heading_consistent :
|
||||
1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter
|
||||
uint64_t aux_gpos : 1; ///< 38 - true if auxiliary global position measurement fusion is intended
|
||||
uint64_t rng_terrain : 1; ///< 39 - true if we are fusing range finder data for terrain
|
||||
uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain
|
||||
@ -601,9 +623,12 @@ union ekf_solution_status_u {
|
||||
uint16_t pos_horiz_abs : 1; ///< 4 - True if the horizontal position (absolute) estimate is good
|
||||
uint16_t pos_vert_abs : 1; ///< 5 - True if the vertical position (absolute) estimate is good
|
||||
uint16_t pos_vert_agl : 1; ///< 6 - True if the vertical position (above ground) estimate is good
|
||||
uint16_t const_pos_mode : 1; ///< 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
|
||||
uint16_t pred_pos_horiz_rel : 1; ///< 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
|
||||
uint16_t pred_pos_horiz_abs : 1; ///< 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
|
||||
uint16_t const_pos_mode :
|
||||
1; ///< 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
|
||||
uint16_t pred_pos_horiz_rel :
|
||||
1; ///< 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
|
||||
uint16_t pred_pos_horiz_abs :
|
||||
1; ///< 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
|
||||
uint16_t gps_glitch : 1; ///< 10 - True if the EKF has detected a GPS glitch
|
||||
uint16_t accel_error : 1; ///< 11 - True if the EKF has detected bad accelerometer data
|
||||
} flags;
|
||||
@ -621,16 +646,22 @@ union information_event_status_u {
|
||||
bool reset_pos_to_last_known : 1; ///< 5 - true when the position states are reset to the last known position
|
||||
bool reset_pos_to_gps : 1; ///< 6 - true when the position states are reset to the gps measurement
|
||||
bool reset_pos_to_vision : 1; ///< 7 - true when the position states are reset to the vision system measurement
|
||||
bool starting_gps_fusion : 1; ///< 8 - true when the filter starts using gps measurements to correct the state estimates
|
||||
bool starting_vision_pos_fusion : 1; ///< 9 - true when the filter starts using vision system position measurements to correct the state estimates
|
||||
bool starting_vision_vel_fusion : 1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
|
||||
bool starting_vision_yaw_fusion : 1; ///< 11 - true when the filter starts using vision system yaw measurements to correct the state estimates
|
||||
bool yaw_aligned_to_imu_gps : 1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
|
||||
bool starting_gps_fusion :
|
||||
1; ///< 8 - true when the filter starts using gps measurements to correct the state estimates
|
||||
bool starting_vision_pos_fusion :
|
||||
1; ///< 9 - true when the filter starts using vision system position measurements to correct the state estimates
|
||||
bool starting_vision_vel_fusion :
|
||||
1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
|
||||
bool starting_vision_yaw_fusion :
|
||||
1; ///< 11 - true when the filter starts using vision system yaw measurements to correct the state estimates
|
||||
bool yaw_aligned_to_imu_gps :
|
||||
1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
|
||||
bool reset_hgt_to_baro : 1; ///< 13 - true when the vertical position state is reset to the baro measurement
|
||||
bool reset_hgt_to_gps : 1; ///< 14 - true when the vertical position state is reset to the gps measurement
|
||||
bool reset_hgt_to_rng : 1; ///< 15 - true when the vertical position state is reset to the rng measurement
|
||||
bool reset_hgt_to_ev : 1; ///< 16 - true when the vertical position state is reset to the ev measurement
|
||||
bool reset_pos_to_ext_obs : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning
|
||||
bool reset_pos_to_ext_obs :
|
||||
1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning
|
||||
} flags;
|
||||
uint32_t value;
|
||||
};
|
||||
@ -639,17 +670,27 @@ union information_event_status_u {
|
||||
union warning_event_status_u {
|
||||
struct {
|
||||
bool gps_quality_poor : 1; ///< 0 - true when the gps is failing quality checks
|
||||
bool gps_fusion_timout : 1; ///< 1 - true when the gps data has not been used to correct the state estimates for a significant time period
|
||||
bool gps_fusion_timout :
|
||||
1; ///< 1 - true when the gps data has not been used to correct the state estimates for a significant time period
|
||||
bool gps_data_stopped : 1; ///< 2 - true when the gps data has stopped for a significant time period
|
||||
bool gps_data_stopped_using_alternate : 1; ///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation
|
||||
bool height_sensor_timeout : 1; ///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period
|
||||
bool stopping_navigation : 1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
|
||||
bool invalid_accel_bias_cov_reset : 1; ///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements
|
||||
bool bad_yaw_using_gps_course : 1; ///< 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course
|
||||
bool stopping_mag_use : 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data
|
||||
bool vision_data_stopped : 1; ///< 9 - true when the vision system data has stopped for a significant time period
|
||||
bool emergency_yaw_reset_mag_stopped : 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data
|
||||
bool emergency_yaw_reset_gps_yaw_stopped: 1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data
|
||||
bool gps_data_stopped_using_alternate :
|
||||
1; ///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation
|
||||
bool height_sensor_timeout :
|
||||
1; ///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period
|
||||
bool stopping_navigation :
|
||||
1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
|
||||
bool invalid_accel_bias_cov_reset :
|
||||
1; ///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements
|
||||
bool bad_yaw_using_gps_course :
|
||||
1; ///< 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course
|
||||
bool stopping_mag_use :
|
||||
1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data
|
||||
bool vision_data_stopped :
|
||||
1; ///< 9 - true when the vision system data has stopped for a significant time period
|
||||
bool emergency_yaw_reset_mag_stopped :
|
||||
1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data
|
||||
bool emergency_yaw_reset_gps_yaw_stopped:
|
||||
1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data
|
||||
} flags;
|
||||
uint32_t value;
|
||||
};
|
||||
|
||||
@ -76,14 +76,17 @@ void Ekf::initialiseCovariance()
|
||||
if (_control_status.flags.gps_hgt) {
|
||||
z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
|
||||
}
|
||||
|
||||
#else
|
||||
const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f));
|
||||
#endif
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
z_pos_var = sq(fmaxf(_params.range_noise, 0.01f));
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
P.uncorrelateCovarianceSetVariance<State::pos.dof>(State::pos.idx, Vector3f(xy_pos_var, xy_pos_var, z_pos_var));
|
||||
@ -191,13 +194,16 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
|
||||
P(i, i) += mag_B_process_noise;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
|
||||
#if defined(CONFIG_EKF2_WIND)
|
||||
|
||||
if (_control_status.flags.wind) {
|
||||
// wind vel: add process noise
|
||||
float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
|
||||
float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f,
|
||||
1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
|
||||
float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
|
||||
|
||||
for (unsigned index = 0; index < State::wind_vel.dof; index++) {
|
||||
@ -205,18 +211,22 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
|
||||
P(i, i) += wind_vel_process_noise;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
|
||||
if (_height_sensor_ref != HeightSensor::RANGE) {
|
||||
// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
|
||||
// process noise due to errors in vehicle height estimate
|
||||
float terrain_process_noise = sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
|
||||
|
||||
// process noise due to terrain gradient
|
||||
terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1)));
|
||||
terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(
|
||||
1)));
|
||||
P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
// covariance matrix is symmetrical, so copy upper half to lower half
|
||||
@ -244,16 +254,20 @@ void Ekf::constrainStateVariances()
|
||||
constrainStateVarLimitRatio(State::accel_bias, kAccelBiasVarianceMin, 1.f);
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
if (_control_status.flags.mag) {
|
||||
constrainStateVarLimitRatio(State::mag_I, kMagVarianceMin, 1.f);
|
||||
constrainStateVarLimitRatio(State::mag_B, kMagVarianceMin, 1.f);
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_WIND)
|
||||
|
||||
if (_control_status.flags.wind) {
|
||||
constrainStateVarLimitRatio(State::wind_vel, 1e-6f, 1e6f);
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
|
||||
@ -168,7 +168,8 @@ bool Ekf::update()
|
||||
// control fusion of observation data
|
||||
controlFusionModes(imu_sample_delayed);
|
||||
|
||||
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias);
|
||||
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos,
|
||||
_state.gyro_bias, _state.accel_bias);
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -281,7 +282,8 @@ void Ekf::predictState(const imuSample &imu_delayed)
|
||||
_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
|
||||
}
|
||||
|
||||
bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
|
||||
bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
|
||||
uint64_t timestamp_observation)
|
||||
{
|
||||
if (!_pos_ref.isInitialized()) {
|
||||
ECL_WARN("unable to reset global position, position reference not initialized");
|
||||
@ -315,7 +317,7 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
|
||||
|
||||
const float sq_gate = sq(5.f); // magic hardcoded gate
|
||||
const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)),
|
||||
sq(innov(1)) / (sq_gate * innov_var(1))};
|
||||
sq(innov(1)) / (sq_gate * innov_var(1))};
|
||||
|
||||
const bool innov_rejected = (test_ratio.max() > 1.f);
|
||||
|
||||
@ -331,8 +333,8 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
|
||||
|
||||
} else {
|
||||
if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0)
|
||||
&& fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
|
||||
) {
|
||||
&& fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
|
||||
) {
|
||||
ECL_INFO("fused external observation as position measurement");
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
@ -366,40 +368,49 @@ void Ekf::print_status()
|
||||
printf("\nStates: (%.4f seconds ago)\n", (_time_latest_us - _time_delayed_us) * 1e-6);
|
||||
printf("Orientation (%d-%d): [%.3f, %.3f, %.3f, %.3f] (Euler [%.1f, %.1f, %.1f] deg) var: [%.1e, %.1e, %.1e]\n",
|
||||
State::quat_nominal.idx, State::quat_nominal.idx + State::quat_nominal.dof - 1,
|
||||
(double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2), (double)_state.quat_nominal(3),
|
||||
(double)math::degrees(matrix::Eulerf(_state.quat_nominal).phi()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).theta()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).psi()),
|
||||
(double)getStateVariance<State::quat_nominal>()(0), (double)getStateVariance<State::quat_nominal>()(1), (double)getStateVariance<State::quat_nominal>()(2)
|
||||
(double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2),
|
||||
(double)_state.quat_nominal(3),
|
||||
(double)math::degrees(matrix::Eulerf(_state.quat_nominal).phi()),
|
||||
(double)math::degrees(matrix::Eulerf(_state.quat_nominal).theta()),
|
||||
(double)math::degrees(matrix::Eulerf(_state.quat_nominal).psi()),
|
||||
(double)getStateVariance<State::quat_nominal>()(0), (double)getStateVariance<State::quat_nominal>()(1),
|
||||
(double)getStateVariance<State::quat_nominal>()(2)
|
||||
);
|
||||
|
||||
printf("Velocity (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
|
||||
State::vel.idx, State::vel.idx + State::vel.dof - 1,
|
||||
(double)_state.vel(0), (double)_state.vel(1), (double)_state.vel(2),
|
||||
(double)getStateVariance<State::vel>()(0), (double)getStateVariance<State::vel>()(1), (double)getStateVariance<State::vel>()(2)
|
||||
(double)getStateVariance<State::vel>()(0), (double)getStateVariance<State::vel>()(1),
|
||||
(double)getStateVariance<State::vel>()(2)
|
||||
);
|
||||
|
||||
printf("Position (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
|
||||
State::pos.idx, State::pos.idx + State::pos.dof - 1,
|
||||
(double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2),
|
||||
(double)getStateVariance<State::pos>()(0), (double)getStateVariance<State::pos>()(1), (double)getStateVariance<State::pos>()(2)
|
||||
(double)getStateVariance<State::pos>()(0), (double)getStateVariance<State::pos>()(1),
|
||||
(double)getStateVariance<State::pos>()(2)
|
||||
);
|
||||
|
||||
printf("Gyro Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n",
|
||||
State::gyro_bias.idx, State::gyro_bias.idx + State::gyro_bias.dof - 1,
|
||||
(double)_state.gyro_bias(0), (double)_state.gyro_bias(1), (double)_state.gyro_bias(2),
|
||||
(double)getStateVariance<State::gyro_bias>()(0), (double)getStateVariance<State::gyro_bias>()(1), (double)getStateVariance<State::gyro_bias>()(2)
|
||||
(double)getStateVariance<State::gyro_bias>()(0), (double)getStateVariance<State::gyro_bias>()(1),
|
||||
(double)getStateVariance<State::gyro_bias>()(2)
|
||||
);
|
||||
|
||||
printf("Accel Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n",
|
||||
State::accel_bias.idx, State::accel_bias.idx + State::accel_bias.dof - 1,
|
||||
(double)_state.accel_bias(0), (double)_state.accel_bias(1), (double)_state.accel_bias(2),
|
||||
(double)getStateVariance<State::accel_bias>()(0), (double)getStateVariance<State::accel_bias>()(1), (double)getStateVariance<State::accel_bias>()(2)
|
||||
(double)getStateVariance<State::accel_bias>()(0), (double)getStateVariance<State::accel_bias>()(1),
|
||||
(double)getStateVariance<State::accel_bias>()(2)
|
||||
);
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
printf("Magnetic Field (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
|
||||
State::mag_I.idx, State::mag_I.idx + State::mag_I.dof - 1,
|
||||
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
|
||||
(double)getStateVariance<State::mag_I>()(0), (double)getStateVariance<State::mag_I>()(1), (double)getStateVariance<State::mag_I>()(2)
|
||||
(double)getStateVariance<State::mag_I>()(0), (double)getStateVariance<State::mag_I>()(1),
|
||||
(double)getStateVariance<State::mag_I>()(2)
|
||||
);
|
||||
|
||||
printf("Magnetic Bias (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
|
||||
|
||||
@ -138,21 +138,27 @@ public:
|
||||
float getHeadingInnov() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||
return Vector3f(_aid_src_mag.innovation).max();
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
return _aid_src_gnss_yaw.innovation;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
return _aid_src_ev_yaw.innovation;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
return 0.f;
|
||||
@ -161,21 +167,27 @@ public:
|
||||
float getHeadingInnovVar() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||
return Vector3f(_aid_src_mag.innovation_variance).max();
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
return _aid_src_gnss_yaw.innovation_variance;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
return _aid_src_ev_yaw.innovation_variance;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
return 0.f;
|
||||
@ -184,21 +196,27 @@ public:
|
||||
float getHeadingInnovRatio() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||
return Vector3f(_aid_src_mag.test_ratio).max();
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
return _aid_src_gnss_yaw.test_ratio;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
return _aid_src_ev_yaw.test_ratio;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
return 0.f;
|
||||
@ -490,6 +508,7 @@ public:
|
||||
P(j, i) = P(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
constrainStateVariances();
|
||||
@ -499,7 +518,8 @@ public:
|
||||
return true;
|
||||
}
|
||||
|
||||
bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
|
||||
bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
|
||||
uint64_t timestamp_observation);
|
||||
|
||||
void updateParameters();
|
||||
|
||||
@ -576,7 +596,7 @@ private:
|
||||
SquareMatrixState P{}; ///< state covariance matrix
|
||||
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
estimator_aid_source2d_s _aid_src_drag{};
|
||||
estimator_aid_source2d_s _aid_src_drag {};
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
@ -587,11 +607,11 @@ private:
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
estimator_aid_source1d_s _aid_src_rng_hgt{};
|
||||
estimator_aid_source1d_s _aid_src_rng_hgt {};
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
estimator_aid_source2d_s _aid_src_optical_flow{};
|
||||
estimator_aid_source2d_s _aid_src_optical_flow {};
|
||||
|
||||
// optical flow processing
|
||||
Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
|
||||
@ -603,17 +623,17 @@ private:
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
estimator_aid_source1d_s _aid_src_airspeed{};
|
||||
estimator_aid_source1d_s _aid_src_airspeed {};
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
estimator_aid_source1d_s _aid_src_sideslip{};
|
||||
estimator_aid_source1d_s _aid_src_sideslip {};
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
estimator_aid_source2d_s _aid_src_fake_pos{};
|
||||
estimator_aid_source1d_s _aid_src_fake_hgt{};
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
estimator_aid_source1d_s _aid_src_ev_hgt{};
|
||||
estimator_aid_source1d_s _aid_src_ev_hgt {};
|
||||
estimator_aid_source2d_s _aid_src_ev_pos{};
|
||||
estimator_aid_source3d_s _aid_src_ev_vel{};
|
||||
estimator_aid_source1d_s _aid_src_ev_yaw{};
|
||||
@ -624,7 +644,7 @@ private:
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
|
||||
bool _gps_data_ready {false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
|
||||
|
||||
// variables used for the GPS quality checks
|
||||
Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec)
|
||||
@ -647,16 +667,16 @@ private:
|
||||
estimator_aid_source3d_s _aid_src_gnss_vel{};
|
||||
|
||||
# if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
estimator_aid_source1d_s _aid_src_gnss_yaw{};
|
||||
estimator_aid_source1d_s _aid_src_gnss_yaw {};
|
||||
# endif // CONFIG_EKF2_GNSS_YAW
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
|
||||
estimator_aid_source3d_s _aid_src_gravity{};
|
||||
estimator_aid_source3d_s _aid_src_gravity {};
|
||||
#endif // CONFIG_EKF2_GRAVITY_FUSION
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
estimator_aid_source2d_s _aid_src_aux_vel{};
|
||||
estimator_aid_source2d_s _aid_src_aux_vel {};
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
// Variables used by the initial filter alignment
|
||||
@ -665,7 +685,7 @@ private:
|
||||
AlphaFilter<Vector3f> _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
estimator_aid_source1d_s _aid_src_baro_hgt{};
|
||||
estimator_aid_source1d_s _aid_src_baro_hgt {};
|
||||
|
||||
// Variables used to perform in flight resets and switch between height sources
|
||||
AlphaFilter<float> _baro_lpf{0.1f}; ///< filtered barometric height measurement (m)
|
||||
@ -729,7 +749,8 @@ private:
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
// ekf sequential fusion of magnetometer measurements
|
||||
bool fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states = false, bool update_tilt = false);
|
||||
bool fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src,
|
||||
bool update_all_states = false, bool update_tilt = false);
|
||||
|
||||
// fuse magnetometer declination measurement
|
||||
// argument passed in is the declination uncertainty in radians
|
||||
@ -790,7 +811,8 @@ private:
|
||||
void resetVerticalVelocityToZero();
|
||||
|
||||
// horizontal and vertical position aid source
|
||||
void updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, const float observation, const float observation_variance, const float innovation_gate = 1.f) const;
|
||||
void updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us,
|
||||
const float observation, const float observation_variance, const float innovation_gate = 1.f) const;
|
||||
|
||||
// horizontal and vertical position fusion
|
||||
bool fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src);
|
||||
@ -870,6 +892,7 @@ private:
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
if (!_control_status.flags.mag) {
|
||||
for (unsigned i = 0; i < State::mag_I.dof; i++) {
|
||||
K(State::mag_I.idx + i) = 0.f;
|
||||
@ -881,14 +904,17 @@ private:
|
||||
K(State::mag_B.idx + i) = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_WIND)
|
||||
|
||||
if (!_control_status.flags.wind) {
|
||||
for (unsigned i = 0; i < State::wind_vel.dof; i++) {
|
||||
K(State::wind_vel.idx + i) = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
}
|
||||
|
||||
@ -912,15 +938,20 @@ private:
|
||||
// control fusion of external vision observations
|
||||
void controlExternalVisionFusion();
|
||||
void updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset);
|
||||
void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
|
||||
void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
|
||||
void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
|
||||
void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
|
||||
void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
|
||||
const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
|
||||
void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
|
||||
const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
|
||||
void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
|
||||
const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
|
||||
void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
|
||||
const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
|
||||
void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame);
|
||||
Vector3f rotateVarianceToEkf(const Vector3f &measurement_var);
|
||||
|
||||
void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
|
||||
void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
|
||||
void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient,
|
||||
bool reset, estimator_aid_source2d_s &aid_src);
|
||||
void stopEvPosFusion();
|
||||
void stopEvHgtFusion();
|
||||
void stopEvVelFusion();
|
||||
@ -1091,7 +1122,7 @@ private:
|
||||
PositionSensor _position_sensor_ref{PositionSensor::GNSS};
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
|
||||
HeightBiasEstimator _ev_hgt_b_est {HeightSensor::EV, _height_sensor_ref};
|
||||
PositionBiasEstimator _ev_pos_b_est{PositionSensor::EV, _position_sensor_ref};
|
||||
AlphaFilter<Quatf> _ev_q_error_filt{0.001f};
|
||||
bool _ev_q_error_initialized{false};
|
||||
@ -1297,7 +1328,7 @@ private:
|
||||
ZeroVelocityUpdate _zero_velocity_update{};
|
||||
|
||||
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
|
||||
AuxGlobalPosition _aux_global_position{};
|
||||
AuxGlobalPosition _aux_global_position {};
|
||||
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
|
||||
};
|
||||
|
||||
|
||||
@ -72,13 +72,14 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo
|
||||
return _NED_origin_initialised;
|
||||
}
|
||||
|
||||
bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph, const float epv)
|
||||
bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph,
|
||||
const float epv)
|
||||
{
|
||||
// sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space
|
||||
if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)
|
||||
&& PX4_ISFINITE(longitude) && (abs(longitude) <= 180)
|
||||
&& PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f)
|
||||
) {
|
||||
&& PX4_ISFINITE(longitude) && (abs(longitude) <= 180)
|
||||
&& PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f)
|
||||
) {
|
||||
bool current_pos_available = false;
|
||||
double current_lat = static_cast<double>(NAN);
|
||||
double current_lon = static_cast<double>(NAN);
|
||||
@ -107,6 +108,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
|
||||
|
||||
_wmm_gps_time_last_set = _time_delayed_us;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
_gpos_origin_eph = eph;
|
||||
@ -176,15 +178,19 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
// and using state variances for accuracy reporting is overly optimistic in these situations
|
||||
if (_horizontal_deadreckon_time_exceeded) {
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_pos) {
|
||||
hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm());
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
}
|
||||
|
||||
@ -203,19 +209,24 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
|
||||
float vel_err_conservative = 0.0f;
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
|
||||
if (_control_status.flags.opt_flow) {
|
||||
float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
|
||||
vel_err_conservative = math::max(getHagl(), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_pos) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_pos.innovation).norm());
|
||||
}
|
||||
@ -223,6 +234,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
|
||||
if (_control_status.flags.ev_vel) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_vel.innovation).norm());
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
hvel_err = math::max(hvel_err, vel_err_conservative);
|
||||
@ -282,6 +294,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
|
||||
*hagl_min = flow_hagl_min;
|
||||
*hagl_max = flow_hagl_max;
|
||||
}
|
||||
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
@ -309,23 +322,29 @@ float Ekf::getHeadingInnovationTestRatio() const
|
||||
float test_ratio = 0.f;
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) {
|
||||
|
||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||
for (auto &test_ratio_filtered : _aid_src_mag.test_ratio_filtered) {
|
||||
test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_yaw.test_ratio_filtered));
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_yaw.test_ratio_filtered));
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
return sqrtf(test_ratio);
|
||||
@ -337,27 +356,33 @@ float Ekf::getVelocityInnovationTestRatio() const
|
||||
float test_ratio = -1.f;
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i]));
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_vel) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[i]));
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
|
||||
if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
|
||||
for (auto &test_ratio_filtered : _aid_src_optical_flow.test_ratio_filtered) {
|
||||
test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
|
||||
@ -373,25 +398,31 @@ float Ekf::getHorizontalPositionInnovationTestRatio() const
|
||||
float test_ratio = -1.f;
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
for (auto &test_ratio_filtered : _aid_src_gnss_pos.test_ratio_filtered) {
|
||||
test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_pos) {
|
||||
for (auto &test_ratio_filtered : _aid_src_ev_pos.test_ratio_filtered) {
|
||||
test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
|
||||
|
||||
if (_control_status.flags.aux_gpos) {
|
||||
test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered()));
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
|
||||
|
||||
if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
|
||||
@ -408,31 +439,39 @@ float Ekf::getVerticalPositionInnovationTestRatio() const
|
||||
int n_hgt_sources = 0;
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
hgt_sum += sqrtf(fabsf(_aid_src_baro_hgt.test_ratio_filtered));
|
||||
n_hgt_sources++;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
|
||||
if (_control_status.flags.gps_hgt) {
|
||||
hgt_sum += sqrtf(fabsf(_aid_src_gnss_hgt.test_ratio_filtered));
|
||||
n_hgt_sources++;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
hgt_sum += sqrtf(fabsf(_aid_src_rng_hgt.test_ratio_filtered));
|
||||
n_hgt_sources++;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
hgt_sum += sqrtf(fabsf(_aid_src_ev_hgt.test_ratio_filtered));
|
||||
n_hgt_sources++;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
if (n_hgt_sources > 0) {
|
||||
@ -445,10 +484,12 @@ float Ekf::getVerticalPositionInnovationTestRatio() const
|
||||
float Ekf::getAirspeedInnovationTestRatio() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
|
||||
if (_control_status.flags.fuse_aspd) {
|
||||
// return the airspeed fusion innovation test ratio
|
||||
return sqrtf(fabsf(_aid_src_airspeed.test_ratio_filtered));
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
return NAN;
|
||||
@ -457,10 +498,12 @@ float Ekf::getAirspeedInnovationTestRatio() const
|
||||
float Ekf::getSyntheticSideslipInnovationTestRatio() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
|
||||
if (_control_status.flags.fuse_beta) {
|
||||
// return the synthetic sideslip innovation test ratio
|
||||
return sqrtf(fabsf(_aid_src_sideslip.test_ratio_filtered));
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
return NAN;
|
||||
@ -472,10 +515,12 @@ float Ekf::getHeightAboveGroundInnovationTestRatio() const
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
if (_control_status.flags.rng_terrain) {
|
||||
// return the terrain height innovation test ratio
|
||||
test_ratio = math::max(test_ratio, fabsf(_aid_src_rng_hgt.test_ratio_filtered));
|
||||
}
|
||||
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
@ -491,10 +536,14 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
|
||||
ekf_solution_status_u soln_status{};
|
||||
// TODO: Is this accurate enough?
|
||||
soln_status.flags.attitude = attitude_valid();
|
||||
soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
|
||||
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
|
||||
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
|
||||
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
|
||||
soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta
|
||||
&& _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
|
||||
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt
|
||||
|| _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
|
||||
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos
|
||||
|| _control_status.flags.opt_flow) && (_fault_status.value == 0);
|
||||
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos)
|
||||
&& (_fault_status.value == 0);
|
||||
soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
|
||||
@ -506,11 +555,13 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
|
||||
bool mag_innov_good = true;
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) {
|
||||
|
||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||
if (Vector3f(_aid_src_mag.test_ratio).max() < 1.f) {
|
||||
mag_innov_good = false;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
@ -529,7 +580,8 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
|
||||
void Ekf::fuse(const VectorState &K, float innovation)
|
||||
{
|
||||
// quat_nominal
|
||||
Quatf delta_quat(matrix::AxisAnglef(K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) * (-1.f * innovation)));
|
||||
Quatf delta_quat(matrix::AxisAnglef(K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx,
|
||||
0) * (-1.f * innovation)));
|
||||
_state.quat_nominal = delta_quat * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
_R_to_earth = Dcmf(_state.quat_nominal);
|
||||
@ -541,26 +593,35 @@ void Ekf::fuse(const VectorState &K, float innovation)
|
||||
_state.pos = matrix::constrain(_state.pos - K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f);
|
||||
|
||||
// gyro_bias
|
||||
_state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) * innovation,
|
||||
-getGyroBiasLimit(), getGyroBiasLimit());
|
||||
_state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx,
|
||||
0) * innovation,
|
||||
-getGyroBiasLimit(), getGyroBiasLimit());
|
||||
|
||||
// accel_bias
|
||||
_state.accel_bias = matrix::constrain(_state.accel_bias - K.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) * innovation,
|
||||
-getAccelBiasLimit(), getAccelBiasLimit());
|
||||
_state.accel_bias = matrix::constrain(_state.accel_bias - K.slice<State::accel_bias.dof, 1>(State::accel_bias.idx,
|
||||
0) * innovation,
|
||||
-getAccelBiasLimit(), getAccelBiasLimit());
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
// mag_I, mag_B
|
||||
if (_control_status.flags.mag) {
|
||||
_state.mag_I = matrix::constrain(_state.mag_I - K.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) * innovation, -1.f, 1.f);
|
||||
_state.mag_B = matrix::constrain(_state.mag_B - K.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) * innovation, -getMagBiasLimit(), getMagBiasLimit());
|
||||
_state.mag_I = matrix::constrain(_state.mag_I - K.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) * innovation, -1.f,
|
||||
1.f);
|
||||
_state.mag_B = matrix::constrain(_state.mag_B - K.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) * innovation,
|
||||
-getMagBiasLimit(), getMagBiasLimit());
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_WIND)
|
||||
|
||||
// wind_vel
|
||||
if (_control_status.flags.wind) {
|
||||
_state.wind_vel = matrix::constrain(_state.wind_vel - K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation, -1.e2f, 1.e2f);
|
||||
_state.wind_vel = matrix::constrain(_state.wind_vel - K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx,
|
||||
0) * innovation, -1.e2f, 1.e2f);
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
@ -581,40 +642,43 @@ void Ekf::updateHorizontalDeadReckoningstatus()
|
||||
|
||||
// velocity aiding active
|
||||
if ((_control_status.flags.gps || _control_status.flags.ev_vel)
|
||||
&& isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
|
||||
) {
|
||||
&& isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
|
||||
) {
|
||||
inertial_dead_reckoning = false;
|
||||
}
|
||||
|
||||
// position aiding active
|
||||
if ((_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.aux_gpos)
|
||||
&& isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
|
||||
) {
|
||||
&& isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
|
||||
) {
|
||||
inertial_dead_reckoning = false;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
|
||||
// optical flow active
|
||||
if (_control_status.flags.opt_flow
|
||||
&& isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)
|
||||
) {
|
||||
&& isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)
|
||||
) {
|
||||
inertial_dead_reckoning = false;
|
||||
|
||||
} else {
|
||||
if (!_control_status.flags.in_air && (_params.flow_ctrl == 1)
|
||||
&& isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max)
|
||||
) {
|
||||
&& isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max)
|
||||
) {
|
||||
// currently landed, but optical flow aiding should be possible once in air
|
||||
aiding_expected_in_air = true;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
|
||||
// air data aiding active
|
||||
if ((_control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max))
|
||||
&& (_control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max))
|
||||
) {
|
||||
&& (_control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max))
|
||||
) {
|
||||
// wind_dead_reckoning: no other aiding but air data
|
||||
_control_status.flags.wind_dead_reckoning = inertial_dead_reckoning;
|
||||
|
||||
@ -625,13 +689,14 @@ 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.beta_fusion_enabled == 1)
|
||||
&& (_params.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;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
// zero velocity update
|
||||
@ -712,6 +777,7 @@ void Ekf::updateGroundEffect()
|
||||
{
|
||||
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
|
||||
if (isTerrainEstimateValid()) {
|
||||
// automatically set ground effect if terrain is valid
|
||||
float height = getHagl();
|
||||
@ -719,12 +785,12 @@ void Ekf::updateGroundEffect()
|
||||
|
||||
} else
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
if (_control_status.flags.gnd_effect) {
|
||||
// Turn off ground effect compensation if it times out
|
||||
if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
|
||||
_control_status.flags.gnd_effect = false;
|
||||
if (_control_status.flags.gnd_effect) {
|
||||
// Turn off ground effect compensation if it times out
|
||||
if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
|
||||
_control_status.flags.gnd_effect = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
_control_status.flags.gnd_effect = false;
|
||||
@ -736,10 +802,12 @@ void Ekf::updateGroundEffect()
|
||||
void Ekf::resetWind()
|
||||
{
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
|
||||
if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
|
||||
resetWindUsingAirspeed(_airspeed_sample_delayed);
|
||||
return;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
resetWindToZero();
|
||||
@ -764,7 +832,8 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
|
||||
{
|
||||
const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f);
|
||||
const float beta = 1.f - alpha;
|
||||
_ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt);
|
||||
_ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt,
|
||||
beta * _ang_rate_magnitude_filt);
|
||||
}
|
||||
|
||||
{
|
||||
@ -789,8 +858,8 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
|
||||
|
||||
// accel bias inhibit
|
||||
const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::AccelBias))
|
||||
|| is_manoeuvre_level_high
|
||||
|| _fault_status.flags.bad_acc_vertical;
|
||||
|| is_manoeuvre_level_high
|
||||
|| _fault_status.flags.bad_acc_vertical;
|
||||
|
||||
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
|
||||
bool is_bias_observable = true;
|
||||
@ -861,6 +930,7 @@ bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c
|
||||
P(j, i) = P(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
constrainStateVariances();
|
||||
|
||||
@ -86,7 +86,8 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
|
||||
_time_latest_us = imu_sample.time_us;
|
||||
|
||||
// the output observer always runs
|
||||
_output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt);
|
||||
_output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt,
|
||||
imu_sample.delta_vel, imu_sample.delta_vel_dt);
|
||||
|
||||
// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
|
||||
if (_imu_down_sampler.update(imu_sample)) {
|
||||
@ -141,7 +142,8 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
|
||||
_time_last_mag_buffer_push = _time_latest_us;
|
||||
|
||||
} else {
|
||||
ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us,
|
||||
_min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
@ -179,13 +181,16 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
|
||||
_time_last_gps_buffer_push = _time_latest_us;
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
|
||||
if (PX4_ISFINITE(gnss_sample.yaw)) {
|
||||
_time_last_gps_yaw_buffer_push = _time_latest_us;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
} else {
|
||||
ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us,
|
||||
_min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
@ -223,7 +228,8 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
|
||||
_time_last_baro_buffer_push = _time_latest_us;
|
||||
|
||||
} else {
|
||||
ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us,
|
||||
_min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
@ -260,7 +266,8 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
|
||||
_airspeed_buffer->push(airspeed_sample_new);
|
||||
|
||||
} else {
|
||||
ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us,
|
||||
_min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
@ -298,7 +305,8 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
|
||||
_time_last_range_buffer_push = _time_latest_us;
|
||||
|
||||
} else {
|
||||
ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us,
|
||||
_min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
@ -335,7 +343,8 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
|
||||
_flow_buffer->push(optflow_sample_new);
|
||||
|
||||
} else {
|
||||
ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us,
|
||||
_min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
@ -374,7 +383,8 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
|
||||
_time_last_ext_vision_buffer_push = _time_latest_us;
|
||||
|
||||
} else {
|
||||
ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us,
|
||||
_min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
@ -411,7 +421,8 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
|
||||
_auxvel_buffer->push(auxvel_sample_new);
|
||||
|
||||
} else {
|
||||
ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us,
|
||||
_min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
@ -446,7 +457,8 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
|
||||
_system_flag_buffer->push(system_flags_new);
|
||||
|
||||
} else {
|
||||
ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us,
|
||||
_system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -348,15 +348,15 @@ protected:
|
||||
OutputPredictor _output_predictor{};
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
airspeedSample _airspeed_sample_delayed{};
|
||||
airspeedSample _airspeed_sample_delayed {};
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
extVisionSample _ev_sample_prev{};
|
||||
extVisionSample _ev_sample_prev {};
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
RingBuffer<sensor::rangeSample> *_range_buffer{nullptr};
|
||||
RingBuffer<sensor::rangeSample> *_range_buffer {nullptr};
|
||||
uint64_t _time_last_range_buffer_push{0};
|
||||
|
||||
sensor::SensorRangeFinder _range_sensor{};
|
||||
@ -364,7 +364,7 @@ protected:
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
RingBuffer<flowSample> *_flow_buffer{nullptr};
|
||||
RingBuffer<flowSample> *_flow_buffer {nullptr};
|
||||
|
||||
flowSample _flow_sample_delayed{};
|
||||
|
||||
@ -387,7 +387,7 @@ protected:
|
||||
float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
RingBuffer<gnssSample> *_gps_buffer{nullptr};
|
||||
RingBuffer<gnssSample> *_gps_buffer {nullptr};
|
||||
uint64_t _time_last_gps_buffer_push{0};
|
||||
|
||||
gnssSample _gps_sample_delayed{};
|
||||
@ -406,7 +406,7 @@ protected:
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
RingBuffer<dragSample> *_drag_buffer{nullptr};
|
||||
RingBuffer<dragSample> *_drag_buffer {nullptr};
|
||||
dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate)
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
|
||||
@ -424,25 +424,25 @@ protected:
|
||||
RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
RingBuffer<magSample> *_mag_buffer{nullptr};
|
||||
RingBuffer<magSample> *_mag_buffer {nullptr};
|
||||
uint64_t _time_last_mag_buffer_push{0};
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
|
||||
RingBuffer<airspeedSample> *_airspeed_buffer {nullptr};
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
|
||||
RingBuffer<extVisionSample> *_ext_vision_buffer {nullptr};
|
||||
uint64_t _time_last_ext_vision_buffer_push{0};
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
|
||||
RingBuffer<auxVelSample> *_auxvel_buffer {nullptr};
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
|
||||
RingBuffer<systemFlagUpdate> *_system_flag_buffer {nullptr};
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
RingBuffer<baroSample> *_baro_buffer{nullptr};
|
||||
RingBuffer<baroSample> *_baro_buffer {nullptr};
|
||||
uint64_t _time_last_baro_buffer_push{0};
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
@ -457,7 +457,7 @@ protected:
|
||||
uint64_t _wmm_gps_time_last_set{0}; // time WMM last set
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
|
||||
float _mag_declination_gps {NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
|
||||
float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
|
||||
float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T)
|
||||
|
||||
|
||||
@ -294,12 +294,15 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
|
||||
} checks[6] {};
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
checks[0] = {ReferenceType::PRESSURE, _aid_src_baro_hgt.innovation, _aid_src_baro_hgt.innovation_variance};
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
|
||||
if (_control_status.flags.gps_hgt) {
|
||||
checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance};
|
||||
}
|
||||
@ -307,16 +310,20 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
|
||||
if (_control_status.flags.gps) {
|
||||
checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]};
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
// Range is a distance to ground measurement, not a direct height observation and has an opposite sign
|
||||
checks[3] = {ReferenceType::GROUND, -_aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
checks[4] = {ReferenceType::GROUND, _aid_src_ev_hgt.innovation, _aid_src_ev_hgt.innovation_variance};
|
||||
}
|
||||
@ -324,6 +331,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
|
||||
if (_control_status.flags.ev_vel) {
|
||||
checks[5] = {ReferenceType::GROUND, _aid_src_ev_vel.innovation[2], _aid_src_ev_vel.innovation_variance[2]};
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// Compute the check based on innovation ratio for all the sources
|
||||
|
||||
@ -261,7 +261,8 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
|
||||
}
|
||||
|
||||
void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
|
||||
const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias)
|
||||
const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias,
|
||||
const matrix::Vector3f &accel_bias)
|
||||
{
|
||||
// calculate an average filter update time
|
||||
if (_time_last_correct_states_us != 0) {
|
||||
@ -365,7 +366,8 @@ void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_corre
|
||||
next_state.vert_vel += vert_vel_correction;
|
||||
|
||||
// position is propagated forward using the corrected velocity and a trapezoidal integrator
|
||||
next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * next_state.dt;
|
||||
next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f *
|
||||
next_state.dt;
|
||||
|
||||
// advance the index
|
||||
index = (index + 1) % size;
|
||||
|
||||
@ -67,7 +67,8 @@ public:
|
||||
const matrix::Vector3f &delta_velocity, const float delta_velocity_dt);
|
||||
|
||||
void correctOutputStates(const uint64_t time_delayed_us,
|
||||
const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias);
|
||||
const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state,
|
||||
const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias);
|
||||
|
||||
void resetQuaternion(const matrix::Quatf &quat_change);
|
||||
|
||||
|
||||
@ -40,9 +40,9 @@ void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, con
|
||||
float innovation_variance = getStateVariance<State::pos>()(2) + observation_variance;
|
||||
|
||||
updateAidSourceStatus(aid_src, time_us,
|
||||
observation, observation_variance,
|
||||
innovation, innovation_variance,
|
||||
innovation_gate);
|
||||
observation, observation_variance,
|
||||
innovation, innovation_variance,
|
||||
innovation_gate);
|
||||
|
||||
// z special case if there is bad vertical acceleration data, then don't reject measurement,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
@ -57,8 +57,10 @@ bool Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
|
||||
{
|
||||
// x & y
|
||||
if (!aid_src.innovation_rejected
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::pos.idx + 0)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::pos.idx + 1)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
|
||||
State::pos.idx + 0)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
|
||||
State::pos.idx + 1)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
@ -76,7 +78,8 @@ bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
|
||||
{
|
||||
// z
|
||||
if (!aid_src.innovation_rejected
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, State::pos.idx + 2)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance,
|
||||
State::pos.idx + 2)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
@ -37,8 +37,10 @@ bool Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
|
||||
{
|
||||
// vx, vy
|
||||
if (!aid_src.innovation_rejected
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
|
||||
State::vel.idx + 0)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
|
||||
State::vel.idx + 1)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
@ -56,9 +58,12 @@ bool Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
|
||||
{
|
||||
// vx, vy, vz
|
||||
if (!aid_src.innovation_rejected
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2], State::vel.idx + 2)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
|
||||
State::vel.idx + 0)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
|
||||
State::vel.idx + 1)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2],
|
||||
State::vel.idx + 2)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
@ -301,7 +301,8 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang
|
||||
// Use fixed values for delta angle process noise variances
|
||||
const float d_ang_var = sq(_gyro_noise * delta_ang_dt);
|
||||
|
||||
_ekf_gsf[model_index].P = sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, Vector2f(dvx, dvy), d_vel_var, daz, d_ang_var);
|
||||
_ekf_gsf[model_index].P = sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, Vector2f(dvx,
|
||||
dvy), d_vel_var, daz, d_ang_var);
|
||||
|
||||
// covariance matrix is symmetrical, so copy upper half to lower half
|
||||
_ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1);
|
||||
|
||||
@ -145,11 +145,13 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
|
||||
_output_predictor.resetQuaternion(q_error);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
// update EV attitude error filter
|
||||
if (_ev_q_error_initialized) {
|
||||
const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized();
|
||||
_ev_q_error_filt.reset(ev_q_error_updated);
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// record the state change
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user