From 40ff4e8ea7529331d14dd7aae979de8e718c7043 Mon Sep 17 00:00:00 2001 From: jonas Date: Sat, 28 Sep 2024 04:05:29 +0200 Subject: [PATCH] Split update step to enhance modularity and readability --- msg/EstimatorAidSource3d.msg | 2 +- src/modules/logger/logged_topics.cpp | 2 +- .../Position/VTEPosition.cpp | 504 +++++++++++------- .../Position/VTEPosition.h | 86 ++- 4 files changed, 380 insertions(+), 214 deletions(-) diff --git a/msg/EstimatorAidSource3d.msg b/msg/EstimatorAidSource3d.msg index 952d8d3ae346..64b9718608e3 100644 --- a/msg/EstimatorAidSource3d.msg +++ b/msg/EstimatorAidSource3d.msg @@ -22,5 +22,5 @@ bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag -# TOPICS vte_aid_gps_pos_target vte_aid_gps_pos_mission vte_aid_gps_vel_target vte_aid_gps_vel_rel +# TOPICS vte_aid_gps_pos_target vte_aid_gps_pos_mission vte_aid_gps_vel_target vte_aid_gps_vel_uav # TOPICS vte_aid_fiducial_marker diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 47dac565489a..086bd43b32a3 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -185,7 +185,7 @@ void LoggedTopics::add_default_topics() add_topic("vision_target_est_orientation", 100); add_optional_topic("vte_aid_gps_pos_target", 100); add_optional_topic("vte_aid_gps_pos_mission", 100); - add_optional_topic("vte_aid_gps_vel_rel", 100); + add_optional_topic("vte_aid_gps_vel_uav", 100); add_optional_topic("vte_aid_gps_vel_target", 100); add_optional_topic("vte_aid_fiducial_marker", 100); add_optional_topic("vte_aid_ev_yaw", 100); diff --git a/src/modules/vision_target_estimator/Position/VTEPosition.cpp b/src/modules/vision_target_estimator/Position/VTEPosition.cpp index 11e80fe33990..237a6c9fe348 100644 --- a/src/modules/vision_target_estimator/Position/VTEPosition.cpp +++ b/src/modules/vision_target_estimator/Position/VTEPosition.cpp @@ -57,7 +57,7 @@ VTEPosition::VTEPosition() : _targetEstimatorStatePub.advertise(); _vte_aid_gps_pos_target_pub.advertise(); _vte_aid_gps_pos_mission_pub.advertise(); - _vte_aid_gps_vel_rel_pub.advertise(); + _vte_aid_gps_vel_uav_pub.advertise(); _vte_aid_gps_vel_target_pub.advertise(); _vte_aid_fiducial_marker_pub.advertise(); @@ -128,9 +128,11 @@ bool VTEPosition::init() if (_vte_aid_mask & SensorFusionMask::USE_TARGET_GPS_POS) { PX4_INFO("VTE target GPS position data fusion enabled");} + if (_vte_aid_mask & SensorFusionMask::USE_TARGET_GPS_VEL) { PX4_INFO("VTE target GPS velocity data fusion enabled");} + if (_vte_aid_mask & SensorFusionMask::USE_MISSION_POS) { PX4_INFO("VTE PX4 mission position fusion enabled");} - if (_vte_aid_mask & SensorFusionMask::USE_GPS_REL_VEL) { PX4_INFO("VTE relative GPS velocity data fusion enabled");} + if (_vte_aid_mask & SensorFusionMask::USE_UAV_GPS_VEL) { PX4_INFO("VTE UAV GPS velocity data fusion enabled");} if (_vte_aid_mask & SensorFusionMask::USE_EXT_VIS_POS) { PX4_INFO("VTE target external vision-based relative position data fusion enabled");} @@ -265,294 +267,382 @@ void VTEPosition::predictionStep(const Vector3f &vehicle_acc_ned) } } - - bool VTEPosition::update_step(const Vector3f &vehicle_acc_ned) { - - sensor_gps_s vehicle_gps_position; - target_gnss_s target_GNSS_report; - fiducial_marker_pos_report_s fiducial_marker_pose; - - targetObsPos obs_gps_pos_target; - targetObsPos obs_gps_pos_mission; - targetObsPos obs_gps_vel_rel; - targetObsPos obs_gps_vel_target; + // Process data from all observations targetObsPos obs_fiducial_marker; + targetObsPos obs_gps_pos_mission, obs_gps_pos_target; + targetObsPos obs_gps_vel_uav, obs_gps_vel_target; - int vte_fusion_aid_mask = ObservationValidMask::NO_VALID_DATA; + // Update the targetObsPos and vte_fusion_aid_mask if any valid observation. + ObservationValidMask vte_fusion_aid_mask = ObservationValidMask::NO_VALID_DATA; + processVisionData(vte_fusion_aid_mask, obs_fiducial_marker); + processGpsData(vte_fusion_aid_mask, obs_gps_pos_target, obs_gps_pos_mission, obs_gps_vel_uav, obs_gps_vel_target); - // Process data from all topics + // No new observations --> no fusion. + if (vte_fusion_aid_mask == ObservationValidMask::NO_VALID_DATA) { + return false; + } - /* VISION */ - if ((_vte_aid_mask & SensorFusionMask::USE_EXT_VIS_POS) - && _fiducial_marker_report_sub.update(&fiducial_marker_pose)) { + // Check if we have new observations + const bool new_pos_sensor = _hasNewPositionSensorData(vte_fusion_aid_mask); - obs_fiducial_marker.type = fiducial_marker; + // Only init estimator once we have a valid position observation + if (!_estimator_initialized && !new_pos_sensor) { + return false; + } - if ((_is_meas_valid(fiducial_marker_pose.timestamp)) && processObsVision(fiducial_marker_pose, obs_fiducial_marker)) { - vte_fusion_aid_mask += ObservationValidMask::FUSE_EXT_VIS_POS; - } + const bool new_vel_sensor = _hasNewVelocitySensorData(vte_fusion_aid_mask); + + // Initialize estimator if not already initialized + if (!_estimator_initialized && + !initializeEstimator(vte_fusion_aid_mask, obs_fiducial_marker, obs_gps_pos_target, obs_gps_pos_mission)) { + return false; } - /* GPS BASED OBSERVATIONS */ - bool vehicle_gps_position_updated = _vehicle_gps_position_sub.update(&vehicle_gps_position); + // Update bias if needed. For now the only non-gnss measurement is vision. + updateBiasIfNeeded(vte_fusion_aid_mask, obs_fiducial_marker.meas_xyz); - if (_is_meas_updated(vehicle_gps_position.timestamp)) { + // Fuse new sensor data + if (new_pos_sensor || new_vel_sensor) { - bool target_GPS_updated = _target_gnss_sub.update(&target_GNSS_report); + return fuseNewSensorData(vehicle_acc_ned, vte_fusion_aid_mask, obs_fiducial_marker, obs_gps_pos_target, + obs_gps_pos_mission, obs_gps_vel_uav, obs_gps_vel_target); + } - /* TARGET GPS */ - if ((_vte_aid_mask & SensorFusionMask::USE_TARGET_GPS_POS) && target_GPS_updated - && target_GNSS_report.abs_pos_updated) { + return false; +} - obs_gps_pos_target.type = target_gps_pos; +void VTEPosition::processVisionData(ObservationValidMask &vte_fusion_aid_mask, targetObsPos &obs_fiducial_marker) +{ + fiducial_marker_pos_report_s fiducial_marker_pose; - if ((_is_meas_valid(target_GNSS_report.timestamp)) - && processObsGNSSPosTarget(target_GNSS_report, vehicle_gps_position, obs_gps_pos_target)) { - vte_fusion_aid_mask += ObservationValidMask::FUSE_TARGET_GPS_POS; - } - } + if (!((_vte_aid_mask & SensorFusionMask::USE_EXT_VIS_POS) + && _fiducial_marker_report_sub.update(&fiducial_marker_pose))) { + return; + } - /* MISSION GPS POSE */ - if ((_vte_aid_mask & SensorFusionMask::USE_MISSION_POS) && vehicle_gps_position_updated && _mission_position.valid) { + obs_fiducial_marker.type = fiducial_marker; - obs_gps_pos_mission.type = mission_gps_pos; + if (_is_meas_valid(fiducial_marker_pose.timestamp) && + processObsVision(fiducial_marker_pose, obs_fiducial_marker)) { - if ((_is_meas_valid(vehicle_gps_position.timestamp)) - && processObsGNSSPosMission(vehicle_gps_position, obs_gps_pos_mission)) { - vte_fusion_aid_mask += ObservationValidMask::FUSE_MISSION_POS; - } - } + vte_fusion_aid_mask = static_cast(vte_fusion_aid_mask | ObservationValidMask::FUSE_EXT_VIS_POS); + } +} - // Keep track of the drone GPS velocity - _uav_gps_vel.timestamp = vehicle_gps_position.timestamp; - _uav_gps_vel.valid = (vehicle_gps_position.vel_ned_valid && (PX4_ISFINITE(vehicle_gps_position.vel_n_m_s) - && PX4_ISFINITE(vehicle_gps_position.vel_e_m_s) && PX4_ISFINITE(vehicle_gps_position.vel_d_m_s))); - _uav_gps_vel.xyz(Direction::x) = vehicle_gps_position.vel_n_m_s; - _uav_gps_vel.xyz(Direction::y) = vehicle_gps_position.vel_e_m_s; - _uav_gps_vel.xyz(Direction::z) = vehicle_gps_position.vel_d_m_s; - - // Keep track of the target GPS velocity - _target_gps_vel.timestamp = target_GNSS_report.timestamp; - _target_gps_vel.valid = (target_GNSS_report.vel_ned_updated && PX4_ISFINITE(target_GNSS_report.vel_n_m_s) - && PX4_ISFINITE(target_GNSS_report.vel_e_m_s) - && PX4_ISFINITE(target_GNSS_report.vel_d_m_s)); - _target_gps_vel.xyz(Direction::x) = target_GNSS_report.vel_n_m_s; - _target_gps_vel.xyz(Direction::y) = target_GNSS_report.vel_e_m_s; - _target_gps_vel.xyz(Direction::z) = target_GNSS_report.vel_d_m_s; - - if ((_vte_aid_mask & SensorFusionMask::USE_GPS_REL_VEL)) { +void VTEPosition::processGpsData(ObservationValidMask &vte_fusion_aid_mask, + targetObsPos &obs_gps_pos_target, + targetObsPos &obs_gps_pos_mission, + targetObsPos &obs_gps_vel_uav, + targetObsPos &obs_gps_vel_target) +{ + sensor_gps_s vehicle_gps_position; + const bool vehicle_gps_position_updated = _vehicle_gps_position_sub.update(&vehicle_gps_position); -#if defined(CONFIG_VTEST_MOVING) + if (!_is_meas_updated(vehicle_gps_position.timestamp)) { + return; + } - /* TARGET GPS VELOCITY */ - if (target_GPS_updated && _target_gps_vel.valid) { + target_gnss_s target_GNSS_report; + const bool target_GPS_updated = _target_gnss_sub.update(&target_GNSS_report); - obs_gps_vel_target.type = vel_target_gps; + // Process target GPS position + processTargetGpsPosition(target_GPS_updated, target_GNSS_report, vehicle_gps_position, vte_fusion_aid_mask, + obs_gps_pos_target); - if ((_is_meas_valid(target_GNSS_report.timestamp)) && processObsGNSSVelTarget(target_GNSS_report, obs_gps_vel_target)) { - vte_fusion_aid_mask += ObservationValidMask::FUSE_TARGET_GPS_VEL; - } - } + // Process mission GPS pose + processMissionGpsPosition(vehicle_gps_position_updated, vehicle_gps_position, vte_fusion_aid_mask, obs_gps_pos_mission); -#endif // CONFIG_VTEST_MOVING + // Update and process UAV GPS velocity + updateUavGpsVelocity(vehicle_gps_position); + processUavGpsVelocity(vehicle_gps_position_updated, vehicle_gps_position, vte_fusion_aid_mask, obs_gps_vel_uav); - /* RELATIVE GPS velocity */ - if (_uav_gps_vel.valid && vehicle_gps_position_updated) { + // Update and process target GPS velocity + updateTargetGpsVelocity(target_GNSS_report); +#if defined(CONFIG_VTEST_MOVING) + processTargetGpsVelocity(target_GPS_updated, target_GNSS_report, vte_fusion_aid_mask, obs_gps_vel_target); +#endif // CONFIG_VTEST_MOVING - obs_gps_vel_rel.type = vel_rel_gps; +} - if ((_is_meas_valid(vehicle_gps_position.timestamp)) && processObsGNSSVelUav(vehicle_gps_position, obs_gps_vel_rel)) { - vte_fusion_aid_mask += ObservationValidMask::FUSE_GPS_REL_VEL; - } - } - } +void VTEPosition::processTargetGpsPosition(const bool target_GPS_updated, + const target_gnss_s &target_GNSS_report, + const sensor_gps_s &vehicle_gps_position, + ObservationValidMask &vte_fusion_aid_mask, + targetObsPos &obs_gps_pos_target) +{ + if (!((_vte_aid_mask & SensorFusionMask::USE_TARGET_GPS_POS) + && target_GPS_updated + && target_GNSS_report.abs_pos_updated)) { + return; } - const bool new_gnss_pos_sensor = (vte_fusion_aid_mask & ObservationValidMask::FUSE_MISSION_POS) || - (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_POS); - const bool new_non_gnss_pos_sensor = (vte_fusion_aid_mask & ObservationValidMask::FUSE_EXT_VIS_POS); - const bool new_pos_sensor = new_non_gnss_pos_sensor || new_gnss_pos_sensor; - const bool new_vel_sensor = (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_VEL) || - (vte_fusion_aid_mask & ObservationValidMask::FUSE_GPS_REL_VEL); + obs_gps_pos_target.type = target_gps_pos; - // Only estimate the GNSS bias if we have a GNSS estimation and a secondary source of position - const bool should_set_bias = new_non_gnss_pos_sensor && (_is_meas_valid(_pos_rel_gnss.timestamp)); + if (_is_meas_valid(target_GNSS_report.timestamp) && + processObsGNSSPosTarget(target_GNSS_report, vehicle_gps_position, obs_gps_pos_target)) { - if (!_estimator_initialized) { + vte_fusion_aid_mask = static_cast(vte_fusion_aid_mask | + ObservationValidMask::FUSE_TARGET_GPS_POS); + } +} - if (!new_pos_sensor) { - return false; - } +void VTEPosition::processMissionGpsPosition(const bool vehicle_gps_position_updated, + const sensor_gps_s &vehicle_gps_position, + ObservationValidMask &vte_fusion_aid_mask, + targetObsPos &obs_gps_pos_mission) +{ + if (!((_vte_aid_mask & SensorFusionMask::USE_MISSION_POS) && vehicle_gps_position_updated && _mission_position.valid)) { + return; + } - const bool has_initial_velocity_estimate = (_local_velocity.valid && (_is_meas_valid(_local_velocity.timestamp))) || - (_uav_gps_vel.valid && (_is_meas_valid(_uav_gps_vel.timestamp))); + obs_gps_pos_mission.type = mission_gps_pos; - if (!has_initial_velocity_estimate) { - PX4_WARN("No UAV velocity estimate. Estimator cannot be started."); - return false; - } + if (_is_meas_valid(vehicle_gps_position.timestamp) && + processObsGNSSPosMission(vehicle_gps_position, obs_gps_pos_mission)) { - Vector3f pos_init; - Vector3f uav_vel_init; - Vector3f bias_init; -#if defined(CONFIG_VTEST_MOVING) - Vector3f target_acc_init; // Assume null target absolute acceleration - Vector3f target_vel_init; -#endif // CONFIG_VTEST_MOVING + vte_fusion_aid_mask = static_cast(vte_fusion_aid_mask | ObservationValidMask::FUSE_MISSION_POS); + } +} +void VTEPosition::processUavGpsVelocity(const bool vehicle_gps_position_updated, + const sensor_gps_s &vehicle_gps_position, + ObservationValidMask &vte_fusion_aid_mask, + targetObsPos &obs_gps_vel_uav) +{ - // Define the initial relative position of target w.r.t. the drone in NED frame using the available measurement - if (vte_fusion_aid_mask & ObservationValidMask::FUSE_EXT_VIS_POS) { - pos_init = obs_fiducial_marker.meas_xyz; + if (!(_vte_aid_mask & SensorFusionMask::USE_UAV_GPS_VEL && vehicle_gps_position_updated && _uav_gps_vel.valid)) { + return; + } - } else if (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_POS) { - pos_init = obs_gps_pos_target.meas_xyz; + obs_gps_vel_uav.type = uav_gps_vel; - } else if (vte_fusion_aid_mask & ObservationValidMask::FUSE_MISSION_POS) { - pos_init = obs_gps_pos_mission.meas_xyz; - } + if (_is_meas_valid(vehicle_gps_position.timestamp) && + processObsGNSSVelUav(vehicle_gps_position, obs_gps_vel_uav)) { - // Compute the initial bias as the difference between the GPS and external position estimate. - if (should_set_bias) { - // We assume that gnss observations have a bias but other position obs don't. It follows: gnss_obs = state + bias <--> bias = gnss_obs - state - PX4_INFO("VTE Position setting GNSS bias."); - bias_init = _pos_rel_gnss.xyz - pos_init; - _bias_set = true; - } + vte_fusion_aid_mask = static_cast(vte_fusion_aid_mask | ObservationValidMask::FUSE_UAV_GPS_VEL); + } + +} #if defined(CONFIG_VTEST_MOVING) +void VTEPosition::processTargetGpsVelocity(const bool target_GPS_updated, + const target_gnss_s &target_GNSS_report, + ObservationValidMask &vte_fusion_aid_mask, + targetObsPos &obs_gps_vel_target) +{ - if (_target_gps_vel.valid && (_is_meas_valid(_target_gps_vel.timestamp))) { - target_vel_init = _target_gps_vel.xyz; - } + if (!(_vte_aid_mask & SensorFusionMask::USE_TARGET_GPS_VEL && target_GPS_updated && _target_gps_vel.valid)) { + return; + } + obs_gps_vel_target.type = target_gps_vel; + + if (_is_meas_valid(target_GNSS_report.timestamp) && + processObsGNSSVelTarget(target_GNSS_report, obs_gps_vel_target)) { + + vte_fusion_aid_mask = static_cast(vte_fusion_aid_mask | + ObservationValidMask::FUSE_TARGET_GPS_VEL); + } + +} #endif // CONFIG_VTEST_MOVING - // Define initial relative velocity of the target w.r.t. to the drone in NED frame - if (_uav_gps_vel.valid && (_is_meas_valid(_uav_gps_vel.timestamp))) { +void VTEPosition::updateUavGpsVelocity(const sensor_gps_s &vehicle_gps_position) +{ + _uav_gps_vel.timestamp = vehicle_gps_position.timestamp; + _uav_gps_vel.valid = (vehicle_gps_position.vel_ned_valid && + PX4_ISFINITE(vehicle_gps_position.vel_n_m_s) && + PX4_ISFINITE(vehicle_gps_position.vel_e_m_s) && + PX4_ISFINITE(vehicle_gps_position.vel_d_m_s)); + + _uav_gps_vel.xyz(Direction::x) = vehicle_gps_position.vel_n_m_s; + _uav_gps_vel.xyz(Direction::y) = vehicle_gps_position.vel_e_m_s; + _uav_gps_vel.xyz(Direction::z) = vehicle_gps_position.vel_d_m_s; +} - uav_vel_init = _uav_gps_vel.xyz; +void VTEPosition::updateTargetGpsVelocity(const target_gnss_s &target_GNSS_report) +{ + _target_gps_vel.timestamp = target_GNSS_report.timestamp; + _target_gps_vel.valid = (target_GNSS_report.vel_ned_updated && + PX4_ISFINITE(target_GNSS_report.vel_n_m_s) && + PX4_ISFINITE(target_GNSS_report.vel_e_m_s) && + PX4_ISFINITE(target_GNSS_report.vel_d_m_s)); + + _target_gps_vel.xyz(Direction::x) = target_GNSS_report.vel_n_m_s; + _target_gps_vel.xyz(Direction::y) = target_GNSS_report.vel_e_m_s; + _target_gps_vel.xyz(Direction::z) = target_GNSS_report.vel_d_m_s; +} - } else if (_local_velocity.valid && (_is_meas_valid(_local_velocity.timestamp))) { +bool VTEPosition::initializeEstimator(const ObservationValidMask &vte_fusion_aid_mask, + const targetObsPos &obs_fiducial_marker, + const targetObsPos &obs_gps_pos_target, + const targetObsPos &obs_gps_pos_mission) +{ + if (!_hasNewPositionSensorData(vte_fusion_aid_mask)) { + return false; + } - uav_vel_init = _local_velocity.xyz; - } + // Check for initial velocity estimate + const bool has_initial_velocity_estimate = (_local_velocity.valid && _is_meas_valid(_local_velocity.timestamp)) || + (_uav_gps_vel.valid && _is_meas_valid(_uav_gps_vel.timestamp)); - // Initial state - matrix::Matrix state_init; - state_init.col(vtest::State::pos_rel.idx) = pos_init; - state_init.col(vtest::State::vel_uav.idx) = uav_vel_init; - state_init.col(vtest::State::bias.idx) = bias_init; + if (!has_initial_velocity_estimate) { + PX4_WARN("No UAV velocity estimate. Estimator cannot be started."); + return false; + } + // Initialize state variables + Vector3f pos_init; + Vector3f uav_vel_init; + Vector3f bias_init; #if defined(CONFIG_VTEST_MOVING) - state_init.col(vtest::State::acc_target.idx) = target_acc_init; - state_init.col(vtest::State::vel_target.idx) = target_vel_init; + Vector3f target_acc_init; // Assume null target absolute acceleration + Vector3f target_vel_init; #endif // CONFIG_VTEST_MOVING - if (initEstimator(state_init)) { - PX4_INFO("VTE Position Estimator properly initialized."); - _estimator_initialized = true; - _uav_gps_vel.valid = false; - _target_gps_vel.valid = false; - _last_update = hrt_absolute_time(); - _last_predict = _last_update; + // Define the initial relative position + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_EXT_VIS_POS) { + pos_init = obs_fiducial_marker.meas_xyz; - } else { - resetFilter(); - } - } + } else if (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_POS) { + pos_init = obs_gps_pos_target.meas_xyz; - if (!_estimator_initialized) { - return false; + } else if (vte_fusion_aid_mask & ObservationValidMask::FUSE_MISSION_POS) { + pos_init = obs_gps_pos_mission.meas_xyz; } - // Once a position measurement other than the target GPS is available, reset the position and bias but keep the velocity estimate. - if (!_bias_set && should_set_bias) { + // Compute initial bias if needed + if (_should_set_bias(vte_fusion_aid_mask)) { + PX4_INFO("VTE Position setting GNSS bias."); + bias_init = _pos_rel_gnss.xyz - pos_init; + _bias_set = true; + } - PX4_INFO("Second relative position measurement available, re-setting position and bias."); +#if defined(CONFIG_VTEST_MOVING) - const float state_pos_var = _param_vte_pos_unc_in.get(); - const float state_bias_var = _param_vte_bias_unc_in.get(); + if (_target_gps_vel.valid && (_is_meas_valid(_target_gps_vel.timestamp))) { + target_vel_init = _target_gps_vel.xyz; + } - const Vector3f state_pos_var_vect(state_pos_var, state_pos_var, state_pos_var); - const Vector3f state_bias_var_vect(state_bias_var, state_bias_var, state_bias_var); - const Vector3f pos_init = obs_fiducial_marker.meas_xyz; +#endif // CONFIG_VTEST_MOVING - // Compute the initial bias as the difference between the GPS and external position estimate. - // We assume that gnss observations have a bias but other position obs don't. It follows: gnss_obs = state + bias <--> bias = gnss_obs - state - const Vector3f bias_init = _pos_rel_gnss.xyz - pos_init; + // Define initial UAV velocity + if (_uav_gps_vel.valid && _is_meas_valid(_uav_gps_vel.timestamp)) { + uav_vel_init = _uav_gps_vel.xyz; - /* Reset filter's state and variance*/ - for (int i = 0; i < Direction::nb_directions; i++) { + } else if (_local_velocity.valid && _is_meas_valid(_local_velocity.timestamp)) { + uav_vel_init = _local_velocity.xyz; + } - matrix::Vector temp_state = _target_estimator[i]->getState(); - temp_state(vtest::State::bias.idx) = bias_init(i); - temp_state(vtest::State::pos_rel.idx) = pos_init(i); - _target_estimator[i]->setState(temp_state); + // Initialize estimator state + matrix::Matrix state_init; + state_init.col(vtest::State::pos_rel.idx) = pos_init; + state_init.col(vtest::State::vel_uav.idx) = uav_vel_init; + state_init.col(vtest::State::bias.idx) = bias_init; - matrix::Vector temp_state_var = - _target_estimator[i]->getStateVar(); - temp_state_var(vtest::State::bias.idx) = state_bias_var_vect(i); - temp_state_var(vtest::State::pos_rel.idx) = state_pos_var_vect(i); - _target_estimator[i]->setStateVar(temp_state_var); - } +#if defined(CONFIG_VTEST_MOVING) + state_init.col(vtest::State::acc_target.idx) = target_acc_init; + state_init.col(vtest::State::vel_target.idx) = target_vel_init; +#endif // CONFIG_VTEST_MOVING - _bias_set = true; + if (!initEstimator(state_init)) { + resetFilter(); + return false; } - // If we have a new sensor: fuse available measurements and publish innov. - if (new_pos_sensor || new_vel_sensor) { + PX4_INFO("VTE Position Estimator properly initialized."); + _estimator_initialized = true; + _uav_gps_vel.valid = false; + _target_gps_vel.valid = false; + _last_update = hrt_absolute_time(); + _last_predict = _last_update; + return true; +} - if (vte_fusion_aid_mask == ObservationValidMask::NO_VALID_DATA) { - return false; - } +void VTEPosition::updateBiasIfNeeded(const ObservationValidMask &vte_fusion_aid_mask, const Vector3f &pos_init) +{ - bool pos_fused = false; + if (_bias_set || !_should_set_bias(vte_fusion_aid_mask)) { + return; + } - /*TARGET GPS*/ - if (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_POS) { + PX4_INFO("Second relative position measurement available, re-setting position and bias."); - if (fuse_meas(vehicle_acc_ned, obs_gps_pos_target)) { - pos_fused = true; - } - } + const float state_pos_var = _param_vte_pos_unc_in.get(); + const float state_bias_var = _param_vte_bias_unc_in.get(); - /*MISSION POS GPS*/ - if (vte_fusion_aid_mask & ObservationValidMask::FUSE_MISSION_POS) { + const Vector3f state_pos_var_vect(state_pos_var, state_pos_var, state_pos_var); + const Vector3f state_bias_var_vect(state_bias_var, state_bias_var, state_bias_var); - if (fuse_meas(vehicle_acc_ned, obs_gps_pos_mission)) { - pos_fused = true; - } - } + // Compute the initial bias + const Vector3f bias_init = _pos_rel_gnss.xyz - pos_init; - /*VISION*/ - if (vte_fusion_aid_mask & ObservationValidMask::FUSE_EXT_VIS_POS) { + // Reset filter's state and variance + for (int i = 0; i < Direction::nb_directions; i++) { + matrix::Vector temp_state = _target_estimator[i]->getState(); + temp_state(vtest::State::bias.idx) = bias_init(i); + temp_state(vtest::State::pos_rel.idx) = pos_init(i); + _target_estimator[i]->setState(temp_state); + + matrix::Vector temp_state_var = _target_estimator[i]->getStateVar(); + temp_state_var(vtest::State::bias.idx) = state_bias_var_vect(i); + temp_state_var(vtest::State::pos_rel.idx) = state_pos_var_vect(i); + _target_estimator[i]->setStateVar(temp_state_var); + } + + _bias_set = true; +} - if (fuse_meas(vehicle_acc_ned, obs_fiducial_marker)) { - _last_vision_obs_fused_time = hrt_absolute_time(); - pos_fused = true; - } +bool VTEPosition::fuseNewSensorData(const Vector3f &vehicle_acc_ned, + const ObservationValidMask &vte_fusion_aid_mask, + const targetObsPos &obs_fiducial_marker, + const targetObsPos &obs_gps_pos_target, + const targetObsPos &obs_gps_pos_mission, + const targetObsPos &obs_gps_vel_uav, + const targetObsPos &obs_gps_vel_target) +{ + bool pos_fused = false; + + // Fuse target GPS position + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_POS) { + if (fuse_meas(vehicle_acc_ned, obs_gps_pos_target)) { + pos_fused = true; } + } - /*GPS RELATIVE VELOCITY*/ - if (vte_fusion_aid_mask & ObservationValidMask::FUSE_GPS_REL_VEL) { - fuse_meas(vehicle_acc_ned, obs_gps_vel_rel); + // Fuse mission GPS position + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_MISSION_POS) { + if (fuse_meas(vehicle_acc_ned, obs_gps_pos_mission)) { + pos_fused = true; } + } - /*TARGET GPS VELOCITY*/ - if (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_VEL) { - fuse_meas(vehicle_acc_ned, obs_gps_vel_target); + // Fuse vision position + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_EXT_VIS_POS) { + if (fuse_meas(vehicle_acc_ned, obs_fiducial_marker)) { + _last_vision_obs_fused_time = hrt_absolute_time(); + pos_fused = true; } + } + + // Fuse GPS relative velocity + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_UAV_GPS_VEL) { + fuse_meas(vehicle_acc_ned, obs_gps_vel_uav); + } - // If at least one pos measurement was fused, consider the filter updated - return pos_fused; +#if defined(CONFIG_VTEST_MOVING) + // Fuse target GPS velocity + if (vte_fusion_aid_mask & ObservationValidMask::FUSE_TARGET_GPS_VEL) { + fuse_meas(vehicle_acc_ned, obs_gps_vel_target); } - return false; +#endif // CONFIG_VTEST_MOVING + + return pos_fused; } /*Vision observation: [rx, ry, rz]*/ @@ -926,11 +1016,11 @@ void VTEPosition::publishInnov(const estimator_aid_source3d_s &target_innov, con _vte_aid_gps_pos_mission_pub.publish(target_innov); break; - case ObservationType::vel_rel_gps: - _vte_aid_gps_vel_rel_pub.publish(target_innov); + case ObservationType::uav_gps_vel: + _vte_aid_gps_vel_uav_pub.publish(target_innov); break; - case ObservationType::vel_target_gps: + case ObservationType::target_gps_vel: _vte_aid_gps_vel_target_pub.publish(target_innov); break; diff --git a/src/modules/vision_target_estimator/Position/VTEPosition.h b/src/modules/vision_target_estimator/Position/VTEPosition.h index 3dd183b25d07..e17e2a53ac5c 100644 --- a/src/modules/vision_target_estimator/Position/VTEPosition.h +++ b/src/modules/vision_target_estimator/Position/VTEPosition.h @@ -141,7 +141,7 @@ class VTEPosition: public ModuleParams uORB::Publication _vte_aid_gps_pos_target_pub{ORB_ID(vte_aid_gps_pos_target)}; uORB::Publication _vte_aid_gps_pos_mission_pub{ORB_ID(vte_aid_gps_pos_mission)}; uORB::Publication _vte_aid_gps_vel_target_pub{ORB_ID(vte_aid_gps_vel_target)}; - uORB::Publication _vte_aid_gps_vel_rel_pub{ORB_ID(vte_aid_gps_vel_rel)}; + uORB::Publication _vte_aid_gps_vel_uav_pub{ORB_ID(vte_aid_gps_vel_uav)}; uORB::Publication _vte_aid_fiducial_marker_pub{ORB_ID(vte_aid_fiducial_marker)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -163,8 +163,8 @@ class VTEPosition: public ModuleParams enum ObservationType { target_gps_pos = 0, mission_gps_pos = 1, - vel_rel_gps = 2, - vel_target_gps = 3, + uav_gps_vel = 2, + target_gps_vel = 3, fiducial_marker = 4, }; @@ -185,16 +185,17 @@ class VTEPosition: public ModuleParams // Bit locations for fusion_mode NO_SENSOR_FUSION = 0, USE_TARGET_GPS_POS = (1 << 0), ///< set to true to use target GPS position data - USE_GPS_REL_VEL = (1 << 1), ///< set to true to use drone GPS velocity data (and target GPS velocity data if the target is moving) + USE_UAV_GPS_VEL = (1 << 1), ///< set to true to use drone GPS velocity data USE_EXT_VIS_POS = (1 << 2), ///< set to true to use target external vision-based relative position data USE_MISSION_POS = (1 << 3), ///< set to true to use the PX4 mission position + USE_TARGET_GPS_VEL = (1 << 4), ///< set to true to use target GPS velocity data. Only for moving targets. }; enum ObservationValidMask : uint8_t { // Bit locations for valid observations NO_VALID_DATA = 0, FUSE_TARGET_GPS_POS = (1 << 0), ///< set to true if target GPS position data is ready to be fused - FUSE_GPS_REL_VEL = (1 << 1), ///< set to true if drone GPS velocity data (and target GPS velocity data if the target is moving) + FUSE_UAV_GPS_VEL = (1 << 1), ///< set to true if drone GPS velocity data (and target GPS velocity data if the target is moving) FUSE_EXT_VIS_POS = (1 << 2), ///< set to true if target external vision-based relative position data is ready to be fused FUSE_MISSION_POS = (1 << 3), ///< set to true if the PX4 mission position is ready to be fused FUSE_TARGET_GPS_VEL = (1 << 4), ///< set to true if target GPS velocity data is ready to be fused @@ -206,12 +207,87 @@ class VTEPosition: public ModuleParams bool update_step(const matrix::Vector3f &vehicle_acc_ned); void predictionStep(const matrix::Vector3f &acc); + void updateTargetGpsVelocity(const target_gnss_s &target_GNSS_report); + void updateUavGpsVelocity(const sensor_gps_s &vehicle_gps_position); + + inline bool _hasNewNonGpsPositionSensorData(const ObservationValidMask &vte_fusion_aid_mask) const + { + return vte_fusion_aid_mask & ObservationValidMask::FUSE_EXT_VIS_POS; + } + + inline bool _hasNewPositionSensorData(const ObservationValidMask &vte_fusion_aid_mask) const + { + return vte_fusion_aid_mask & (ObservationValidMask::FUSE_MISSION_POS | + ObservationValidMask::FUSE_TARGET_GPS_POS | + ObservationValidMask::FUSE_EXT_VIS_POS); + } + + inline bool _hasNewVelocitySensorData(const ObservationValidMask &vte_fusion_aid_mask) const + { + return vte_fusion_aid_mask & (ObservationValidMask::FUSE_TARGET_GPS_VEL | + ObservationValidMask::FUSE_UAV_GPS_VEL); + } + + // Only estimate the GNSS bias if we have a GNSS estimation and a secondary source of position + inline bool _should_set_bias(const ObservationValidMask &vte_fusion_aid_mask) + { + return _is_meas_valid(_pos_rel_gnss.timestamp) && _hasNewNonGpsPositionSensorData(vte_fusion_aid_mask); + }; + + + bool initializeEstimator(const ObservationValidMask &vte_fusion_aid_mask, + const targetObsPos &obs_fiducial_marker, + const targetObsPos &obs_gps_pos_target, + const targetObsPos &obs_gps_pos_mission); + + void updateBiasIfNeeded(const ObservationValidMask &vte_fusion_aid_mask, const matrix::Vector3f &pos_init); + + bool fuseNewSensorData(const matrix::Vector3f &vehicle_acc_ned, + const ObservationValidMask &vte_fusion_aid_mask, + const targetObsPos &obs_fiducial_marker, + const targetObsPos &obs_gps_pos_target, + const targetObsPos &obs_gps_pos_mission, + const targetObsPos &obs_gps_vel_rel, + const targetObsPos &obs_gps_vel_target); + + /* Vision data */ + void processVisionData(ObservationValidMask &vte_fusion_aid_mask, targetObsPos &obs_fiducial_marker); bool processObsVision(const fiducial_marker_pos_report_s &fiducial_marker_pose, targetObsPos &obs); + + /* GPS data */ + void processGpsData(ObservationValidMask &vte_fusion_aid_mask, + targetObsPos &obs_gps_pos_target, + targetObsPos &obs_gps_pos_mission, + targetObsPos &obs_gps_vel_rel, + targetObsPos &obs_gps_vel_target); + + void processTargetGpsPosition(const bool target_GPS_updated, + const target_gnss_s &target_GNSS_report, + const sensor_gps_s &vehicle_gps_position, + ObservationValidMask &vte_fusion_aid_mask, + targetObsPos &obs_gps_pos_target); + + void processMissionGpsPosition(const bool vehicle_gps_position_updated, + const sensor_gps_s &vehicle_gps_position, + ObservationValidMask &vte_fusion_aid_mask, + targetObsPos &obs_gps_pos_mission); + + void processUavGpsVelocity( + const bool vehicle_gps_position_updated, + const sensor_gps_s &vehicle_gps_position, + ObservationValidMask &vte_fusion_aid_mask, + targetObsPos &obs_gps_vel_uav); + bool processObsGNSSPosTarget(const target_gnss_s &target_GNSS_report, const sensor_gps_s &vehicle_gps_position, targetObsPos &obs); bool processObsGNSSPosMission(const sensor_gps_s &vehicle_gps_position, targetObsPos &obs); bool processObsGNSSVelUav(const sensor_gps_s &vehicle_gps_position, targetObsPos &obs); #if defined(CONFIG_VTEST_MOVING) + void processTargetGpsVelocity( + const bool target_GPS_updated, + const target_gnss_s &target_GNSS_report, + ObservationValidMask &vte_fusion_aid_mask, + targetObsPos &obs_gps_vel_target); bool processObsGNSSVelTarget(const target_gnss_s &target_GNSS_report, targetObsPos &obs); #endif // CONFIG_VTEST_MOVING