Skip to content

Commit

Permalink
overhaul approach to work on unit tests and snow/blocking/normal flights
Browse files Browse the repository at this point in the history
  • Loading branch information
@haumarco
haumarco committed Dec 20, 2024
1 parent 8be67f2 commit f7052d6
Show file tree
Hide file tree
Showing 8 changed files with 140 additions and 97 deletions.
100 changes: 61 additions & 39 deletions src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,80 +40,103 @@

using namespace matrix;

void RangeFinderConsistencyCheck::init(float var_z, float var_terrain, float z, float dist_bottom)
void RangeFinderConsistencyCheck::init(const float z, const float z_var, const float dist_bottom,
const float dist_bottom_var)
{
_R.setZero();
_A.setIdentity();
float p[4] = {var_z, 0.f, 0.f, var_terrain};
float p[4] = {z_var, 0.f, 0.f, z_var + dist_bottom_var};
_P = Matrix<float, RangeFilter::size, RangeFilter::size>(p);
sym::RangeValidationFilter(&_H);
_x(RangeFilter::z.idx) = z;
_x(RangeFilter::terrain.idx) = z + dist_bottom;
_x(RangeFilter::terrain.idx) = z - dist_bottom;
_initialized = true;
_sample_count = 0;
_state = KinematicState::UNKNOWN;
_state = _test_ratio_lpf.getState() < 1.f ? KinematicState::UNKNOWN : KinematicState::INCONSISTENT;
_test_ratio_lpf.reset(2.f);
_t_since_first_sample = 0.f;
_test_ratio_lpf.setAlpha(0.2f);
}

