fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 00:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix bugs, rejection was too sensitive

Browse Source
This commit is contained in:
Marco Hauswirth
2024-10-23 13:53:18 +02:00
parent bd746d761b
commit 16eeddc957
5 changed files with 85 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
+42 -14
View File
@@ -39,16 +39,16 @@
using namespace matrix;
void RangeFinderConsistencyCheck::initMiniKF(float p1, float p2, float x1, float x2)
void RangeFinderConsistencyCheck::initMiniKF(float var_z, float var_terrain, float z, float dist_bottom)
{
_R.setZero();
_A.setIdentity();
float p[4] = {p1, 0.f, 0.f, p2};
float p[4] = {var_z, 0.f, 0.f, var_terrain};
_P = Matrix<float, 2, 2>(p);
float h[4] = {1.f, 0.f, -1.f, 1.f};
_H = Matrix<float, 2, 2>(h);
_x(0) = x1;
_x(1) = x2;
_x(0) = z;
_x(1) = z + dist_bottom;
_initialized = true;
_sample_count = 0;
_state = KinematicState::UNKNOWN;
@@ -64,41 +64,69 @@ void RangeFinderConsistencyCheck::UpdateMiniKF(float z, float z_var, float vz, f
return;
}
if (_min_nr_of_samples == 0) {
_min_nr_of_samples = (int)(1.f / dt);
}
_time_last_update_us = time_us;
_R(0, 0) = z_var;
_R(1, 1) = dist_bottom_var;
SquareMatrix<float, 2> Q;
const float process_noise = 0.01f;
const float process_noise = 0.001f;
Q(0, 0) = dt * dt * vz_var + process_noise;
Q(1, 1) = process_noise * 0.5f;
Q(1, 1) = process_noise;
_x(0) += dt * vz;
_P = _A * _P * _A.transpose() + Q;
const Vector2f measurements(z, dist_bottom);
const Vector2f y = measurements - _H * _x;
const Matrix2f S = _H * _P * _H.transpose() + _R;
float test_ratio = math::min(sq(y(1)) / (sq(_gate) * S(1, 1)), 2.f);
float test_ratio = fminf(abs(sq(y(1)) / (sq(_gate) * S(1, 1))), 2.f);
if (test_ratio < 1.f) {
Matrix2f K = _P * _H.transpose() * S.I();
_x = _x + K * y;
_P = _P - K * _H * _P;
_P = 0.5f * (_P + _P.transpose()); // Ensure symmetry
Matrix2f K = _P * _H.transpose() * S.I();
K(0, 0) = 1.f;
K(0, 1) = 0.f;
if (test_ratio > 1.f) {
K(1, 0) = 0.f;
K(1, 1) = 0.f;
}
_x = _x + K * y;
_P = _P - K * _H * _P;
_P = 0.5f * (_P + _P.transpose()); // Ensure symmetry
_innov = y(1);
_innov_var = S(1, 1);
if (_sample_count++ > _min_nr_of_samples) {
if (_test_ratio_lpf.update(test_ratio) < 1.f) {
_state = KinematicState::CONSISTENT;
} else {
_sample_count = 0;
_state = KinematicState::INCONSISTENT;
}
} else {
_test_ratio_lpf.update(test_ratio);
}
}
void RangeFinderConsistencyCheck::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)
{
const float z_var = P(estimator::State::pos.idx + 2, estimator::State::pos.idx + 2);
const float vz_var = P(estimator::State::vel.idx + 2, estimator::State::vel.idx + 2);
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);
initMiniKF(z_var, terrain_var, z, dist_bottom);
}
UpdateMiniKF(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us);
}
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
+17 -7
View File
@@ -41,6 +41,8 @@
#define EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
#include <mathlib/math/filter/AlphaFilter.hpp>
#include <ekf_derivation/generated/state.h>
class RangeFinderConsistencyCheck final
{
@@ -62,13 +64,15 @@ public:
bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT || _fixed_wing; }
void UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var,
uint64_t time_us);
void initMiniKF(float p1, float p2, float x1, float x2);
void initMiniKF(float var_z, float var_terrain, float z, float dist_bottom);
void stopMiniKF()
{
_initialized = false;
if (_initialized) {
if (_state == KinematicState::CONSISTENT) {
_state = KinematicState::UNKNOWN;
}
if (_state == KinematicState::CONSISTENT) {
_state = KinematicState::UNKNOWN;
_initialized = false;
}
}
int getConsistencyState() const { return static_cast<int>(_state); }
@@ -88,14 +92,19 @@ public:
_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);
uint8_t current_posD_reset_count{0};
private:
matrix::SquareMatrix<float, 2> _R{};
matrix::SquareMatrix<float, 2> _P{};
matrix::SquareMatrix<float, 2> _A{};
matrix::SquareMatrix<float, 2> _H{};
matrix::Vector2f _x{};
bool _initialized{false};
private:
float _innov{};
float _innov_var{};
uint64_t _time_last_update_us{0};
@@ -104,8 +113,9 @@ private:
float _gate{1.f};
int _sample_count{0};
KinematicState _state{KinematicState::UNKNOWN};
const int _min_nr_of_samples{10}; // hardcoded
int _min_nr_of_samples{0};
bool _fixed_wing{false};
uint8_t _last_posD_reset_count{0};
};
#endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+15 -28
View File
@@ -66,20 +66,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
if (_control_status.flags.in_air) {
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float dist_var = sq(_params.range_noise) + dist_dependant_var;
const float dist_var = getRngVar();
_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
if (_control_status.flags.fixed_wing) {
_rng_consistency_check.setFixedWing(true, 2.0f * _params.range_kin_consistency_gate);
if (!_rng_consistency_check.isRunning()) {
_rng_consistency_check.initMiniKF(P(State::pos.idx + 2, State::pos.idx + 2), P(State::terrain.idx, State::terrain.idx),
_state.pos(2), _state.pos(2) + _range_sensor.getRange());
}
_rng_consistency_check.UpdateMiniKF(_state.pos(2), P(State::pos.idx + 2, State::pos.idx + 2), _state.vel(2),
P(State::vel.idx + 2, State::vel.idx + 2), _range_sensor.getRange(), dist_var, imu_sample.time_us);
_rng_consistency_check.run(_state.pos(2), _state.vel(2), P, _range_sensor.getRange(), dist_var, imu_sample.time_us);
} else {
_rng_consistency_check.setFixedWing(false, _params.range_kin_consistency_gate);
@@ -92,17 +84,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
}
} else if (vertical_motion) {
if (!_rng_consistency_check.isRunning()) {
_rng_consistency_check.initMiniKF(P(State::pos.idx + 2, State::pos.idx + 2), P(State::terrain.idx, State::terrain.idx),
_state.pos(2), _state.pos(2) + _range_sensor.getRange());
}
_rng_consistency_check.UpdateMiniKF(_state.pos(2), P(State::pos.idx + 2, State::pos.idx + 2), _state.vel(2),
P(State::vel.idx + 2, State::vel.idx + 2), _range_sensor.getRange(), dist_var, imu_sample.time_us);
_rng_consistency_check.run(_state.pos(2), _state.vel(2), P, _range_sensor.getRange(), dist_var, imu_sample.time_us);
} else {
_rng_consistency_check.setNotMoving();
}
}
}
@@ -110,12 +95,16 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
} 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()
if (_range_sensor.isRegularlySendingData()
&& _range_sensor.isDataReady()) {
if (!_control_status.flags.in_air) {
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setValidity(true); // bypass the checks
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setValidity(true); // bypass the checks
} else {
_rng_consistency_check.stopMiniKF();
}
}
}
@@ -300,11 +289,9 @@ 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);
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float dist_var = sq(_params.range_noise) + dist_dependant_var;
return dist_var;
}
void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
src/modules/ekf2/test/test_EKF_height_fusion.cpp
+8 -2
View File
@@ -113,8 +113,14 @@ TEST_F(EkfHeightFusionTest, baroRef)
// AND WHEN: the baro data increases
const float baro_increment = 4.f;
_sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_increment);
_sensor_simulator.runSeconds(60);
const float baro_initial = _sensor_simulator._baro.getData();
const float baro_inc = 0.2f;
// increase slowly, height jump leads to invalid terrain estimate
while (_sensor_simulator._baro.getData() + baro_inc < baro_initial + baro_increment + 0.01f) {
_sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_inc);
_sensor_simulator.runSeconds(3);
}
// THEN: the height estimate converges to the baro value
// and the other height sources are getting their bias estimated
src/modules/ekf2/test/test_EKF_terrain.cpp
+3 -3
View File
@@ -234,13 +234,13 @@ TEST_F(EkfTerrainTest, testRngStartInAir)
const float corr_terrain_vz = P(State::vel.idx + 2,
State::terrain.idx) / sqrtf(_ekf->getVelocityVariance()(2) * var_terrain);
EXPECT_NEAR(corr_terrain_vz, 0.6f, 0.03f);
EXPECT_TRUE(corr_terrain_vz > 0.6f);
const float corr_terrain_z = P(State::pos.idx + 2,
State::terrain.idx) / sqrtf(_ekf->getPositionVariance()(2) * var_terrain);
EXPECT_NEAR(corr_terrain_z, 0.8f, 0.03f);
EXPECT_TRUE(corr_terrain_z > 0.8f);
const float corr_terrain_abias_z = P(State::accel_bias.idx + 2,
State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain);
EXPECT_NEAR(corr_terrain_abias_z, -0.4f, 0.03f);
EXPECT_TRUE(corr_terrain_abias_z < -0.4f);
}