dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: use dedicated aid_src message for flow for terrain aiding

This commit is contained in:
bresch 2023-01-19 15:21:27 +01:00 committed by Daniel Agar
parent b4b48cae75
commit 6e30f8f5cb
8 changed files with 49 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/EstimatorAidSource2d.msg
View File

@ -19,4 +19,4 @@ bool innovation_rejected # true if the observation has been rejected
bool fused # true if the sample was successfully fused
# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow
# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow

1
msg/EstimatorInnovations.msg
View File

@ -23,6 +23,7 @@ float32 aux_vvel # vertical auxiliary velocity innovation from landing target
# Optical flow
float32[2] flow # flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)
float32[2] terr_flow # flow innvoation (rad/sec) and innovation variance computed by the terrain estimator ((rad/sec)**2)
# Various
float32 heading # heading innovation (rad) and innovation variance (rad**2)

8
src/modules/ekf2/EKF/ekf.h
View File

@ -115,6 +115,10 @@ public:
const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
void getTerrainFlowInnov(float flow_innov[2]) const;
void getTerrainFlowInnovVar(float flow_innov_var[2]) const;
void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); }
void getHeadingInnov(float &heading_innov) const
{
if (_control_status.flags.mag_hdg) {
@ -431,6 +435,7 @@ public:
const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
private:
@ -570,6 +575,7 @@ private:
estimator_aid_source2d_s _aid_src_aux_vel{};
estimator_aid_source2d_s _aid_src_optical_flow{};
estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
// variables used for the GPS quality checks
Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec)
@ -745,7 +751,7 @@ private:
void startHaglFlowFusion();
void resetHaglFlow();
void stopHaglFlowFusion();
void fuseFlowForTerrain();
void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
void controlHaglFakeFusion();

12
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -427,6 +427,18 @@ void Ekf::getFlowInnovVar(float flow_innov_var[2]) const
flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1];
}
void Ekf::getTerrainFlowInnov(float flow_innov[2]) const
{
flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0];
flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1];
}
void Ekf::getTerrainFlowInnovVar(float flow_innov_var[2]) const
{
flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0];
flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1];
}
// get the state vector at the delayed time horizon
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
{

38
src/modules/ekf2/EKF/terrain_estimator.cpp
View File

@ -254,7 +254,7 @@ void Ekf::controlHaglFlowFusion()
}
if (_flow_data_ready) {
updateOptFlow(_aid_src_optical_flow);
updateOptFlow(_aid_src_terrain_optical_flow);
const bool continuing_conditions_passing = _control_status.flags.in_air
&& !_control_status.flags.opt_flow
@ -267,7 +267,7 @@ void Ekf::controlHaglFlowFusion()
if (continuing_conditions_passing) {
// TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon
fuseFlowForTerrain();
fuseFlowForTerrain(_aid_src_terrain_optical_flow);
_flow_data_ready = false;
// TODO: do something when failing continuously the innovation check
@ -298,15 +298,15 @@ void Ekf::startHaglFlowFusion()
{
_hagl_sensor_status.flags.flow = true;
// TODO: do a reset instead of trying to fuse the data?
fuseFlowForTerrain();
fuseFlowForTerrain(_aid_src_terrain_optical_flow);
_flow_data_ready = false;
}
void Ekf::stopHaglFlowFusion()
{
if (_hagl_sensor_status.flags.flow) {
_hagl_sensor_status.flags.flow = false;
resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
}
}
@ -318,11 +318,11 @@ void Ekf::resetHaglFlow()
_terrain_vpos_reset_counter++;
}
void Ekf::fuseFlowForTerrain()
void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
{
_aid_src_optical_flow.fusion_enabled = true;
flow.fusion_enabled = true;
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
const float R_LOS = flow.observation_variance[0];
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
// a positive offset in earth frame leads to a smaller height above the ground.
@ -332,10 +332,10 @@ void Ekf::fuseFlowForTerrain()
Vector2f innov_var;
float H;
sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H);
innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
innov_var.copyTo(flow.innovation_variance);
if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
|| (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) {
if ((flow.innovation_variance[0] < R_LOS)
|| (flow.innovation_variance[1] < R_LOS)) {
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
_terrain_var = 100.0f;
@ -343,13 +343,13 @@ void Ekf::fuseFlowForTerrain()
}
// run the innovation consistency check and record result
setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
setEstimatorAidStatusTestRatio(flow, math::max(_params.flow_innov_gate, 1.f));
_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
_innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f);
// if either axis fails we abort the fusion
if (_aid_src_optical_flow.innovation_rejected) {
if (flow.innovation_rejected) {
return;
}
@ -360,14 +360,14 @@ void Ekf::fuseFlowForTerrain()
} else if (index == 1) {
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &flow.innovation_variance[1], &H);
// recalculate the innovation using the updated state
const Vector2f vel_body = predictFlowVelBody();
range = predictFlowRange();
_aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1];
flow.innovation[1] = (-vel_body(0) / range) - flow.observation[1];
if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
if (flow.innovation_variance[1] < R_LOS) {
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
_terrain_var = 100.0f;
@ -375,9 +375,9 @@ void Ekf::fuseFlowForTerrain()
}
}
float Kfusion = _terrain_var * H / _aid_src_optical_flow.innovation_variance[index];
float Kfusion = _terrain_var * H / flow.innovation_variance[index];
_terrain_vpos += Kfusion * _aid_src_optical_flow.innovation[0];
_terrain_vpos += Kfusion * flow.innovation[0];
// constrain terrain to minimum allowed value and predict height above ground
_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));

5
src/modules/ekf2/EKF2.cpp
View File

@ -787,6 +787,8 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
// optical flow
PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub);
PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last,
_estimator_aid_src_terrain_optical_flow_pub);
}
void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@ -1073,6 +1075,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
_ekf.getBetaInnov(innovations.beta);
_ekf.getHaglInnov(innovations.hagl);
_ekf.getHaglRateInnov(innovations.hagl_rate);
_ekf.getTerrainFlowInnov(innovations.terr_flow);
// Not yet supported
innovations.aux_vvel = NAN;
@ -1122,6 +1125,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
_ekf.getBetaInnovRatio(test_ratios.beta);
_ekf.getHaglInnovRatio(test_ratios.hagl);
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
// Not yet supported
test_ratios.aux_vvel = NAN;
@ -1147,6 +1151,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
_ekf.getBetaInnovVar(variances.beta);
_ekf.getHaglInnovVar(variances.hagl);
_ekf.getHaglRateInnovVar(variances.hagl_rate);
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
// Not yet supported
variances.aux_vvel = NAN;

2
src/modules/ekf2/EKF2.hpp
View File

@ -288,6 +288,7 @@ private:
hrt_abstime _status_aux_vel_pub_last{0};
hrt_abstime _status_optical_flow_pub_last{0};
hrt_abstime _status_terrain_optical_flow_pub_last{0};
float _last_baro_bias_published{};
float _last_gnss_hgt_bias_published{};
@ -378,6 +379,7 @@ private:
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)};
// publications with topic dependent on multi-mode
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;

2
src/modules/logger/logged_topics.cpp
View File

@ -196,6 +196,7 @@ void LoggedTopics::add_default_topics()
// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
// log all raw sensors at minimal rate (at least 1 Hz)
@ -283,6 +284,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES);
#endif /* CONFIG_ARCH_BOARD_PX4_SITL */