void RangeFinderConsistencyCheck::update(float z, float z_var, float vz, float vz_var, float dist_bottom,
float dist_bottom_var, uint64_t time_us)
void RangeFinderConsistencyCheck::update(const float z, const float z_var, const float vz, const float vz_var,
const float dist_bottom, const float dist_bottom_var, const uint64_t time_us)
{
const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;

if (_time_last_update_us == 0 || dt > 1.f) {
_time_last_update_us = time_us;
init(z, z_var, dist_bottom, dist_bottom_var);
return;
}

if (_min_nr_of_samples == 0) {
_min_nr_of_samples = (int)(1.f / dt);
}

_time_last_update_us = time_us;

_R(RangeFilter::z.idx, RangeFilter::z.idx) = z_var;
_R(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = dist_bottom_var;
_x(RangeFilter::z.idx) -= dt * vz;
_P(0, 0) += dt * dt * vz_var + 0.001f;
_P(1, 1) += terrain_process_noise;

SquareMatrix<float, 2> Q;
Q(RangeFilter::z.idx, RangeFilter::z.idx) = dt * dt * vz_var + 0.001f;
Q(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = terrain_process_noise;
const Vector2f measurements(z, dist_bottom);

_x(RangeFilter::z.idx) += dt * vz;
_P = _A * _P * _A.transpose() + Q;
Vector2f Kv{1.f, 0.f};
Vector2f test_ratio{0.f, 0.f};
Vector2f R{z_var, dist_bottom_var};
Vector2f y;

const Vector2f measurements(z, dist_bottom);
const Vector2f y = measurements - _H * _x;
const Matrix2f S = _H * _P * _H.transpose() + _R;
const float normalized_residual = (y.transpose() * S.I() * y)(0, 0);
float test_ratio = fminf(normalized_residual / sq(_gate), 2.f);
for (int i = 0; i < 2 ; i++) {
y = measurements - _H * _x;
Vector2f H = _H.row(i);
_innov_var = (H.transpose() * _P * H + R(i))(0, 0);
Kv(RangeFilter::terrain.idx) = _P(RangeFilter::terrain.idx, i) / _innov_var;

Vector2f PH = _P.row(i);

for (int u = 0; u < RangeFilter::size; u++) {
for (int v = 0; v < RangeFilter::size; v++) {
_P(u, v) -= Kv(u) * PH(v);
}
}

Matrix2f K = _P * _H.transpose() * S.I();
PH = _P.col(i);

K(RangeFilter::z.idx, RangeFilter::z.idx) = 1.f;
K(RangeFilter::z.idx, RangeFilter::terrain.idx) = 0.f;
for (int u = 0; u < RangeFilter::size; u++) {
for (int v = 0; v <= u; v++) {
_P(u, v) = _P(u, v) - PH(u) * Kv(v) + Kv(u) * R(i) * Kv(v);
_P(v, u) = _P(u, v);
}
}

test_ratio(i) = fminf(sq(y(i)) / sq(_gate), 4.f);

if (i == (int)RangeFilter::z.idx && test_ratio(1) > 1.f) {
Kv(1) = 0.f;
}

if (test_ratio > 1.f) {
K(RangeFilter::terrain.idx, RangeFilter::z.idx) = 0.f;
K(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = 0.f;
_x = _x + Kv * y;
}

_x = _x + K * y;
_P = _P - K * _H * _P;
_P = 0.5f * (_P + _P.transpose()); // Ensure symmetry
_innov = y(RangeFilter::terrain.idx);
_innov_var = S(RangeFilter::terrain.idx, RangeFilter::terrain.idx);
_test_ratio_lpf.update(sign(_innov) * test_ratio(1));

_test_ratio_lpf.update(sign(_innov) * test_ratio);

if (_sample_count++ > _min_nr_of_samples) {
// start the consistency check after 1s
if (_t_since_first_sample + dt > 1.0f) {
_t_since_first_sample = 2.0f;

if (abs(_test_ratio_lpf.getState()) < 1.f) {
_state = KinematicState::CONSISTENT;
const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f);

if (!horizontal_motion && vertical_motion) {
_state = KinematicState::CONSISTENT;

} else {
_state = KinematicState::UNKNOWN;
}

} else {
_sample_count = 0;
_state = KinematicState::INCONSISTENT;
}

} else {
_t_since_first_sample += dt;
}
}

Expand All @@ -126,8 +149,7 @@ void RangeFinderConsistencyCheck::run(const float z, const float vz,

if (!_initialized || current_posD_reset_count != _last_posD_reset_count) {
_last_posD_reset_count = current_posD_reset_count;
const float terrain_var = P(estimator::State::terrain.idx, estimator::State::terrain.idx);
init(z_var, terrain_var, z, dist_bottom);
init(z, z_var, dist_bottom, dist_bottom_var);
}

update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us);
Expand Down
40 changes: 16 additions & 24 deletions src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,51 +56,43 @@ class RangeFinderConsistencyCheck final
UNKNOWN = 2
};

float getTestRatioLpf() const { return _test_ratio_lpf.getState(); }
float getInnov() const { return _innov; }
float getInnovVar() const { return _innov_var; }

float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; }
float getInnov() const { return _initialized ? _innov : 0.f; }
float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; }
bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT || _fixed_wing; }
bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT; }
void setGate(const float gate) { _gate = gate; }
void run(const float z, const float vz, const matrix::SquareMatrix<float, estimator::State::size> P,
const float dist_bottom, const float dist_bottom_var, uint64_t time_us);
void reset()
{
if (_initialized) {
if (_state == KinematicState::CONSISTENT) {
_state = KinematicState::UNKNOWN;
}

_initialized = false;
}
_state = (_initialized && _state == KinematicState::CONSISTENT) ? KinematicState::UNKNOWN : _state;
_initialized = false;
}
int getConsistencyState() const { return static_cast<int>(_state); }

void run(const float z, const float vz, const matrix::SquareMatrix<float, estimator::State::size> P,
const float dist_bottom, const float dist_bottom_var, uint64_t time_us);

uint8_t current_posD_reset_count{0};
float terrain_process_noise{0.0f};
bool horizontal_motion{false};

private:

