mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-29 21:03:35 +08:00
rename file
This commit is contained in:
Jacob Dahl
@@ -211,7 +211,7 @@ endif()
|
|||||||
if(CONFIG_EKF2_RANGE_FINDER)
|
if(CONFIG_EKF2_RANGE_FINDER)
|
||||||
list(APPEND EKF_SRCS
|
list(APPEND EKF_SRCS
|
||||||
EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
|
EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
|
||||||
EKF/aid_sources/range_finder/jake_range_height_control.cpp
|
EKF/aid_sources/range_finder/range_height_control.cpp
|
||||||
EKF/aid_sources/range_finder/range_height_fusion.cpp
|
EKF/aid_sources/range_finder/range_height_fusion.cpp
|
||||||
EKF/aid_sources/range_finder/sensor_range_finder.cpp
|
EKF/aid_sources/range_finder/sensor_range_finder.cpp
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -124,7 +124,7 @@ endif()
|
|||||||
if(CONFIG_EKF2_RANGE_FINDER)
|
if(CONFIG_EKF2_RANGE_FINDER)
|
||||||
list(APPEND EKF_SRCS
|
list(APPEND EKF_SRCS
|
||||||
aid_sources/range_finder/range_finder_consistency_check.cpp
|
aid_sources/range_finder/range_finder_consistency_check.cpp
|
||||||
aid_sources/range_finder/jake_range_height_control.cpp
|
aid_sources/range_finder/range_height_control.cpp
|
||||||
aid_sources/range_finder/range_height_fusion.cpp
|
aid_sources/range_finder/range_height_fusion.cpp
|
||||||
aid_sources/range_finder/sensor_range_finder.cpp
|
aid_sources/range_finder/sensor_range_finder.cpp
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -1,388 +0,0 @@
|
|||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* 1. Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* 2. Redistributions in binary form must reproduce the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer in
|
|
||||||
* the documentation and/or other materials provided with the
|
|
||||||
* distribution.
|
|
||||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
||||||
* used to endorse or promote products derived from this software
|
|
||||||
* without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
||||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
||||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file range_height_control.cpp
|
|
||||||
* Control functions for ekf range finder height fusion
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "ekf.h"
|
|
||||||
#include "ekf_derivation/generated/compute_hagl_h.h"
|
|
||||||
#include "ekf_derivation/generated/compute_hagl_innov_var.h"
|
|
||||||
|
|
||||||
void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
|
|
||||||
{
|
|
||||||
if (!_range_buffer) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// TODO: why isn't this being done anywhere?
|
|
||||||
_aid_src_rng_hgt.fused = false;
|
|
||||||
|
|
||||||
// Pop rangefinder measurement from buffer of samples into active sample
|
|
||||||
sensor::rangeSample sample = {};
|
|
||||||
if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
|
|
||||||
if (_range_sensor.timedOut(imu_sample.time_us)) {
|
|
||||||
stopRangeAltitudeFusion("sensor timed out");
|
|
||||||
stopRangeTerrainFusion("sensor timed out");
|
|
||||||
}
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Set the raw sample
|
|
||||||
_range_sensor.setSample(sample);
|
|
||||||
|
|
||||||
// TODO: move setting params to init function
|
|
||||||
_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
|
|
||||||
float cosine_max_tilt = 0.9659; // 10 degrees
|
|
||||||
_range_sensor.setCosMaxTilt(cosine_max_tilt);
|
|
||||||
_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
|
|
||||||
_range_sensor.updateSensorToEarthRotation(_R_to_earth);
|
|
||||||
|
|
||||||
bool quality_ok = sample.quality > 0;
|
|
||||||
|
|
||||||
// While on ground fake a measurement at ground level if the signal quality is bad
|
|
||||||
if (!quality_ok && !_control_status.flags.in_air) {
|
|
||||||
sample.quality = 100;
|
|
||||||
quality_ok = true;
|
|
||||||
sample.range = _range_sensor.getValidMinVal();
|
|
||||||
}
|
|
||||||
|
|
||||||
bool tilt_ok = _range_sensor.isTiltOk();
|
|
||||||
bool range_ok = sample.range <= _range_sensor.getValidMaxVal() && sample.range >= _range_sensor.getValidMinVal();
|
|
||||||
|
|
||||||
// If quality, tilt, or value are outside of bounds -- throw away measurement
|
|
||||||
if (!quality_ok || !tilt_ok || !range_ok) {
|
|
||||||
stopRangeAltitudeFusion("pre checks failed");
|
|
||||||
stopRangeTerrainFusion("pre checks failed");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// TODO: refactor into apply_body_offset()
|
|
||||||
// Correct the range data for position offset relative to the IMU
|
|
||||||
const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
|
|
||||||
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
|
||||||
sample.range = sample.range + pos_offset_earth(2) / _range_sensor.getCosTilt();
|
|
||||||
|
|
||||||
// Provide sample from buffer to object
|
|
||||||
_range_sensor.setSample(sample);
|
|
||||||
|
|
||||||
// TODO: refactor into consintency_filter_update() that runs consistency status
|
|
||||||
// Check kinematic consistency of rangefinder measurement w.r.t Altitude Estimate
|
|
||||||
_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
|
|
||||||
|
|
||||||
const float z = _gpos.altitude();
|
|
||||||
const float vz = _state.vel(2);
|
|
||||||
const float dist = _range_sensor.getDistBottom(); // NOTE: applies rotation into world frame
|
|
||||||
const float dist_var = 0.05;
|
|
||||||
const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
|
|
||||||
const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
|
|
||||||
|
|
||||||
// Run the kinematic consistency check
|
|
||||||
_rng_consistency_check.run(z, z_var, vz, vz_var, dist, dist_var, imu_sample.time_us);
|
|
||||||
|
|
||||||
// Track kinematic consistency
|
|
||||||
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
|
|
||||||
|
|
||||||
// Publish EstimatorAidSource1d (observation, variance, rejected, fused)
|
|
||||||
updateRangeHagl(_aid_src_rng_hgt);
|
|
||||||
|
|
||||||
if (!PX4_ISFINITE(_aid_src_rng_hgt.observation) || !PX4_ISFINITE(_aid_src_rng_hgt.observation_variance)) {
|
|
||||||
ECL_INFO("INFINIFY OBSERVATION INVALID");
|
|
||||||
}
|
|
||||||
|
|
||||||
// Fuse Range data as Primary height source
|
|
||||||
// NOTE: terrain is not estimated in this mode
|
|
||||||
if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
|
|
||||||
fuseRangeAsHeightSource();
|
|
||||||
} else {
|
|
||||||
// Fuse Range data as aiding height source (Primary GPS or Baro)
|
|
||||||
fuseRangeAsHeightAiding();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::fuseRangeAsHeightAiding()
|
|
||||||
{
|
|
||||||
// ISSUES: 6/25/2025
|
|
||||||
// - optical flow terrain fucks everything up, I've hardcoded disabled it
|
|
||||||
// - when rng fusion starts again, EKF Z state is corrupted (oscillation)
|
|
||||||
// -
|
|
||||||
|
|
||||||
const char* kNotKinematicallyConsistentText = "not kinematically consistent";
|
|
||||||
const char* kConditionsFailingText = "conditions failing";
|
|
||||||
|
|
||||||
// Stop fusion if rangefinder kinematic consistency fails
|
|
||||||
if (!_control_status.flags.rng_kin_consistent) {
|
|
||||||
stopRangeTerrainFusion(kNotKinematicallyConsistentText);
|
|
||||||
stopRangeAltitudeFusion(kNotKinematicallyConsistentText);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//// TERRAIN FUSION ////
|
|
||||||
|
|
||||||
// Fuse Range into Terrain if:
|
|
||||||
// - kinematically consistent (hagl_rate < 1)
|
|
||||||
|
|
||||||
// Start fusion
|
|
||||||
if (!_control_status.flags.rng_terrain) {
|
|
||||||
ECL_INFO("START RNG Terrain fusion");
|
|
||||||
_control_status.flags.rng_terrain = true;
|
|
||||||
|
|
||||||
// We must reset terrain to range before we start fusing again, otherwise there
|
|
||||||
// can be a delta between RNG terrain estimate and previous, which will cause a
|
|
||||||
// discontinuity in the lpos.z state
|
|
||||||
ECL_INFO("resetting terrain to range");
|
|
||||||
resetTerrainToRng(_aid_src_rng_hgt);
|
|
||||||
resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//// ALTITUDE FUSION ////
|
|
||||||
|
|
||||||
bool range_aid_conditional = (RngCtrl)_params.ekf2_rng_ctrl == RngCtrl::CONDITIONAL;
|
|
||||||
bool range_aid_enabled = (RngCtrl)_params.ekf2_rng_ctrl == RngCtrl::ENABLED;
|
|
||||||
|
|
||||||
bool range_aid_conditions_passed = rangeAidConditionsPassed();
|
|
||||||
|
|
||||||
bool do_range_aid = range_aid_enabled || (range_aid_conditional && range_aid_conditions_passed);
|
|
||||||
|
|
||||||
// Fuse Range into Altitude if:
|
|
||||||
// - passes range_aid_conditionalchecks
|
|
||||||
// - kinematically consistent
|
|
||||||
if (do_range_aid) {
|
|
||||||
|
|
||||||
if (!_control_status.flags.rng_hgt) {
|
|
||||||
ECL_INFO("START RNG Altitude fusion");
|
|
||||||
_control_status.flags.rng_hgt = true;
|
|
||||||
|
|
||||||
// Reset altitude to rangefinder if on ground
|
|
||||||
if (!_control_status.flags.in_air) {
|
|
||||||
ECL_INFO("JAKE GND resetting altitude to range");
|
|
||||||
resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
|
|
||||||
}
|
|
||||||
|
|
||||||
// TODO: review for correctness
|
|
||||||
if (_aid_src_rng_hgt.innovation_rejected) {
|
|
||||||
// Reset aid source
|
|
||||||
ECL_INFO("resetting aid source");
|
|
||||||
resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
stopRangeAltitudeFusion(kConditionsFailingText);
|
|
||||||
}
|
|
||||||
|
|
||||||
// If we make it here, fuse
|
|
||||||
fuseHaglRng(_aid_src_rng_hgt, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::fuseRangeAsHeightSource()
|
|
||||||
{
|
|
||||||
// When primary height source is RANGE, we always fuse it
|
|
||||||
// I don't think conditional range aid mode makes sense in the context of RANGE = primary
|
|
||||||
|
|
||||||
// Fusion init logic
|
|
||||||
if (_height_sensor_ref != HeightSensor::RANGE) {
|
|
||||||
|
|
||||||
_height_sensor_ref = HeightSensor::RANGE;
|
|
||||||
_information_events.flags.reset_hgt_to_rng = true;
|
|
||||||
|
|
||||||
// Reset aid source innovation
|
|
||||||
resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
|
|
||||||
|
|
||||||
// Reset altitude to RANGE
|
|
||||||
resetAltitudeTo(_aid_src_rng_hgt.observation, _aid_src_rng_hgt.observation_variance);
|
|
||||||
_control_status.flags.rng_hgt = true;
|
|
||||||
|
|
||||||
// Cannot have terrain estimate fusion while RANGE is primary
|
|
||||||
stopRangeTerrainFusion("Cannot have terrain estimate fusion while RANGE is primary");
|
|
||||||
ECL_INFO("initializing range as primary");
|
|
||||||
_state.terrain = 0.f;
|
|
||||||
|
|
||||||
// TODO: needed? It's set above in --> resetAidSourceStatusZeroInnovation()
|
|
||||||
// _aid_src_rng_hgt.time_last_fuse = imu_sample.time_us;
|
|
||||||
}
|
|
||||||
|
|
||||||
// TODO:
|
|
||||||
// When RNG is primary height source
|
|
||||||
if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
|
|
||||||
ECL_INFO("RNG height fusion reset required, all height sources failing");
|
|
||||||
|
|
||||||
uint64_t timestamp = _aid_src_rng_hgt.timestamp;
|
|
||||||
_information_events.flags.reset_hgt_to_rng = true;
|
|
||||||
resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
|
|
||||||
resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
|
|
||||||
|
|
||||||
// reset vertical velocity if no valid sources available
|
|
||||||
if (!isVerticalVelocityAidingActive()) {
|
|
||||||
ECL_INFO("resetting vertical velocity");
|
|
||||||
resetVerticalVelocityToZero();
|
|
||||||
}
|
|
||||||
|
|
||||||
_aid_src_rng_hgt.time_last_fuse = timestamp;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Ekf::rangeAidConditionsPassed()
|
|
||||||
{
|
|
||||||
bool is_in_range = getHagl() < _params.ekf2_rng_a_hmax;
|
|
||||||
// bool is_hagl_stable = _aid_src_rng_hgt.test_ratio < 1.f;
|
|
||||||
// bool is_horizontal_aiding_active = isHorizontalAidingActive();
|
|
||||||
// bool is_below_max_speed = is_horizontal_aiding_active && !_state.vel.xy().longerThan(_params.ekf2_rng_a_vmax);
|
|
||||||
bool is_below_max_speed = !_state.vel.xy().longerThan(_params.ekf2_rng_a_vmax);
|
|
||||||
|
|
||||||
// Require conditions passing for 1_s (same as kinematic consistency check)
|
|
||||||
bool conditions_passing = false;
|
|
||||||
// bool conditions_met = is_in_range && is_hagl_stable && is_below_max_speed;
|
|
||||||
bool conditions_met = is_in_range && is_below_max_speed;
|
|
||||||
|
|
||||||
if (conditions_met) {
|
|
||||||
if (!_rng_aid_conditions_valid) {
|
|
||||||
// Conditions just became valid
|
|
||||||
ECL_INFO("RNG AID conditions valid");
|
|
||||||
_rng_aid_conditions_valid = true;
|
|
||||||
_time_rng_aid_conditions_valid = _time_latest_us;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_time_latest_us > _time_rng_aid_conditions_valid + 1'000'000) {
|
|
||||||
// Conditions have been valid for at least 1s
|
|
||||||
conditions_passing = true;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
|
|
||||||
if (_rng_aid_conditions_valid) {
|
|
||||||
_rng_aid_conditions_valid = false;
|
|
||||||
|
|
||||||
// if (!is_hagl_stable) {
|
|
||||||
// ECL_INFO("!is_hagl_stable");
|
|
||||||
// }
|
|
||||||
// if (is_horizontal_aiding_active && !is_below_max_speed) {
|
|
||||||
if (!is_below_max_speed) {
|
|
||||||
ECL_INFO("!is_below_max_speed");
|
|
||||||
}
|
|
||||||
if (!is_in_range) {
|
|
||||||
ECL_INFO("!is_in_range");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return conditions_passing;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
|
|
||||||
{
|
|
||||||
// Since the distance is not a direct observation of the terrain state but is based
|
|
||||||
// on the height state, a reset should consider the height uncertainty. This can be
|
|
||||||
// done by manipulating the Kalman gain to inject all the innovation in the terrain state
|
|
||||||
// and create the correct correlation with the terrain state with a covariance update.
|
|
||||||
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 0.f);
|
|
||||||
|
|
||||||
const float old_terrain = _state.terrain;
|
|
||||||
|
|
||||||
VectorState H;
|
|
||||||
sym::ComputeHaglH(&H);
|
|
||||||
|
|
||||||
VectorState K;
|
|
||||||
K(State::terrain.idx) = 1.f; // innovation is forced into the terrain state to create a "reset"
|
|
||||||
|
|
||||||
measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
|
|
||||||
|
|
||||||
// record the state change
|
|
||||||
const float delta_terrain = _state.terrain - old_terrain;
|
|
||||||
|
|
||||||
if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
|
|
||||||
_state_reset_status.hagl_change = delta_terrain;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
// there's already a reset this update, accumulate total delta
|
|
||||||
_state_reset_status.hagl_change += delta_terrain;
|
|
||||||
}
|
|
||||||
|
|
||||||
_state_reset_status.reset_count.hagl++;
|
|
||||||
|
|
||||||
aid_src.time_last_fuse = _time_delayed_us;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
|
|
||||||
{
|
|
||||||
const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng);
|
|
||||||
const float measurement_variance = getRngVar();
|
|
||||||
|
|
||||||
float innovation_variance;
|
|
||||||
sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
|
|
||||||
|
|
||||||
const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
|
|
||||||
updateAidSourceStatus(aid_src,
|
|
||||||
_range_sensor.sample()->time_us, // sample timestamp
|
|
||||||
measurement, // observation
|
|
||||||
measurement_variance, // observation variance
|
|
||||||
getHagl() - measurement, // innovation
|
|
||||||
innovation_variance, // innovation variance
|
|
||||||
innov_gate); // 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
|
|
||||||
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
|
|
||||||
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
|
|
||||||
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
|
|
||||||
aid_src.innovation_rejected = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
float Ekf::getRngVar() const
|
|
||||||
{
|
|
||||||
const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
|
|
||||||
const float dist_var = sq(_params.ekf2_rng_noise) + dist_dependant_var;
|
|
||||||
return dist_var;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::stopRangeAltitudeFusion(const char* reason)
|
|
||||||
{
|
|
||||||
if (_control_status.flags.rng_hgt) {
|
|
||||||
ECL_INFO("STOP RNG Altitude fusion: %s", reason);
|
|
||||||
_control_status.flags.rng_hgt = false;
|
|
||||||
if (_height_sensor_ref == HeightSensor::RANGE) {
|
|
||||||
_height_sensor_ref = HeightSensor::UNKNOWN;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::stopRangeTerrainFusion(const char* reason)
|
|
||||||
{
|
|
||||||
if (_control_status.flags.rng_terrain) {
|
|
||||||
ECL_INFO("STOP RNG Terrain fusion: %s", reason);
|
|
||||||
_control_status.flags.rng_terrain = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -42,237 +42,263 @@
|
|||||||
|
|
||||||
void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
|
void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
|
||||||
{
|
{
|
||||||
static constexpr const char *HGT_SRC_NAME = "RNG";
|
if (!_range_buffer) {
|
||||||
|
|
||||||
bool rng_data_ready = false;
|
|
||||||
|
|
||||||
if (_range_buffer) {
|
|
||||||
// Get range data from buffer and check validity
|
|
||||||
rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
|
|
||||||
_range_sensor.setDataReadiness(rng_data_ready);
|
|
||||||
|
|
||||||
// update range sensor angle parameters in case they have changed
|
|
||||||
_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
|
|
||||||
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
|
|
||||||
_range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t);
|
|
||||||
_range_sensor.setMaxFogDistance(_params.ekf2_rng_fog);
|
|
||||||
|
|
||||||
_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
|
|
||||||
|
|
||||||
if (_range_sensor.isDataHealthy()) {
|
|
||||||
// correct the range data for position offset relative to the IMU
|
|
||||||
const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
|
|
||||||
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
|
||||||
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
|
|
||||||
|
|
||||||
if (_control_status.flags.in_air) {
|
|
||||||
const bool horizontal_motion = _control_status.flags.fixed_wing
|
|
||||||
|| (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
|
|
||||||
|
|
||||||
const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
|
|
||||||
const float var = sq(_params.ekf2_rng_noise) + dist_dependant_var;
|
|
||||||
|
|
||||||
_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
|
|
||||||
_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
|
|
||||||
P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us);
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
// If we are supposed to be using range finder data but have bad range measurements
|
|
||||||
// and are on the ground, then synthesise a measurement at the expected on ground value
|
|
||||||
if (!_control_status.flags.in_air
|
|
||||||
&& _range_sensor.isRegularlySendingData()
|
|
||||||
&& _range_sensor.isDataReady()) {
|
|
||||||
|
|
||||||
_range_sensor.setRange(_params.ekf2_min_rng);
|
|
||||||
_range_sensor.setValidity(true); // bypass the checks
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
|
|
||||||
|
|
||||||
} else {
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
auto &aid_src = _aid_src_rng_hgt;
|
// TODO: why isn't this being done anywhere?
|
||||||
|
_aid_src_rng_hgt.fused = false;
|
||||||
|
|
||||||
if (rng_data_ready && _range_sensor.getSampleAddress()) {
|
// Pop rangefinder measurement from buffer of samples into active sample
|
||||||
|
sensor::rangeSample sample = {};
|
||||||
updateRangeHagl(aid_src);
|
if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
|
||||||
const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
|
if (_range_sensor.timedOut(imu_sample.time_us)) {
|
||||||
|
stopRangeAltitudeFusion("sensor timed out");
|
||||||
const bool continuing_conditions_passing = ((_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
|
stopRangeTerrainFusion("sensor timed out");
|
||||||
|| (_params.ekf2_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)
|
|
||||||
&& _range_sensor.isRegularlySendingData();
|
|
||||||
|
|
||||||
|
|
||||||
const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
|
|
||||||
&& (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
|
|
||||||
&& isConditionalRangeAidSuitable();
|
|
||||||
|
|
||||||
const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
|
|
||||||
&& (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
|
|
||||||
|
|
||||||
if (_control_status.flags.rng_hgt) {
|
|
||||||
if (!(do_conditional_range_aid || do_range_aid)) {
|
|
||||||
ECL_INFO("stopping %s fusion", HGT_SRC_NAME);
|
|
||||||
stopRngHgtFusion();
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
|
|
||||||
if (do_conditional_range_aid) {
|
|
||||||
// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
|
|
||||||
ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
|
|
||||||
_height_sensor_ref = HeightSensor::RANGE;
|
|
||||||
|
|
||||||
_control_status.flags.rng_hgt = true;
|
|
||||||
stopRngTerrFusion();
|
|
||||||
|
|
||||||
if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
|
|
||||||
resetTerrainToRng(aid_src);
|
|
||||||
resetAidSourceStatusZeroInnovation(aid_src);
|
|
||||||
}
|
|
||||||
|
|
||||||
} else if (do_range_aid) {
|
|
||||||
// Range finder is the primary height source, the ground is now the datum used
|
|
||||||
// to compute the local vertical position
|
|
||||||
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
|
|
||||||
_height_sensor_ref = HeightSensor::RANGE;
|
|
||||||
|
|
||||||
_information_events.flags.reset_hgt_to_rng = true;
|
|
||||||
resetAltitudeTo(aid_src.observation, aid_src.observation_variance);
|
|
||||||
_state.terrain = 0.f;
|
|
||||||
resetAidSourceStatusZeroInnovation(aid_src);
|
|
||||||
_control_status.flags.rng_hgt = true;
|
|
||||||
stopRngTerrFusion();
|
|
||||||
|
|
||||||
aid_src.time_last_fuse = imu_sample.time_us;
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
if (do_conditional_range_aid || do_range_aid) {
|
|
||||||
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
|
|
||||||
_control_status.flags.rng_hgt = true;
|
|
||||||
|
|
||||||
if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
|
|
||||||
resetTerrainToRng(aid_src);
|
|
||||||
resetAidSourceStatusZeroInnovation(aid_src);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
|
// Set the raw sample
|
||||||
if (continuing_conditions_passing) {
|
_range_sensor.setSample(sample);
|
||||||
|
|
||||||
fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
|
// TODO: move setting params to init function
|
||||||
|
_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
|
||||||
|
float cosine_max_tilt = 0.9659; // 10 degrees
|
||||||
|
_range_sensor.setCosMaxTilt(cosine_max_tilt);
|
||||||
|
_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
|
||||||
|
_range_sensor.updateSensorToEarthRotation(_R_to_earth);
|
||||||
|
|
||||||
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
|
bool quality_ok = sample.quality > 0;
|
||||||
|
|
||||||
if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
|
// While on ground fake a measurement at ground level if the signal quality is bad
|
||||||
// All height sources are failing
|
if (!quality_ok && !_control_status.flags.in_air) {
|
||||||
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
|
sample.quality = 100;
|
||||||
|
quality_ok = true;
|
||||||
|
sample.range = _range_sensor.getValidMinVal();
|
||||||
|
}
|
||||||
|
|
||||||
_information_events.flags.reset_hgt_to_rng = true;
|
bool tilt_ok = _range_sensor.isTiltOk();
|
||||||
resetAltitudeTo(aid_src.observation - _state.terrain);
|
bool range_ok = sample.range <= _range_sensor.getValidMaxVal() && sample.range >= _range_sensor.getValidMinVal();
|
||||||
resetAidSourceStatusZeroInnovation(aid_src);
|
|
||||||
|
|
||||||
// reset vertical velocity if no valid sources available
|
// If quality, tilt, or value are outside of bounds -- throw away measurement
|
||||||
if (!isVerticalVelocityAidingActive()) {
|
if (!quality_ok || !tilt_ok || !range_ok) {
|
||||||
resetVerticalVelocityToZero();
|
stopRangeAltitudeFusion("pre checks failed");
|
||||||
}
|
stopRangeTerrainFusion("pre checks failed");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
aid_src.time_last_fuse = imu_sample.time_us;
|
// TODO: refactor into apply_body_offset()
|
||||||
|
// Correct the range data for position offset relative to the IMU
|
||||||
|
const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
|
||||||
|
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||||
|
sample.range = sample.range + pos_offset_earth(2) / _range_sensor.getCosTilt();
|
||||||
|
|
||||||
} else if (is_fusion_failing) {
|
// Provide sample from buffer to object
|
||||||
// Some other height source is still working
|
_range_sensor.setSample(sample);
|
||||||
if (_control_status.flags.opt_flow_terrain && isTerrainEstimateValid()) {
|
|
||||||
ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME);
|
|
||||||
stopRngHgtFusion();
|
|
||||||
stopRngTerrFusion();
|
|
||||||
|
|
||||||
} else {
|
// TODO: refactor into consintency_filter_update() that runs consistency status
|
||||||
resetTerrainToRng(aid_src);
|
// Check kinematic consistency of rangefinder measurement w.r.t Altitude Estimate
|
||||||
resetAidSourceStatusZeroInnovation(aid_src);
|
_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
const float z = _gpos.altitude();
|
||||||
ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
|
const float vz = _state.vel(2);
|
||||||
stopRngHgtFusion();
|
const float dist = _range_sensor.getDistBottom(); // NOTE: applies rotation into world frame
|
||||||
stopRngTerrFusion();
|
const float dist_var = 0.05;
|
||||||
}
|
const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
|
||||||
|
const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
|
||||||
|
|
||||||
} else {
|
// Run the kinematic consistency check
|
||||||
if (starting_conditions_passing) {
|
_rng_consistency_check.run(z, z_var, vz, vz_var, dist, dist_var, imu_sample.time_us);
|
||||||
if (_control_status.flags.opt_flow_terrain) {
|
|
||||||
if (!aid_src.innovation_rejected) {
|
|
||||||
_control_status.flags.rng_terrain = true;
|
|
||||||
fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
// Track kinematic consistency
|
||||||
if (aid_src.innovation_rejected) {
|
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
|
||||||
resetTerrainToRng(aid_src);
|
|
||||||
resetAidSourceStatusZeroInnovation(aid_src);
|
|
||||||
}
|
|
||||||
|
|
||||||
_control_status.flags.rng_terrain = true;
|
// Publish EstimatorAidSource1d (observation, variance, rejected, fused)
|
||||||
}
|
updateRangeHagl(_aid_src_rng_hgt);
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
} else if ((_control_status.flags.rng_hgt || _control_status.flags.rng_terrain)
|
if (!PX4_ISFINITE(_aid_src_rng_hgt.observation) || !PX4_ISFINITE(_aid_src_rng_hgt.observation_variance)) {
|
||||||
&& !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
|
ECL_INFO("INFINIFY OBSERVATION INVALID");
|
||||||
// No data anymore. Stop until it comes back.
|
}
|
||||||
ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME);
|
|
||||||
stopRngHgtFusion();
|
// Fuse Range data as Primary height source
|
||||||
stopRngTerrFusion();
|
// NOTE: terrain is not estimated in this mode
|
||||||
|
if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
|
||||||
|
fuseRangeAsHeightSource();
|
||||||
|
} else {
|
||||||
|
// Fuse Range data as aiding height source (Primary GPS or Baro)
|
||||||
|
fuseRangeAsHeightAiding();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
|
void Ekf::fuseRangeAsHeightAiding()
|
||||||
{
|
{
|
||||||
const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng);
|
// ISSUES: 6/25/2025
|
||||||
const float measurement_variance = getRngVar();
|
// - optical flow terrain fucks everything up, I've hardcoded disabled it
|
||||||
|
// - when rng fusion starts again, EKF Z state is corrupted (oscillation)
|
||||||
|
// -
|
||||||
|
|
||||||
float innovation_variance;
|
const char* kNotKinematicallyConsistentText = "not kinematically consistent";
|
||||||
sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
|
const char* kConditionsFailingText = "conditions failing";
|
||||||
|
|
||||||
const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
|
// Stop fusion if rangefinder kinematic consistency fails
|
||||||
updateAidSourceStatus(aid_src,
|
if (!_control_status.flags.rng_kin_consistent) {
|
||||||
_range_sensor.getSampleAddress()->time_us, // sample timestamp
|
stopRangeTerrainFusion(kNotKinematicallyConsistentText);
|
||||||
measurement, // observation
|
stopRangeAltitudeFusion(kNotKinematicallyConsistentText);
|
||||||
measurement_variance, // observation variance
|
return;
|
||||||
getHagl() - measurement, // innovation
|
}
|
||||||
innovation_variance, // innovation variance
|
|
||||||
innov_gate); // 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
|
//// TERRAIN FUSION ////
|
||||||
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
|
|
||||||
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
|
// Fuse Range into Terrain if:
|
||||||
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
|
// - kinematically consistent (hagl_rate < 1)
|
||||||
aid_src.innovation_rejected = false;
|
|
||||||
|
// Start fusion
|
||||||
|
if (!_control_status.flags.rng_terrain) {
|
||||||
|
ECL_INFO("START RNG Terrain fusion");
|
||||||
|
_control_status.flags.rng_terrain = true;
|
||||||
|
|
||||||
|
// We must reset terrain to range before we start fusing again, otherwise there
|
||||||
|
// can be a delta between RNG terrain estimate and previous, which will cause a
|
||||||
|
// discontinuity in the lpos.z state
|
||||||
|
ECL_INFO("resetting terrain to range");
|
||||||
|
resetTerrainToRng(_aid_src_rng_hgt);
|
||||||
|
resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//// ALTITUDE FUSION ////
|
||||||
|
|
||||||
|
bool range_aid_conditional = (RngCtrl)_params.ekf2_rng_ctrl == RngCtrl::CONDITIONAL;
|
||||||
|
bool range_aid_enabled = (RngCtrl)_params.ekf2_rng_ctrl == RngCtrl::ENABLED;
|
||||||
|
|
||||||
|
bool range_aid_conditions_passed = rangeAidConditionsPassed();
|
||||||
|
|
||||||
|
bool do_range_aid = range_aid_enabled || (range_aid_conditional && range_aid_conditions_passed);
|
||||||
|
|
||||||
|
// Fuse Range into Altitude if:
|
||||||
|
// - passes range_aid_conditionalchecks
|
||||||
|
// - kinematically consistent
|
||||||
|
if (do_range_aid) {
|
||||||
|
|
||||||
|
if (!_control_status.flags.rng_hgt) {
|
||||||
|
ECL_INFO("START RNG Altitude fusion");
|
||||||
|
_control_status.flags.rng_hgt = true;
|
||||||
|
|
||||||
|
// Reset altitude to rangefinder if on ground
|
||||||
|
if (!_control_status.flags.in_air) {
|
||||||
|
ECL_INFO("GND resetting altitude to range");
|
||||||
|
resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO: review for correctness
|
||||||
|
if (_aid_src_rng_hgt.innovation_rejected) {
|
||||||
|
// Reset aid source
|
||||||
|
ECL_INFO("resetting aid source");
|
||||||
|
resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
stopRangeAltitudeFusion(kConditionsFailingText);
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we make it here, fuse
|
||||||
|
fuseHaglRng(_aid_src_rng_hgt, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ekf::fuseRangeAsHeightSource()
|
||||||
|
{
|
||||||
|
// When primary height source is RANGE, we always fuse it
|
||||||
|
// I don't think conditional range aid mode makes sense in the context of RANGE = primary
|
||||||
|
|
||||||
|
// Fusion init logic
|
||||||
|
if (_height_sensor_ref != HeightSensor::RANGE) {
|
||||||
|
|
||||||
|
_height_sensor_ref = HeightSensor::RANGE;
|
||||||
|
_information_events.flags.reset_hgt_to_rng = true;
|
||||||
|
|
||||||
|
// Reset aid source innovation
|
||||||
|
resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
|
||||||
|
|
||||||
|
// Reset altitude to RANGE
|
||||||
|
resetAltitudeTo(_aid_src_rng_hgt.observation, _aid_src_rng_hgt.observation_variance);
|
||||||
|
_control_status.flags.rng_hgt = true;
|
||||||
|
|
||||||
|
// Cannot have terrain estimate fusion while RANGE is primary
|
||||||
|
stopRangeTerrainFusion("Cannot have terrain estimate fusion while RANGE is primary");
|
||||||
|
ECL_INFO("initializing range as primary");
|
||||||
|
_state.terrain = 0.f;
|
||||||
|
|
||||||
|
// TODO: needed? It's set above in --> resetAidSourceStatusZeroInnovation()
|
||||||
|
// _aid_src_rng_hgt.time_last_fuse = imu_sample.time_us;
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO:
|
||||||
|
// When RNG is primary height source
|
||||||
|
if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
|
||||||
|
ECL_INFO("RNG height fusion reset required, all height sources failing");
|
||||||
|
|
||||||
|
uint64_t timestamp = _aid_src_rng_hgt.timestamp;
|
||||||
|
_information_events.flags.reset_hgt_to_rng = true;
|
||||||
|
resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
|
||||||
|
resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
|
||||||
|
|
||||||
|
// reset vertical velocity if no valid sources available
|
||||||
|
if (!isVerticalVelocityAidingActive()) {
|
||||||
|
ECL_INFO("resetting vertical velocity");
|
||||||
|
resetVerticalVelocityToZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
_aid_src_rng_hgt.time_last_fuse = timestamp;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
float Ekf::getRngVar() const
|
bool Ekf::rangeAidConditionsPassed()
|
||||||
{
|
{
|
||||||
return fmaxf(
|
bool is_in_range = getHagl() < _params.ekf2_rng_a_hmax;
|
||||||
P(State::pos.idx + 2, State::pos.idx + 2)
|
// bool is_hagl_stable = _aid_src_rng_hgt.test_ratio < 1.f;
|
||||||
+ sq(_params.ekf2_rng_noise)
|
// bool is_horizontal_aiding_active = isHorizontalAidingActive();
|
||||||
+ sq(_params.ekf2_rng_sfe * _range_sensor.getRange()),
|
// bool is_below_max_speed = is_horizontal_aiding_active && !_state.vel.xy().longerThan(_params.ekf2_rng_a_vmax);
|
||||||
0.f);
|
bool is_below_max_speed = !_state.vel.xy().longerThan(_params.ekf2_rng_a_vmax);
|
||||||
|
|
||||||
|
// Require conditions passing for 1_s (same as kinematic consistency check)
|
||||||
|
bool conditions_passing = false;
|
||||||
|
// bool conditions_met = is_in_range && is_hagl_stable && is_below_max_speed;
|
||||||
|
bool conditions_met = is_in_range && is_below_max_speed;
|
||||||
|
|
||||||
|
if (conditions_met) {
|
||||||
|
if (!_rng_aid_conditions_valid) {
|
||||||
|
// Conditions just became valid
|
||||||
|
ECL_INFO("RNG AID conditions valid");
|
||||||
|
_rng_aid_conditions_valid = true;
|
||||||
|
_time_rng_aid_conditions_valid = _time_latest_us;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_time_latest_us > _time_rng_aid_conditions_valid + 1'000'000) {
|
||||||
|
// Conditions have been valid for at least 1s
|
||||||
|
conditions_passing = true;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
|
||||||
|
if (_rng_aid_conditions_valid) {
|
||||||
|
_rng_aid_conditions_valid = false;
|
||||||
|
|
||||||
|
// if (!is_hagl_stable) {
|
||||||
|
// ECL_INFO("!is_hagl_stable");
|
||||||
|
// }
|
||||||
|
// if (is_horizontal_aiding_active && !is_below_max_speed) {
|
||||||
|
if (!is_below_max_speed) {
|
||||||
|
ECL_INFO("!is_below_max_speed");
|
||||||
|
}
|
||||||
|
if (!is_in_range) {
|
||||||
|
ECL_INFO("!is_in_range");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return conditions_passing;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
|
void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
|
||||||
@@ -309,47 +335,54 @@ void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
|
|||||||
aid_src.time_last_fuse = _time_delayed_us;
|
aid_src.time_last_fuse = _time_delayed_us;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Ekf::isConditionalRangeAidSuitable()
|
void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
|
||||||
{
|
{
|
||||||
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
|
const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng);
|
||||||
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
|
const float measurement_variance = getRngVar();
|
||||||
float range_hagl_max = _params.ekf2_rng_a_hmax;
|
|
||||||
float max_vel_xy = _params.ekf2_rng_a_vmax;
|
|
||||||
|
|
||||||
const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio;
|
float innovation_variance;
|
||||||
|
sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
|
||||||
|
|
||||||
bool is_hagl_stable = (hagl_test_ratio < 1.f);
|
const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
|
||||||
|
updateAidSourceStatus(aid_src,
|
||||||
|
_range_sensor.sample()->time_us, // sample timestamp
|
||||||
|
measurement, // observation
|
||||||
|
measurement_variance, // observation variance
|
||||||
|
getHagl() - measurement, // innovation
|
||||||
|
innovation_variance, // innovation variance
|
||||||
|
innov_gate); // innovation gate
|
||||||
|
|
||||||
if (!_control_status.flags.rng_hgt) {
|
// z special case if there is bad vertical acceleration data, then don't reject measurement,
|
||||||
range_hagl_max = 0.7f * _params.ekf2_rng_a_hmax;
|
// but limit innovation to prevent spikes that could destabilise the filter
|
||||||
max_vel_xy = 0.7f * _params.ekf2_rng_a_vmax;
|
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
|
||||||
is_hagl_stable = (hagl_test_ratio < 0.01f);
|
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
|
||||||
|
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
|
||||||
|
aid_src.innovation_rejected = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
const bool is_in_range = (getHagl() < range_hagl_max);
|
|
||||||
|
|
||||||
bool is_below_max_speed = true;
|
|
||||||
|
|
||||||
if (isHorizontalAidingActive()) {
|
|
||||||
is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
|
|
||||||
}
|
|
||||||
|
|
||||||
return is_in_range && is_hagl_stable && is_below_max_speed;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Ekf::stopRngHgtFusion()
|
float Ekf::getRngVar() const
|
||||||
|
{
|
||||||
|
const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
|
||||||
|
const float dist_var = sq(_params.ekf2_rng_noise) + dist_dependant_var;
|
||||||
|
return dist_var;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ekf::stopRangeAltitudeFusion(const char* reason)
|
||||||
{
|
{
|
||||||
if (_control_status.flags.rng_hgt) {
|
if (_control_status.flags.rng_hgt) {
|
||||||
|
ECL_INFO("STOP RNG Altitude fusion: %s", reason);
|
||||||
|
_control_status.flags.rng_hgt = false;
|
||||||
if (_height_sensor_ref == HeightSensor::RANGE) {
|
if (_height_sensor_ref == HeightSensor::RANGE) {
|
||||||
_height_sensor_ref = HeightSensor::UNKNOWN;
|
_height_sensor_ref = HeightSensor::UNKNOWN;
|
||||||
}
|
}
|
||||||
|
|
||||||
_control_status.flags.rng_hgt = false;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void Ekf::stopRngTerrFusion()
|
void Ekf::stopRangeTerrainFusion(const char* reason)
|
||||||
{
|
{
|
||||||
_control_status.flags.rng_terrain = false;
|
if (_control_status.flags.rng_terrain) {
|
||||||
|
ECL_INFO("STOP RNG Terrain fusion: %s", reason);
|
||||||
|
_control_status.flags.rng_terrain = false;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user