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

ekf2-rng: consider height covariance for terrain reset to range

Browse Source 
Range does not provide a direct terrain observation but a measurement
relative to the height state. Correlation between height and terrain
must be set properly.
This commit is contained in:
bresch 2024-09-23 14:43:57 +02:00 committed by Daniel Agar
parent d269ddfb49
commit 23f343aa3a
2 changed files with 47 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -37,6 +37,7 @@
*/
#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)
@ -270,13 +271,25 @@ float Ekf::getRngVar() const
void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
{
const float new_terrain = _state.pos(2) + aid_src.observation;
const float delta_terrain = new_terrain - _state.terrain;
// 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);
_state.terrain = new_terrain;
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, aid_src.observation_variance);
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;
@ -287,7 +300,6 @@ void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
_state_reset_status.reset_count.hagl++;
aid_src.time_last_fuse = _time_delayed_us;
}

30
src/modules/ekf2/test/test_EKF_terrain.cpp
View File

@ -214,3 +214,33 @@ TEST_F(EkfTerrainTest, testHeightReset)
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
EXPECT_NEAR(estimated_distance_to_ground, _ekf->getHagl(), 1e-3f);
}
TEST_F(EkfTerrainTest, testRngStartInAir)
{
// GIVEN: rng for terrain but not flow
_ekf_wrapper.disableFlowFusion();
_ekf_wrapper.enableRangeHeightFusion();
const float rng_height = 18;
const float flow_height = 1.f;
runFlowAndRngScenario(rng_height, flow_height);
// THEN: the terrain should reset using rng
EXPECT_NEAR(rng_height, _ekf->getHagl(), 1e-3f);
// AND: the terrain state should be highly correlated with the other height states
auto P = _ekf->covariances();
const float var_terrain = _ekf->getTerrainVariance();
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);
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);
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);
}