void update(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var,
uint64_t time_us);
void init(float var_z, float var_terrain, float z, float dist_bottom);
matrix::SquareMatrix<float, 2> _R{};
void update(const float z, const float z_var, const float vz, const float vz_var, const float dist_bottom,
const float dist_bottom_var, const uint64_t time_us);
void init(const float z, const float z_var, const float dist_bottom, const float dist_bottom_var);
matrix::SquareMatrix<float, 2> _P{};
matrix::SquareMatrix<float, 2> _A{};
matrix::SquareMatrix<float, 2> _H{};
matrix::Vector2f _x{};
bool _initialized{false};
float _innov{};
float _innov_var{};
float _innov{0.f};
float _innov_var{0.f};
uint64_t _time_last_update_us{0};
float _dist_bottom_prev{};
AlphaFilter<float> _test_ratio_lpf{0.3};
AlphaFilter<float> _test_ratio_lpf{};
float _gate{1.f};
int _sample_count{0};
KinematicState _state{KinematicState::UNKNOWN};
int _min_nr_of_samples{0};
float _t_since_first_sample{0.f};
bool _fixed_wing{false};
uint8_t _last_posD_reset_count{0};
};
Expand Down
14 changes: 9 additions & 5 deletions src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
_range_sensor.setMaxFogDistance(_params.rng_fog);
_rng_consistency_check.setGate(_params.range_kin_consistency_gate);

_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);

Expand All @@ -69,11 +70,13 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
const float dist_var = getRngVar();
_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();

const bool updated_horizontal_motion = (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f);

if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion
&& _rng_consistency_check.isNotKinematicallyInconsistent()) {
_rng_consistency_check.reset();
}

_rng_consistency_check.horizontal_motion = updated_horizontal_motion;
_rng_consistency_check.run(_gpos.altitude(), _state.vel(2), P, _range_sensor.getRange(), dist_var, imu_sample.time_us);
}
Expand All @@ -94,8 +97,9 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
}
}

_control_status.flags.rng_kin_consistent = _rng_consistency_check.getConsistencyState() == 1;
_control_status.flags.rng_kin_unknown = _rng_consistency_check.getConsistencyState() == 2;
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
_control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent()
&& _rng_consistency_check.isNotKinematicallyInconsistent();

} else {
return;
Expand Down Expand Up @@ -228,7 +232,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
}

} else {
if (aid_src.innovation_rejected) {
if (aid_src.innovation_rejected && _rng_consistency_check.isKinematicallyConsistent()) {
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}
Expand Down
5 changes: 4 additions & 1 deletion src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,10 +81,10 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)

} else if (current_time_us - _time_bad_quality_us > _quality_hyst_us) {
// We did not receive bad quality data for some time
updateFogCheck(_sample.rng, _sample.time_us);

if (isTiltOk() && isDataInRange()) {
updateStuckCheck();
updateFogCheck(getDistBottom(), _sample.time_us);

if (!_is_stuck && !_is_blocked) {
_is_sample_valid = true;
Expand Down Expand Up @@ -162,6 +162,9 @@ void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t t
}

_prev_median_dist = median_dist;

} else {
_prev_median_dist = dist_bottom;
}
}

Expand Down
18 changes: 8 additions & 10 deletions src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -725,19 +725,17 @@ def compute_gravity_z_innov_var_and_h(

return (innov_var, H.T)

def range_validation_filter() -> sf.matrix:
def range_validation_filter() -> sf.Matrix:

z = sf.Symbol("z")
dist_bottom = sf.Symbol("dist_bottom")
terrain = dist_bottom - z
state = sf.V2.symbolic("state")
state_indices = {"z": 0, "dist_bottom": 1}
state[state_indices["z"]] = z
state[state_indices["dist_bottom"]] = dist_bottom
state = Values(
z=sf.Symbol("z"),
terrain=sf.Symbol("terrain")
)
dist_bottom = state["z"] - state["terrain"]

H = sf.M22()
H[0, :] = sf.V1(z).jacobian(state, tangent_space=False)
H[1, :] = sf.V1(terrain).jacobian(state, tangent_space=False)
H[0, :] = sf.V1(state["z"]).jacobian(state.to_storage(), tangent_space=False)
H[1, :] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False)

return H

Expand Down
4 changes: 2 additions & 2 deletions src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,8 @@ void RangeValidationFilter(matrix::Matrix<Scalar, 2, 2>* const H = nullptr) {
_h.setZero();

_h(0, 0) = 1;
_h(1, 0) = -1;
_h(1, 1) = 1;
_h(1, 0) = 1;
_h(1, 1) = -1;
}
} // NOLINT(readability/fn_size)

Expand Down
17 changes: 8 additions & 9 deletions src/modules/ekf2/test/test_EKF_height_fusion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ TEST_F(EkfHeightFusionTest, baroRef)
_ekf_wrapper.enableGpsHeightFusion();
_ekf_wrapper.enableRangeHeightFusion();
/* _ekf_wrapper.enableExternalVisionHeightFusion(); */ //TODO: this currently sets the reference to EV
_sensor_simulator.runSeconds(1);
_sensor_simulator.runSeconds(10);

EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO);
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
Expand Down Expand Up @@ -131,8 +131,8 @@ TEST_F(EkfHeightFusionTest, baroRef)
const BiasEstimator::status &gps_status = _ekf->getGpsHgtBiasEstimatorStatus();
EXPECT_NEAR(gps_status.bias, _sensor_simulator._gps.getData().alt - _sensor_simulator._baro.getData(), 0.2f);

const float terrain = _ekf->getTerrainVertPos();
EXPECT_NEAR(terrain, -baro_increment, 1.2f);
const float hagl = _ekf->output_predictor().getLatLonAlt().altitude() + _ekf->state().terrain;
EXPECT_NEAR(hagl, baro_increment, 1.2f);

const BiasEstimator::status &ev_status = _ekf->getEvHgtBiasEstimatorStatus();
EXPECT_EQ(ev_status.bias, 0.f);
Expand All @@ -145,7 +145,6 @@ TEST_F(EkfHeightFusionTest, baroRef)
EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());

// AND WHEN: the gps height increases
const float gps_increment = 1.f;
Expand All @@ -157,8 +156,8 @@ TEST_F(EkfHeightFusionTest, baroRef)
// the estimated height follows the GPS height
EXPECT_NEAR(_ekf->getPosition()(2), -(baro_increment + gps_increment), 0.3f);
// and the range finder bias is adjusted to follow the new reference
const float terrain2 = _ekf->getTerrainVertPos();
EXPECT_NEAR(terrain2, -(baro_increment + gps_increment), 1.3f);
const float hagl2 = _ekf->output_predictor().getLatLonAlt().altitude() + _ekf->state().terrain;
EXPECT_NEAR(hagl2, (baro_increment + gps_increment), 1.3f);
}

TEST_F(EkfHeightFusionTest, gpsRef)
Expand All @@ -168,7 +167,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
_ekf_wrapper.enableBaroHeightFusion();
_ekf_wrapper.enableGpsHeightFusion();
_ekf_wrapper.enableRangeHeightFusion();
_sensor_simulator.runSeconds(1);
_sensor_simulator.runSeconds(10);

EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
Expand Down Expand Up @@ -221,7 +220,7 @@ TEST_F(EkfHeightFusionTest, gpsRefNoAltFusion)
_ekf_wrapper.enableBaroHeightFusion();
_ekf_wrapper.disableGpsHeightFusion();
_ekf_wrapper.enableRangeHeightFusion();
_sensor_simulator.runSeconds(1);
_sensor_simulator.runSeconds(10);

EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); // Fallback to baro as GNSS alt is disabled
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
Expand All @@ -248,7 +247,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
_ekf_wrapper.enableGpsHeightFusion();
_ekf_wrapper.enableRangeHeightFusion();

_sensor_simulator.runSeconds(0.1);
_sensor_simulator.runSeconds(10);

EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO);

Expand Down
Loading

0 comments on commit f7052d6

Please sign in to comment.