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: EV overhaul yaw and position fusion (#20501)

Browse Source 
- move EV yaw and EV position to new state machines
 - EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK)
 - new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion)
 - yaw_align now strictly means north (no more rotate external vision aid mask)
 - automatic switching between EV yaw, and yaw align north based on GPS quality
This commit is contained in:
Daniel Agar 2022-12-20 10:23:56 -05:00 committed by GitHub
parent 20342216e2
commit 54a32eb2f7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
34 changed files with 1019 additions and 553 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -74,6 +74,7 @@ set(msg_files
EstimatorAidSource2d.msg
EstimatorAidSource3d.msg
EstimatorBias.msg
EstimatorBias3d.msg
EstimatorEventFlags.msg
EstimatorGpsStatus.msg
EstimatorInnovations.msg

2
msg/EstimatorBias.msg
View File

@ -9,4 +9,4 @@ float32 innov # innovation of the last measurement fusion (m)
float32 innov_var # innovation variance of the last measurement fusion (m^2)
float32 innov_test_ratio # normalized innovation squared test ratio
# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias estimator_ev_hgt_bias
# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias

14
msg/EstimatorBias3d.msg Normal file
View File

@ -0,0 +1,14 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32[3] bias # estimated barometric altitude bias (m)
float32[3] bias_var # estimated barometric altitude bias variance (m^2)
float32[3] innov # innovation of the last measurement fusion (m)
float32[3] innov_var # innovation variance of the last measurement fusion (m^2)
float32[3] innov_test_ratio # normalized innovation squared test ratio
# TOPICS estimator_bias3d
# TOPICS estimator_ev_pos_bias

2
msg/VehicleOdometry.msg
View File

@ -28,4 +28,4 @@ uint8 reset_counter
int8 quality
# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
# TOPICS estimator_odometry estimator_visual_odometry_aligned
# TOPICS estimator_odometry

3
src/modules/ekf2/CMakeLists.txt
View File

@ -91,8 +91,11 @@ px4_add_module(
EKF/ekf_helper.cpp
EKF/EKFGSF_yaw.cpp
EKF/estimator_interface.cpp
EKF/ev_control.cpp
EKF/ev_height_control.cpp
EKF/ev_pos_control.cpp
EKF/ev_vel_control.cpp
EKF/ev_yaw_control.cpp
EKF/fake_height_control.cpp
EKF/fake_pos_control.cpp
EKF/gnss_height_control.cpp

3
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -42,8 +42,11 @@ add_library(ecl_EKF
ekf_helper.cpp
EKFGSF_yaw.cpp
estimator_interface.cpp
ev_control.cpp
ev_height_control.cpp
ev_pos_control.cpp
ev_vel_control.cpp
ev_yaw_control.cpp
fake_height_control.cpp
fake_pos_control.cpp
gnss_height_control.cpp

2
src/modules/ekf2/EKF/bias_estimator.hpp
View File

@ -64,7 +64,9 @@ public:
float innov_test_ratio{INFINITY};
};
BiasEstimator() {}
BiasEstimator(float state_init, float state_var_init): _state{state_init}, _state_var{state_var_init} {};
virtual ~BiasEstimator() = default;
void reset()

25
src/modules/ekf2/EKF/common.h
View File

@ -79,6 +79,11 @@ static constexpr float BADACC_BIAS_PNOISE = 4.9f; ///< The delta velocity proce
// ground effect compensation
static constexpr uint64_t GNDEFFECT_TIMEOUT = 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
enum class PositionFrame : uint8_t {
LOCAL_FRAME_NED = 0,
LOCAL_FRAME_FRD = 1,
};
enum class VelocityFrame : uint8_t {
LOCAL_FRAME_NED = 0,
LOCAL_FRAME_FRD = 1,
@ -115,6 +120,12 @@ enum HeightSensor : uint8_t {
UNKNOWN = 4
};
enum class PositionSensor : uint8_t {
UNKNOWN = 0,
GNSS = 1,
EV = 2,
};
enum GnssCtrl : uint8_t {
HPOS = (1<<0),
VPOS = (1<<1),
@ -140,11 +151,11 @@ enum SensorFusionMask : uint16_t {
DEPRECATED_USE_GPS = (1<<0), ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
USE_OPT_FLOW = (1<<1), ///< set to true to use optical flow data
INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias
USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data
USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw
DEPRECATED_USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data
DEPRECATED_USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw
USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind
ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
DEPRECATED_USE_GPS_YAW = (1<<7),///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl)
DEPRECATED_ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
DEPRECATED_USE_GPS_YAW = (1<<7), ///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl)
DEPRECATED_USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
};
@ -238,9 +249,10 @@ struct extVisionSample {
Vector3f pos{}; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
Vector3f vel{}; ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
Quatf quat{}; ///< quaternion defining rotation from body to earth frame
Vector3f posVar{}; ///< XYZ position variances (m**2)
Vector3f position_var{}; ///< XYZ position variances (m**2)
Vector3f velocity_var{}; ///< XYZ velocity variances ((m/sec)**2)
Vector3f orientation_var{}; ///< orientation variance (rad**2)
PositionFrame pos_frame = PositionFrame::LOCAL_FRAME_FRD;
VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
uint8_t reset_counter{};
int8_t quality{}; ///< quality indicator between 0 and 100
@ -283,6 +295,7 @@ struct parameters {
// measurement source control
int32_t fusion_mode{}; ///< bitmasked integer that selects some aiding sources
int32_t height_sensor_ref{HeightSensor::BARO};
int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
int32_t baro_ctrl{1};
int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
int32_t rng_ctrl{RngCtrl::CONDITIONAL};
@ -325,7 +338,7 @@ struct parameters {
const float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec)
// position and velocity fusion
float gps_vel_noise{5.0e-1f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec)
float gps_vel_noise{0.5f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec)
float gps_pos_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m)
float gps_hgt_bias_nsd{0.13f}; ///< process noise for gnss height bias estimation (m/s/sqrt(Hz))
float pos_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m)

213
src/modules/ekf2/EKF/control.cpp
View File

@ -183,10 +183,6 @@ void Ekf::controlFusionModes()
}
}
if (_ext_vision_buffer) {
_ev_data_ready = _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
}
if (_airspeed_buffer) {
_tas_data_ready = _airspeed_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
}
@ -203,7 +199,7 @@ void Ekf::controlFusionModes()
controlDragFusion();
controlHeightFusion();
// Additional data odoemtery data from an external estimator can be fused.
// Additional data odometry data from an external estimator can be fused.
controlExternalVisionFusion();
// Additional horizontal velocity data from an auxiliary sensor can be fused
@ -221,213 +217,6 @@ void Ekf::controlFusionModes()
updateDeadReckoningStatus();
}
void Ekf::controlExternalVisionFusion()
{
// Check for new external vision data
if (_ev_data_ready) {
bool reset = false;
if (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) {
reset = true;
}
if (_inhibit_ev_yaw_use) {
stopEvYawFusion();
}
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
// needs to be calculated and the observations rotated into the EKF frame of reference
if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) && ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)
|| (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) && !_control_status.flags.ev_yaw) {
// rotate EV measurements into the EKF Navigation frame
calcExtVisRotMat();
}
// external vision aiding selection logic
if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
// check for a external vision measurement that has fallen behind the fusion time horizon
if (isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) {
// turn on use of external vision measurements for position
if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) {
startEvPosFusion();
}
}
}
// external vision yaw aiding selection logic
if (!_inhibit_ev_yaw_use && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw
&& _control_status.flags.tilt_align) {
// don't start using EV data unless data is arriving frequently
if (isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) {
if (resetYawToEv()) {
_control_status.flags.yaw_align = true;
startEvYawFusion();
}
}
}
// determine if we should use the horizontal position observations
if (_control_status.flags.ev_pos) {
resetEstimatorAidStatus(_aid_src_ev_pos);
if (reset && _control_status_prev.flags.ev_pos) {
if (!_fuse_hpos_as_odom) {
resetHorizontalPositionToVision();
}
}
Vector3f ev_pos_obs_var;
// correct position and height for offset relative to IMU
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_ev_sample_delayed.pos -= pos_offset_earth;
// Use an incremental position fusion method for EV position data if GPS is also used
if (_params.gnss_ctrl & GnssCtrl::HPOS) {
_fuse_hpos_as_odom = true;
} else {
_fuse_hpos_as_odom = false;
}
if (_fuse_hpos_as_odom) {
if (!_hpos_prev_available) {
// no previous observation available to calculate position change
_hpos_prev_available = true;
} else {
// calculate the change in position since the last measurement
// rotate measurement into body frame is required when fusing with GPS
Vector3f ev_delta_pos = _R_ev_to_ekf * Vector3f(_ev_sample_delayed.pos - _ev_sample_delayed_prev.pos);
// use the change in position since the last measurement
_aid_src_ev_pos.observation[0] = ev_delta_pos(0);
_aid_src_ev_pos.observation[1] = ev_delta_pos(1);
_aid_src_ev_pos.innovation[0] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
_aid_src_ev_pos.innovation[1] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
// observation 1-STD error, incremental pos observation is expected to have more uncertainty
Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.5f));
ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.5f));
_aid_src_ev_pos.observation_variance[0] = ev_pos_obs_var(0);
_aid_src_ev_pos.observation_variance[1] = ev_pos_obs_var(1);
_aid_src_ev_pos.innovation_variance[0] = P(7, 7) + _aid_src_ev_pos.observation_variance[0];
_aid_src_ev_pos.innovation_variance[1] = P(8, 8) + _aid_src_ev_pos.observation_variance[1];
}
} else {
// use the absolute position
Vector3f ev_pos_meas = _ev_sample_delayed.pos;
Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
ev_pos_meas = _R_ev_to_ekf * ev_pos_meas;
ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
}
_aid_src_ev_pos.observation[0] = ev_pos_meas(0);
_aid_src_ev_pos.observation[1] = ev_pos_meas(1);
_aid_src_ev_pos.observation_variance[0] = fmaxf(ev_pos_var(0, 0), sq(0.01f));
_aid_src_ev_pos.observation_variance[1] = fmaxf(ev_pos_var(1, 1), sq(0.01f));
_aid_src_ev_pos.innovation[0] = _state.pos(0) - _aid_src_ev_pos.observation[0];
_aid_src_ev_pos.innovation[1] = _state.pos(1) - _aid_src_ev_pos.observation[1];
_aid_src_ev_pos.innovation_variance[0] = P(7, 7) + _aid_src_ev_pos.observation_variance[0];
_aid_src_ev_pos.innovation_variance[1] = P(8, 8) + _aid_src_ev_pos.observation_variance[1];
// check if we have been deadreckoning too long
if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) {
resetHorizontalPositionToVision();
}
}
// innovation gate size
const float ev_pos_innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.0f);
setEstimatorAidStatusTestRatio(_aid_src_ev_pos, ev_pos_innov_gate);
_aid_src_ev_pos.timestamp_sample = _ev_sample_delayed.time_us;
_aid_src_ev_pos.fusion_enabled = true;
fuseHorizontalPosition(_aid_src_ev_pos);
}
// determine if we should use the yaw observation
resetEstimatorAidStatus(_aid_src_ev_yaw);
const float measured_hdg = getEulerYaw(_ev_sample_delayed.quat);
const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.orientation_var(2), 1.e-4f);
if (PX4_ISFINITE(measured_hdg)) {
_aid_src_ev_yaw.timestamp_sample = _ev_sample_delayed.time_us;
_aid_src_ev_yaw.observation = measured_hdg;
_aid_src_ev_yaw.observation_variance = ev_yaw_obs_var;
_aid_src_ev_yaw.fusion_enabled = _control_status.flags.ev_yaw;
if (_control_status.flags.ev_yaw) {
if (reset && _control_status_prev.flags.ev_yaw) {
resetYawToEv();
}
const float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
fuseYaw(innovation, ev_yaw_obs_var, _aid_src_ev_yaw);
} else {
// populate estimator_aid_src_ev_yaw with delta heading innovations for logging
// use the change in yaw since the last measurement
const float measured_hdg_prev = getEulerYaw(_ev_sample_delayed_prev.quat);
// calculate the change in yaw since the last measurement
const float ev_delta_yaw = wrap_pi(measured_hdg - measured_hdg_prev);
_aid_src_ev_yaw.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _yaw_pred_prev - ev_delta_yaw);
}
}
bool ev_reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter);
// determine if we should use the horizontal position observations
bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (_ev_sample_delayed.quality >= _params.ev_quality_minimum);
const bool starting_conditions_passing = quality_sufficient
&& ((_ev_sample_delayed.time_us - _ev_sample_delayed_prev.time_us) < EV_MAX_INTERVAL)
&& ((_params.ev_quality_minimum <= 0) || (_ev_sample_delayed_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
&& ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
// EV velocity
controlEvVelFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
// EV height
controlEvHeightFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
// record observation and estimate for use next time
_ev_sample_delayed_prev = _ev_sample_delayed;
_hpos_pred_prev = _state.pos.xy();
_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw || _control_status.flags.ev_hgt)
&& !isRecent(_ev_sample_delayed.time_us, (uint64_t)_params.reset_timeout_max)) {
// Turn off EV fusion mode if no data has been received
stopEvFusion();
_warning_events.flags.vision_data_stopped = true;
ECL_WARN("vision data stopped");
}
}
void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
{
if (!(_params.gnss_ctrl & GnssCtrl::YAW)

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

@ -48,6 +48,7 @@
#include "EKFGSF_yaw.h"
#include "bias_estimator.hpp"
#include "height_bias_estimator.hpp"
#include "position_bias_estimator.hpp"
#include <uORB/topics/estimator_aid_source1d.h>
#include <uORB/topics/estimator_aid_source2d.h>
@ -379,9 +380,6 @@ public:
// return a bitmask integer that describes which state estimates can be used for flight control
void get_ekf_soln_status(uint16_t *status) const;
// return the quaternion defining the rotation from the External Vision to the EKF reference frame
matrix::Quatf getVisionAlignmentQuaternion() const { return Quatf(_R_ev_to_ekf); };
// use the latest IMU data at the current time horizon.
Quatf calculate_quaternion() const;
@ -410,6 +408,8 @@ public:
const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
const auto &aid_src_sideslip() const { return _aid_src_sideslip; }
@ -476,23 +476,18 @@ private:
bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised
// variables used when position data is being fused using a relative position odometry model
bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption
Vector2f _hpos_pred_prev{}; ///< previous value of NE position state used by odometry fusion (m)
float _yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m)
bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use
Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m)
bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
// booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
bool _rng_data_ready{false};
bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
bool _ev_data_ready{false}; ///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
uint64_t _time_prev_gps_us{0}; ///< time stamp of previous GPS data retrieved from the buffer (uSec)
uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
uint64_t _time_last_v_pos_aiding{0};
uint64_t _time_last_v_vel_aiding{0};
@ -508,7 +503,9 @@ private:
uint64_t _time_last_healthy_rng_data{0};
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
uint8_t _nb_ev_pos_reset_available{0};
uint8_t _nb_ev_vel_reset_available{0};
uint8_t _nb_ev_yaw_reset_available{0};
Vector3f _last_known_pos{}; ///< last known local position vector (m)
@ -708,7 +705,6 @@ private:
void resetHorizontalVelocityToZero();
void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
void resetHorizontalPositionToVision();
void resetHorizontalPositionToLastKnown();
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
@ -776,19 +772,12 @@ private:
// return true if successful
bool resetMagHeading();
// reset the heading using the external vision measurements
// return true if successful
bool resetYawToEv();
// Return the magnetic declination in radians to be used by the alignment and fusion processing
float getMagDeclination();
// modify output filter to match the the EKF state at the fusion time horizon
void alignOutputFilter();
// update the rotation matrix which transforms EV navigation frame measurements into NED
void calcExtVisRotMat();
bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
{
for (unsigned i = 0; i < 3; i++) {
@ -851,8 +840,16 @@ private:
// control fusion of external vision observations
void controlExternalVisionFusion();
void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src);
void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s& aid_src);
void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
void stopEvPosFusion();
void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s& aid_src);
void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src);
// control fusion of optical flow observations
void controlOpticalFlowFusion();
@ -1008,11 +1005,6 @@ private:
void startGpsYawFusion(const gpsSample &gps_sample);
void stopGpsYawFusion();
void startEvPosFusion();
void startEvYawFusion();
void stopEvFusion();
void stopEvPosFusion();
void stopEvVelFusion();
void stopEvYawFusion();
@ -1036,11 +1028,13 @@ private:
EKFGSF_yaw _yawEstimator{};
uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
uint8_t _position_sensor_ref{static_cast<uint8_t>(PositionSensor::GNSS)};
HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
void runYawEKFGSF();

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

@ -124,22 +124,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
_time_last_ver_vel_fuse = _imu_sample_delayed.time_us;
}
void Ekf::resetHorizontalPositionToVision()
{
_information_events.flags.reset_pos_to_vision = true;
ECL_INFO("reset position to ev position");
Vector3f _ev_pos = _ev_sample_delayed.pos;
if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
_ev_pos = _R_ev_to_ekf * _ev_sample_delayed.pos;
}
resetHorizontalPositionTo(Vector2f(_ev_pos), _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
// let the next odometry update know that the previous value of states cannot be used to calculate the change in position
_hpos_prev_available = false;
}
void Ekf::resetHorizontalPositionToLastKnown()
{
_information_events.flags.reset_pos_to_last_known = true;
@ -178,6 +162,9 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
_state_reset_status.reset_count.posNE++;
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
// Reset the timout timer
_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
}
@ -193,7 +180,6 @@ bool Ekf::isHeightResetRequired() const
return (continuous_bad_accel_hgt || hgt_fusion_timeout);
}
void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
{
const float old_vert_pos = _state.pos(2);
@ -275,8 +261,8 @@ void Ekf::alignOutputFilter()
bool Ekf::resetMagHeading()
{
// prevent a reset being performed more than once on the same frame
if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) {
return true;
if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
return false;
}
const Vector3f mag_init = _mag_lpf.getState();
@ -320,17 +306,6 @@ bool Ekf::resetMagHeading()
return false;
}
bool Ekf::resetYawToEv()
{
const float yaw_new = getEulerYaw(_ev_sample_delayed.quat);
const float yaw_new_variance = fmaxf(_ev_sample_delayed.orientation_var(2), sq(1.0e-2f));
resetQuatStateYaw(yaw_new, yaw_new_variance);
_R_ev_to_ekf.setIdentity();
return true;
}
// Return the magnetic declination in radians to be used by the alignment and fusion processing
float Ekf::getMagDeclination()
{
@ -755,6 +730,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
} else if (_control_status.flags.gps_yaw) {
mag = sqrtf(_aid_src_gnss_yaw.test_ratio);
} else {
mag = NAN;
}
@ -1092,7 +1068,7 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
void Ekf::stopMagFusion()
{
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
ECL_INFO("stopping mag fusion");
ECL_INFO("stopping all mag fusion");
stopMag3DFusion();
stopMagHdgFusion();
clearMagCov();
@ -1166,14 +1142,6 @@ void Ekf::updateGroundEffect()
}
}
// update the rotation matrix which rotates EV measurements into the EKF's navigation frame
void Ekf::calcExtVisRotMat()
{
// Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized());
_R_ev_to_ekf = Dcmf(q_error);
}
// Increase the yaw error variance of the quaternions
// Argument is additional yaw variance in rad**2
void Ekf::increaseQuatYawErrVariance(float yaw_variance)
@ -1320,52 +1288,6 @@ void Ekf::stopGpsYawFusion()
}
}
void Ekf::startEvPosFusion()
{
_control_status.flags.ev_pos = true;
resetHorizontalPositionToVision();
_information_events.flags.starting_vision_pos_fusion = true;
ECL_INFO("starting vision pos fusion");
}
void Ekf::startEvYawFusion()
{
// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
_control_status.flags.ev_yaw = true;
_control_status.flags.mag_dec = false;
stopMagHdgFusion();
stopMag3DFusion();
_information_events.flags.starting_vision_yaw_fusion = true;
ECL_INFO("starting vision yaw fusion");
}
void Ekf::stopEvFusion()
{
stopEvPosFusion();
stopEvVelFusion();
stopEvYawFusion();
stopEvHgtFusion();
}
void Ekf::stopEvPosFusion()
{
if (_control_status.flags.ev_pos) {
ECL_INFO("stopping EV pos fusion");
_control_status.flags.ev_pos = false;
resetEstimatorAidStatus(_aid_src_ev_pos);
}
}
void Ekf::stopEvYawFusion()
{
if (_control_status.flags.ev_yaw) {
ECL_INFO("stopping EV yaw fusion");
_control_status.flags.ev_yaw = false;
}
}
void Ekf::stopAuxVelFusion()
{
ECL_INFO("stopping aux vel fusion");

2
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -542,7 +542,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
}
if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl > 0)) {
if (_params.ev_ctrl > 0) {
max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
}

4
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -258,7 +258,6 @@ public:
const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
const extVisionSample &get_ev_sample_delayed() const { return _ev_sample_delayed; }
const bool &global_origin_valid() const { return _NED_origin_initialised; }
const MapProjection &global_origin() const { return _pos_ref; }
@ -305,8 +304,7 @@ protected:
sensor::SensorRangeFinder _range_sensor{};
airspeedSample _airspeed_sample_delayed{};
flowSample _flow_sample_delayed{};
extVisionSample _ev_sample_delayed{};
extVisionSample _ev_sample_delayed_prev{};
extVisionSample _ev_sample_prev{};
dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate)
RangeFinderConsistencyCheck _rng_consistency_check;

86
src/modules/ekf2/EKF/ev_control.cpp Normal file
View File

@ -0,0 +1,86 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ev_control.cpp
* Control functions for ekf external vision control
*/
#include "ekf.h"
void Ekf::controlExternalVisionFusion()
{
_ev_pos_b_est.predict(_dt_ekf_avg);
// Check for new external vision data
extVisionSample ev_sample;
if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &ev_sample)) {
bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);
// determine if we should use the horizontal position observations
bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (ev_sample.quality >= _params.ev_quality_minimum);
const bool starting_conditions_passing = quality_sufficient
&& ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL)
&& ((_params.ev_quality_minimum <= 0) || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
&& ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
controlEvHeightFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
if (quality_sufficient) {
_ev_sample_prev = ev_sample;
}
// record corresponding yaw state for future EV delta heading innovation (logging only)
_ev_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw
|| _control_status.flags.ev_hgt)
&& isTimedOut(_ev_sample_prev.time_us, 2 * EV_MAX_INTERVAL)) {
// Turn off EV fusion mode if no data has been received
stopEvPosFusion();
stopEvVelFusion();
stopEvYawFusion();
stopEvHgtFusion();
_warning_events.flags.vision_data_stopped = true;
ECL_WARN("vision data stopped");
}
}

10
src/modules/ekf2/EKF/ev_height_control.cpp
View File

@ -53,7 +53,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
// rotate measurement into correct earth frame if required
Vector3f pos{ev_sample.pos};
Matrix3f pos_cov{matrix::diag(ev_sample.posVar)};
Matrix3f pos_cov{matrix::diag(ev_sample.position_var)};
// rotate EV to the EKF reference frame unless we're operating entirely in vision frame
// TODO: only necessary if there's a roll/pitch offset between VIO and EKF
@ -65,7 +65,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
const Dcmf R_ev_to_ekf(q_error);
pos = R_ev_to_ekf * ev_sample.pos;
pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.posVar) * R_ev_to_ekf.transpose();
pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();
// increase minimum variance to include EV orientation variance
// TODO: do this properly
@ -98,9 +98,11 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
}
const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS)) && measurement_valid;
const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
&& measurement_valid;
const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing;
const bool starting_conditions_passing = common_starting_conditions_passing
&& continuing_conditions_passing;
if (_control_status.flags.ev_hgt) {
aid_src.fusion_enabled = true;

304
src/modules/ekf2/EKF/ev_pos_control.cpp Normal file
View File

@ -0,0 +1,304 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ev_pos_control.cpp
* Control functions for ekf external vision position fusion
*/
#include "ekf.h"
static constexpr const char *EV_AID_SRC_NAME = "EV position";
void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src)
{
const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
// determine if we should use EV position aiding
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
&& _control_status.flags.tilt_align
&& PX4_ISFINITE(ev_sample.pos(0))
&& PX4_ISFINITE(ev_sample.pos(1));
// correct position for offset relative to IMU
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
const bool bias_fusion_was_active = _ev_pos_b_est.fusionActive();
// rotate measurement into correct earth frame if required
Vector3f pos{NAN, NAN, NAN};
Matrix3f pos_cov{};
switch (ev_sample.pos_frame) {
case PositionFrame::LOCAL_FRAME_NED:
if (_control_status.flags.yaw_align) {
pos = ev_sample.pos - pos_offset_earth;
pos_cov = matrix::diag(ev_sample.position_var);
if (_control_status.flags.gps) {
_ev_pos_b_est.setFusionActive();
} else {
_ev_pos_b_est.setFusionInactive();
}
} else {
continuing_conditions_passing = false;
_ev_pos_b_est.setFusionInactive();
_ev_pos_b_est.reset();
}
break;
case PositionFrame::LOCAL_FRAME_FRD:
if (_control_status.flags.ev_yaw) {
// using EV frame
pos = ev_sample.pos - pos_offset_earth;
pos_cov = matrix::diag(ev_sample.position_var);
_ev_pos_b_est.setFusionInactive();
_ev_pos_b_est.reset();
} else {
// rotate EV to the EKF reference frame
const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
const Dcmf R_ev_to_ekf = Dcmf(q_error);
pos = R_ev_to_ekf * ev_sample.pos - pos_offset_earth;
pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();
// increase minimum variance to include EV orientation variance
// TODO: do this properly
const float orientation_var_max = ev_sample.orientation_var.max();
for (int i = 0; i < 2; i++) {
pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max);
}
if (_control_status.flags.gps) {
_ev_pos_b_est.setFusionActive();
} else {
_ev_pos_b_est.setFusionInactive();
}
}
break;
default:
continuing_conditions_passing = false;
_ev_pos_b_est.setFusionInactive();
_ev_pos_b_est.reset();
break;
}
// increase minimum variance if GPS active (position reference)
if (_control_status.flags.gps) {
for (int i = 0; i < 2; i++) {
pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
}
}
const Vector2f measurement{pos(0), pos(1)};
const Vector2f measurement_var{
math::max(pos_cov(0, 0), sq(_params.ev_pos_noise), sq(0.01f)),
math::max(pos_cov(1, 1), sq(_params.ev_pos_noise), sq(0.01f))
};
const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
// bias fusion activated (GPS activated)
if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) {
if (quality_sufficient) {
// reset the bias estimator
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
} else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) {
// otherwise stop EV position, when quality is good again it will restart with reset bias
stopEvPosFusion();
}
}
updateHorizontalPositionAidSrcStatus(ev_sample.time_us,
measurement - _ev_pos_b_est.getBias(), // observation
measurement_var + _ev_pos_b_est.getBiasVar(), // observation variance
math::max(_params.ev_pos_innov_gate, 1.f), // innovation gate
aid_src);
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
if (measurement_valid && quality_sufficient) {
_ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1))));
_ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO
_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(P(7, 7), P(8, 8)));
}
if (!measurement_valid) {
continuing_conditions_passing = false;
}
const bool starting_conditions_passing = common_starting_conditions_passing
&& continuing_conditions_passing;
if (_control_status.flags.ev_pos) {
aid_src.fusion_enabled = true;
if (continuing_conditions_passing) {
const bool bias_estimator_change = (bias_fusion_was_active != _ev_pos_b_est.fusionActive());
const bool reset = ev_reset || yaw_alignment_changed || bias_estimator_change;
updateEvPosFusion(measurement, measurement_var, quality_sufficient, reset, aid_src);
} else {
// Stop fusion but do not declare it faulty
ECL_WARN("stopping %s fusion, continuing conditions failing", EV_AID_SRC_NAME);
stopEvPosFusion();
}
} else {
if (starting_conditions_passing) {
startEvPosFusion(measurement, measurement_var, aid_src);
}
}
}
void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src)
{
// activate fusion
// TODO: (_params.position_sensor_ref == PositionSensor::EV)
if (_control_status.flags.gps) {
ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
_ev_pos_b_est.setFusionActive();
} else {
ECL_INFO("starting %s fusion, resetting state", EV_AID_SRC_NAME);
//_position_sensor_ref = PositionSensor::EV;
_information_events.flags.reset_pos_to_vision = true;
resetHorizontalPositionTo(measurement, measurement_var);
_ev_pos_b_est.reset();
}
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
_nb_ev_pos_reset_available = 5;
_information_events.flags.starting_vision_pos_fusion = true;
_control_status.flags.ev_pos = true;
}
void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src)
{
if (reset) {
if (quality_sufficient) {
if (!_control_status.flags.gps) {
ECL_INFO("reset to %s", EV_AID_SRC_NAME);
_information_events.flags.reset_pos_to_vision = true;
resetHorizontalPositionTo(measurement, measurement_var);
_ev_pos_b_est.reset();
} else {
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
}
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
} else {
// EV has reset, but quality isn't sufficient
// we have no choice but to stop EV and try to resume once quality is acceptable
stopEvPosFusion();
return;
}
} else if (quality_sufficient) {
fuseHorizontalPosition(aid_src);
} else {
aid_src.innovation_rejected = true;
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max); // 1 second
if (is_fusion_failing) {
bool pos_xy_fusion_failing = isTimedOut(_time_last_hor_pos_fuse, _params.no_aid_timeout_max);
if ((_nb_ev_pos_reset_available > 0) && quality_sufficient) {
// Data seems good, attempt a reset
ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME);
if (_control_status.flags.gps && !pos_xy_fusion_failing) {
// reset EV position bias
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
} else {
_information_events.flags.reset_pos_to_vision = true;
if (_control_status.flags.gps) {
resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
} else {
resetHorizontalPositionTo(measurement, measurement_var);
_ev_pos_b_est.reset();
}
}
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
if (_control_status.flags.in_air) {
_nb_ev_pos_reset_available--;
}
} else {
// A reset did not fix the issue but all the starting checks are not passing
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
ECL_WARN("stopping %s, fusion failing", EV_AID_SRC_NAME);
stopEvPosFusion();
}
}
}
void Ekf::stopEvPosFusion()
{
if (_control_status.flags.ev_pos) {
resetEstimatorAidStatus(_aid_src_ev_pos);
_control_status.flags.ev_pos = false;
}
}

3
src/modules/ekf2/EKF/ev_vel_control.cpp
View File

@ -134,7 +134,8 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
continuing_conditions_passing = false;
}
const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing
const bool starting_conditions_passing = common_starting_conditions_passing
&& continuing_conditions_passing
&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
if (_control_status.flags.ev_vel) {

186
src/modules/ekf2/EKF/ev_yaw_control.cpp Normal file
View File

@ -0,0 +1,186 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ev_yaw_control.cpp
* Control functions for ekf external vision yaw fusion
*/
#include "ekf.h"
void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src)
{
static constexpr const char *AID_SRC_NAME = "EV yaw";
resetEstimatorAidStatus(aid_src);
aid_src.timestamp_sample = ev_sample.time_us;
aid_src.observation = getEulerYaw(ev_sample.quat);
aid_src.observation_variance = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
// determine if we should use EV yaw aiding
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
&& _control_status.flags.tilt_align
&& !_inhibit_ev_yaw_use
&& PX4_ISFINITE(aid_src.observation)
&& PX4_ISFINITE(aid_src.observation_variance);
// if GPS enabled don't allow EV yaw if EV isn't NED
if (_control_status.flags.gps && _control_status.flags.yaw_align
&& (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED)
) {
continuing_conditions_passing = false;
// use delta yaw for innovation logging
aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _ev_yaw_pred_prev)
- wrap_pi(getEulerYaw(ev_sample.quat) - getEulerYaw(_ev_sample_prev.quat)));
}
const bool starting_conditions_passing = common_starting_conditions_passing
&& continuing_conditions_passing
&& isTimedOut(aid_src.time_last_fuse, (uint32_t)1e6);
if (_control_status.flags.ev_yaw) {
aid_src.fusion_enabled = true;
if (continuing_conditions_passing) {
if (ev_reset) {
if (quality_sufficient) {
ECL_INFO("reset to %s", AID_SRC_NAME);
//_information_events.flags.reset_yaw_to_vision = true; // TODO
resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
} else {
// EV has reset, but quality isn't sufficient
// we have no choice but to stop EV and try to resume once quality is acceptable
stopEvYawFusion();
return;
}
} else if (quality_sufficient) {
fuseYaw(aid_src.innovation, aid_src.observation_variance, aid_src);
} else {
aid_src.innovation_rejected = true;
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max);
if (is_fusion_failing) {
if ((_nb_ev_yaw_reset_available > 0) && quality_sufficient) {
// Data seems good, attempt a reset
//_information_events.flags.reset_yaw_to_vision = true; // TODO
ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
resetQuatStateYaw(aid_src.innovation, aid_src.observation_variance);
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
if (_control_status.flags.in_air) {
_nb_ev_yaw_reset_available--;
}
} else if (starting_conditions_passing) {
// Data seems good, but previous reset did not fix the issue
// something else must be wrong, declare the sensor faulty and stop the fusion
//_control_status.flags.ev_yaw_fault = true;
ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
stopEvYawFusion();
} else {
// A reset did not fix the issue but all the starting checks are not passing
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
stopEvYawFusion();
}
}
} else {
// Stop fusion but do not declare it faulty
ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
stopEvYawFusion();
}
} else {
if (starting_conditions_passing) {
// activate fusion
if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_NED) {
if (_control_status.flags.yaw_align) {
ECL_INFO("starting %s fusion", AID_SRC_NAME);
} else {
// reset yaw to EV and set yaw_align
ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
_control_status.flags.yaw_align = true;
}
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
_information_events.flags.starting_vision_yaw_fusion = true;
_control_status.flags.ev_yaw = true;
} else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) {
// turn on fusion of external vision yaw measurements and disable all other heading fusion
stopMagFusion();
stopGpsYawFusion();
stopGpsFusion();
ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
// reset yaw to EV
resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
_information_events.flags.starting_vision_yaw_fusion = true;
_control_status.flags.yaw_align = false;
_control_status.flags.ev_yaw = true;
}
if (_control_status.flags.ev_yaw) {
_nb_ev_yaw_reset_available = 5;
}
}
}
}
void Ekf::stopEvYawFusion()
{
if (_control_status.flags.ev_yaw) {
resetEstimatorAidStatus(_aid_src_ev_yaw);
_control_status.flags.ev_yaw = false;
}
}

1
src/modules/ekf2/EKF/fake_pos_control.cpp
View File

@ -95,7 +95,6 @@ void Ekf::controlFakePosFusion()
if (starting_conditions_passing) {
ECL_INFO("start fake position fusion");
_control_status.flags.fake_pos = true;
_fuse_hpos_as_odom = false; // TODO: needed?
resetFakePosFusion();
if (_control_status.flags.tilt_align) {

90
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -57,16 +57,18 @@ void Ekf::controlGpsFusion()
controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
// GNSS velocity
const Vector3f velocity{gps_sample.vel};
const float vel_var = sq(math::max(gps_sample.sacc, _params.gps_vel_noise));
const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
updateVelocityAidSrcStatus(gps_sample.time_us,
gps_sample.vel, // observation
velocity, // observation
vel_obs_var, // observation variance
math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate
_aid_src_gnss_vel);
_aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
// GNSS position
const Vector2f position{gps_sample.pos};
// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
float pos_noise = math::max(gps_sample.hacc, _params.gps_pos_noise);
@ -81,12 +83,34 @@ void Ekf::controlGpsFusion()
const float pos_var = sq(pos_noise);
const Vector2f pos_obs_var(pos_var, pos_var);
updateHorizontalPositionAidSrcStatus(gps_sample.time_us,
gps_sample.pos, // observation
position, // observation
pos_obs_var, // observation variance
math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
_aid_src_gnss_pos);
_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
// if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start
if (_control_status.flags.tilt_align && _NED_origin_initialised
&& gps_checks_passing && !gps_checks_failing) {
if (!_control_status.flags.yaw_align) {
if (_control_status.flags.ev_yaw && !_control_status.flags.yaw_align) {
// give mag a chance to start and yaw align if currently blocked by EV yaw
const bool mag_enabled = (_params.mag_fusion_type <= MagFuseType::MAG_3D);
const bool mag_available = (_mag_counter != 0);
if (mag_enabled && mag_available
&& !_control_status.flags.mag_field_disturbed
&& !_control_status.flags.mag_fault) {
stopEvYawFusion();
}
}
}
}
// Determine if we should use GPS aiding for velocity and horizontal position
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
const bool mandatory_conditions_passing = ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))
@ -134,12 +158,6 @@ void Ekf::controlGpsFusion()
stopGpsFusion();
_warning_events.flags.gps_quality_poor = true;
ECL_WARN("GPS quality poor - stopping use");
// TODO: move this to EV control logic
// Reset position state to external vision if we are going to use absolute values
if (_control_status.flags.ev_pos && !(_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS)) {
resetHorizontalPositionToVision();
}
}
} else { // mandatory conditions are not passing
@ -150,44 +168,48 @@ void Ekf::controlGpsFusion()
if (starting_conditions_passing) {
// Do not use external vision for yaw if using GPS because yaw needs to be
// defined relative to an NED reference frame
if (_control_status.flags.ev_yaw
|| _mag_inhibit_yaw_reset_req
|| _mag_yaw_reset_req) {
_mag_yaw_reset_req = true;
if (_control_status.flags.ev_yaw) {
// Stop the vision for yaw fusion and do not allow it to start again
stopEvYawFusion();
_inhibit_ev_yaw_use = true;
} else {
ECL_INFO("starting GPS fusion");
_information_events.flags.starting_gps_fusion = true;
// reset position
_information_events.flags.reset_pos_to_gps = true;
resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
// when already using another velocity source velocity reset is not necessary
if (!isHorizontalAidingActive()) {
_information_events.flags.reset_vel_to_gps = true;
resetVelocityTo(gps_sample.vel, vel_obs_var);
}
_control_status.flags.gps = true;
}
ECL_INFO("starting GPS fusion");
_information_events.flags.starting_gps_fusion = true;
// when already using another velocity source velocity reset is not necessary
if (!isHorizontalAidingActive()
|| isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|| !_control_status_prev.flags.yaw_align
) {
// reset velocity
_information_events.flags.reset_vel_to_gps = true;
resetVelocityTo(velocity, vel_obs_var);
_aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us;
}
// reset position
_information_events.flags.reset_pos_to_gps = true;
resetHorizontalPositionTo(position, pos_obs_var);
_aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us;
_control_status.flags.gps = true;
} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) {
// If no mag is used, align using the yaw estimator (if available)
if (resetYawToEKFGSF()) {
_information_events.flags.yaw_aligned_to_imu_gps = true;
ECL_INFO("Yaw aligned using IMU and GPS");
ECL_INFO("GPS yaw aligned using IMU, resetting vel and pos");
ECL_INFO("reset velocity and position to GPS");
// reset velocity
_information_events.flags.reset_vel_to_gps = true;
resetVelocityTo(velocity, vel_obs_var);
_aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us;
// reset position
_information_events.flags.reset_pos_to_gps = true;
resetVelocityTo(gps_sample.vel, vel_obs_var);
resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
resetHorizontalPositionTo(position, pos_obs_var);
_aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us;
}
}
}

31
src/modules/ekf2/EKF/mag_control.cpp
View File

@ -218,6 +218,12 @@ bool Ekf::canResetMagHeading() const
void Ekf::runInAirYawReset()
{
// prevent a reset being performed more than once on the same frame
if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us)
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
return;
}
if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) {
bool has_realigned_yaw = false;
@ -252,6 +258,8 @@ void Ekf::runInAirYawReset()
);
resetMagCov();
has_realigned_yaw = true;
}
if (!has_realigned_yaw && canResetMagHeading()) {
@ -296,14 +304,19 @@ void Ekf::check3DMagFusionSuitability()
void Ekf::checkYawAngleObservability()
{
// Check if there has been enough change in horizontal velocity to make yaw observable
// Apply hysteresis to check to avoid rapid toggling
_yaw_angle_observable = _yaw_angle_observable
? _accel_lpf_NE.norm() > _params.mag_acc_gate
: _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate;
if (_control_status.flags.gps) {
// Check if there has been enough change in horizontal velocity to make yaw observable
// Apply hysteresis to check to avoid rapid toggling
if (_yaw_angle_observable) {
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
_yaw_angle_observable = _yaw_angle_observable
&& (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here?
} else {
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f;
}
} else {
_yaw_angle_observable = false;
}
}
void Ekf::checkMagBiasObservability()
@ -366,9 +379,7 @@ bool Ekf::shouldInhibitMag() const
// has explicitly stopped magnetometer use.
const bool user_selected = (_params.mag_fusion_type == MagFuseType::INDOOR);
const bool heading_not_required_for_navigation = !_control_status.flags.gps
&& !_control_status.flags.ev_pos
&& !_control_status.flags.ev_vel;
const bool heading_not_required_for_navigation = !_control_status.flags.gps;
return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed;
}

113
src/modules/ekf2/EKF/position_bias_estimator.hpp Normal file
View File

@ -0,0 +1,113 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file position_bias_estimator.hpp
*/
#pragma once
#include "bias_estimator.hpp"
class PositionBiasEstimator
{
public:
PositionBiasEstimator(uint8_t sensor, const uint8_t &sensor_ref):
_sensor(sensor),
_sensor_ref(sensor_ref)
{}
virtual ~PositionBiasEstimator() = default;
bool fusionActive() const { return _is_sensor_fusion_active; }
void setFusionActive() { _is_sensor_fusion_active = true; }
void setFusionInactive() { _is_sensor_fusion_active = false; }
void predict(float dt)
{
if ((_sensor_ref != _sensor) && _is_sensor_fusion_active) {
_bias[0].predict(dt);
_bias[1].predict(dt);
}
}
void fuseBias(Vector2f bias, Vector2f bias_var)
{
if ((_sensor_ref != _sensor) && _is_sensor_fusion_active) {
_bias[0].fuseBias(bias(0), bias_var(0));
_bias[1].fuseBias(bias(1), bias_var(1));
}
}
void setBias(const Vector2f &bias)
{
_bias[0].setBias(bias(0));
_bias[1].setBias(bias(1));
}
void setProcessNoiseSpectralDensity(float nsd)
{
_bias[0].setProcessNoiseSpectralDensity(nsd);
_bias[1].setProcessNoiseSpectralDensity(nsd);
}
// void setBiasStdDev(float state_noise) { _state_var = state_noise * state_noise; }
// void setInnovGate(float gate_size) { _gate_size = gate_size; }
void setMaxStateNoise(const Vector2f &max_noise)
{
_bias[0].setMaxStateNoise(max_noise(0));
_bias[1].setMaxStateNoise(max_noise(1));
}
Vector2f getBias() const { return Vector2f(_bias[0].getBias(), _bias[1].getBias()); }
float getBias(int index) const { return _bias[index].getBias(); }
Vector2f getBiasVar() const { return Vector2f(_bias[0].getBiasVar(), _bias[1].getBiasVar()); }
float getBiasVar(int index) const { return _bias[index].getBiasVar(); }
const BiasEstimator::status &getStatus(int index) const { return _bias[index].getStatus(); }
void reset()
{
_bias[0].reset();
_bias[1].reset();
}
private:
BiasEstimator _bias[2] {};
const uint8_t _sensor;
const uint8_t &_sensor_ref;
bool _is_sensor_fusion_active{false}; // TODO: replace by const ref and remove setter when migrating _control_status.flags from union to bool
};

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

@ -207,23 +207,67 @@ bool EKF2::multi_init(int imu, int mag)
{
// advertise all topics to ensure consistent uORB instance numbering
_ekf2_timestamps_pub.advertise();
_estimator_baro_bias_pub.advertise();
_estimator_ev_hgt_bias_pub.advertise();
_estimator_event_flags_pub.advertise();
_estimator_gnss_hgt_bias_pub.advertise();
_estimator_gps_status_pub.advertise();
_estimator_innovation_test_ratios_pub.advertise();
_estimator_innovation_variances_pub.advertise();
_estimator_innovations_pub.advertise();
_estimator_optical_flow_vel_pub.advertise();
_estimator_rng_hgt_bias_pub.advertise();
_estimator_sensor_bias_pub.advertise();
_estimator_states_pub.advertise();
_estimator_status_flags_pub.advertise();
_estimator_status_pub.advertise();
_estimator_visual_odometry_aligned_pub.advertise();
_yaw_est_pub.advertise();
// baro advertise
if (_param_ekf2_baro_ctrl.get()) {
_estimator_aid_src_baro_hgt_pub.advertise();
_estimator_baro_bias_pub.advertise();
}
// EV advertise
if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VPOS)) {
_estimator_aid_src_ev_hgt_pub.advertise();
_estimator_ev_pos_bias_pub.advertise();
}
if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::HPOS)) {
_estimator_aid_src_ev_pos_pub.advertise();
_estimator_ev_pos_bias_pub.advertise();
}
if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VEL)) {
_estimator_aid_src_ev_vel_pub.advertise();
}
if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::YAW)) {
_estimator_aid_src_ev_yaw_pub.advertise();
}
// GNSS advertise
if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VPOS)) {
_estimator_aid_src_gnss_hgt_pub.advertise();
_estimator_gnss_hgt_bias_pub.advertise();
}
if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::HPOS)) {
_estimator_aid_src_gnss_pos_pub.advertise();
_estimator_gps_status_pub.advertise();
}
if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VEL)) {
_estimator_aid_src_gnss_vel_pub.advertise();
}
if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::YAW)) {
_estimator_aid_src_gnss_yaw_pub.advertise();
}
// RNG advertise
if (_param_ekf2_rng_ctrl.get()) {
_estimator_aid_src_rng_hgt_pub.advertise();
_estimator_rng_hgt_bias_pub.advertise();
}
_attitude_pub.advertise();
_local_position_pub.advertise();
_global_position_pub.advertise();
@ -545,15 +589,13 @@ void EKF2::Run()
UpdateAirspeedSample(ekf2_timestamps);
UpdateAuxVelSample(ekf2_timestamps);
UpdateBaroSample(ekf2_timestamps);
UpdateExtVisionSample(ekf2_timestamps);
UpdateFlowSample(ekf2_timestamps);
UpdateGpsSample(ekf2_timestamps);
UpdateMagSample(ekf2_timestamps);
UpdateRangeSample(ekf2_timestamps);
UpdateSystemFlagsSample(ekf2_timestamps);
vehicle_odometry_s ev_odom;
const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom);
// run the EKF update and output
const hrt_abstime ekf_update_start = hrt_absolute_time();
@ -569,7 +611,7 @@ void EKF2::Run()
PublishBaroBias(now);
PublishGnssHgtBias(now);
PublishRngHgtBias(now);
PublishEvHgtBias(now);
PublishEvPosBias(now);
PublishEventFlags(now);
PublishGpsStatus(now);
PublishInnovations(now);
@ -593,11 +635,6 @@ void EKF2::Run()
perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
}
// publish external visual odometry after fixed frame alignment if new odometry is received
if (new_ev_odom) {
PublishOdometryAligned(now, ev_odom);
}
// publish ekf2_timestamps
_ekf2_timestamps_pub.publish(ekf2_timestamps);
}
@ -666,12 +703,32 @@ void EKF2::VerifyParams()
}
// EV EKF2_AID_MASK -> EKF2_EV_CTRL
if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)) {
if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)
|| (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS)
|| (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW)
) {
_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::VEL));
_param_ekf2_ev_ctrl.commit();
// EKF2_EV_CTRL set VEL bit
if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)) {
_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::VEL));
}
// EKF2_EV_CTRL set HPOS/VPOS bits
if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS)) {
_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get()
| static_cast<int32_t>(EvCtrl::HPOS) | static_cast<int32_t>(EvCtrl::VPOS));
}
// EKF2_EV_CTRL set YAW bit
if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW)) {
_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::YAW));
}
_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL));
_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS));
_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW));
_param_ekf2_ev_ctrl.commit();
_param_ekf2_aid_mask.commit();
mavlink_log_critical(&_mavlink_log_pub, "EKF2 EV use EKF2_EV_CTRL instead of EKF2_AID_MASK\n");
@ -787,15 +844,35 @@ void EKF2::PublishRngHgtBias(const hrt_abstime &timestamp)
}
}
void EKF2::PublishEvHgtBias(const hrt_abstime &timestamp)
void EKF2::PublishEvPosBias(const hrt_abstime &timestamp)
{
if (_ekf.get_ev_sample_delayed().time_us != 0) {
const BiasEstimator::status &status = _ekf.getEvHgtBiasEstimatorStatus();
if (_ekf.aid_src_ev_hgt().timestamp_sample) {
if (fabsf(status.bias - _last_ev_hgt_bias_published) > 0.001f) {
_estimator_ev_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_ev_sample_delayed().time_us, timestamp));
estimator_bias3d_s bias{};
_last_ev_hgt_bias_published = status.bias;
// height
BiasEstimator::status bias_est_status[3];
bias_est_status[0] = _ekf.getEvPosBiasEstimatorStatus(0);
bias_est_status[1] = _ekf.getEvPosBiasEstimatorStatus(1);
bias_est_status[2] = _ekf.getEvHgtBiasEstimatorStatus();
for (int i = 0; i < 3; i++) {
bias.bias[i] = bias_est_status[i].bias;
bias.bias_var[i] = bias_est_status[i].bias_var;
bias.innov[i] = bias_est_status[i].innov;
bias.innov_var[i] = bias_est_status[i].innov_var;
bias.innov_test_ratio[i] = bias_est_status[i].innov_test_ratio;
}
const Vector3f bias_vec{bias.bias};
if ((bias_vec - _last_ev_bias_published).longerThan(0.01f)) {
bias.timestamp_sample = _ekf.aid_src_ev_hgt().timestamp_sample;
bias.timestamp = hrt_absolute_time();
_estimator_ev_pos_bias_pub.publish(bias);
_last_ev_bias_published = Vector3f(bias.bias);
}
}
}
@ -1210,43 +1287,6 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp)
_odometry_pub.publish(odom);
}
void EKF2::PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_odometry_s &ev_odom)
{
const Quatf quat_ev2ekf = _ekf.getVisionAlignmentQuaternion(); // rotates from EV to EKF navigation frame
const Dcmf ev_rot_mat(quat_ev2ekf);
vehicle_odometry_s aligned_ev_odom{ev_odom};
// Rotate external position and velocity into EKF navigation frame
const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.position);
aligned_pos.copyTo(aligned_ev_odom.position);
switch (ev_odom.velocity_frame) {
case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD: {
const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.velocity);
aligned_vel.copyTo(aligned_ev_odom.velocity);
break;
}
case vehicle_odometry_s::VELOCITY_FRAME_FRD: {
const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.velocity);
aligned_vel.copyTo(aligned_ev_odom.velocity);
break;
}
}
aligned_ev_odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
// Compute orientation in EKF navigation frame
Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q);
ev_quat_aligned.normalize();
ev_quat_aligned.copyTo(aligned_ev_odom.q);
aligned_ev_odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom);
}
void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
{
// estimator_sensor_bias
@ -1700,12 +1740,14 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
}
}
bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom)
bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF external vision sample
bool new_ev_odom = false;
const unsigned last_generation = _ev_odom_sub.get_last_generation();
vehicle_odometry_s ev_odom;
if (_ev_odom_sub.update(&ev_odom)) {
if (_msg_missed_odometry_perf == nullptr) {
_msg_missed_odometry_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_visual_odometry messages missed");
@ -1764,38 +1806,38 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
}
// check for valid position data
if (Vector3f(ev_odom.position).isAllFinite()) {
bool position_valid = true;
const Vector3f ev_odom_pos(ev_odom.position);
const Vector3f ev_odom_pos_var(ev_odom.position_variance);
// switch (ev_odom.pose_frame) {
// case vehicle_odometry_s::POSE_FRAME_NED:
// ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
// break;
if (ev_odom_pos.isAllFinite()) {
bool position_frame_valid = false;
// case vehicle_odometry_s::POSE_FRAME_FRD:
// ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
// break;
switch (ev_odom.pose_frame) {
case vehicle_odometry_s::POSE_FRAME_NED:
ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
position_frame_valid = true;
break;
// default:
// position_valid = false;
// break;
// }
case vehicle_odometry_s::POSE_FRAME_FRD:
ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
position_frame_valid = true;
break;
}
if (position_valid) {
ev_data.pos(0) = ev_odom.position[0];
ev_data.pos(1) = ev_odom.position[1];
ev_data.pos(2) = ev_odom.position[2];
if (position_frame_valid) {
ev_data.pos = ev_odom_pos;
const float evp_noise_var = sq(_param_ekf2_evp_noise.get());
// position measurement error from ev_data or parameters
if (!_param_ekf2_ev_noise_md.get() && Vector3f(ev_odom.position_variance).isAllFinite()) {
ev_data.posVar(0) = fmaxf(evp_noise_var, ev_odom.position_variance[0]);
ev_data.posVar(1) = fmaxf(evp_noise_var, ev_odom.position_variance[1]);
ev_data.posVar(2) = fmaxf(evp_noise_var, ev_odom.position_variance[2]);
if ((_param_ekf2_ev_noise_md.get() == 0) && ev_odom_pos_var.isAllFinite()) {
ev_data.position_var(0) = fmaxf(evp_noise_var, ev_odom_pos_var(0));
ev_data.position_var(1) = fmaxf(evp_noise_var, ev_odom_pos_var(1));
ev_data.position_var(2) = fmaxf(evp_noise_var, ev_odom_pos_var(2));
} else {
ev_data.posVar.setAll(evp_noise_var);
ev_data.position_var.setAll(evp_noise_var);
}
new_ev_odom = true;

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

@ -69,6 +69,7 @@
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/estimator_bias.h>
#include <uORB/topics/estimator_bias3d.h>
#include <uORB/topics/estimator_event_flags.h>
#include <uORB/topics/estimator_gps_status.h>
#include <uORB/topics/estimator_innovations.h>
@ -142,7 +143,7 @@ private:
void PublishBaroBias(const hrt_abstime &timestamp);
void PublishGnssHgtBias(const hrt_abstime &timestamp);
void PublishRngHgtBias(const hrt_abstime &timestamp);
void PublishEvHgtBias(const hrt_abstime &timestamp);
void PublishEvPosBias(const hrt_abstime &timestamp);
estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us,
uint64_t timestamp, uint32_t device_id = 0);
void PublishEventFlags(const hrt_abstime &timestamp);
@ -165,7 +166,7 @@ private:
void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom);
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
@ -291,7 +292,7 @@ private:
float _last_baro_bias_published{};
float _last_gnss_hgt_bias_published{};
float _last_rng_hgt_bias_published{};
float _last_ev_hgt_bias_published{};
matrix::Vector3f _last_ev_bias_published{};
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
hrt_abstime _airspeed_validated_timestamp_last{0};
@ -340,7 +341,7 @@ private:
uORB::PublicationMulti<estimator_bias_s> _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
uORB::PublicationMulti<estimator_bias_s> _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)};
uORB::PublicationMulti<estimator_bias_s> _estimator_rng_hgt_bias_pub{ORB_ID(estimator_rng_hgt_bias)};
uORB::PublicationMulti<estimator_bias_s> _estimator_ev_hgt_bias_pub{ORB_ID(estimator_ev_hgt_bias)};
uORB::PublicationMulti<estimator_bias3d_s> _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)};
uORB::PublicationMultiData<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
uORB::PublicationMulti<estimator_gps_status_s> _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
@ -350,7 +351,6 @@ private:
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationMulti<estimator_status_flags_s> _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::PublicationMulti<vehicle_odometry_s> _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};

14
src/modules/ekf2/ekf2_params.c
View File

@ -603,10 +603,10 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
* 0 : Deprecated, use EKF2_GPS_CTRL instead
* 1 : Set to true to use optical flow data if available
* 2 : Set to true to inhibit IMU delta velocity bias estimation
* 3 : Set to true to enable vision position fusion
* 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true.
* 3 : Deprecated, use EKF2_EV_CTRL instead
* 4 : Deprecated, use EKF2_EV_CTRL instead
* 5 : Set to true to enable multi-rotor drag specific force fusion
* 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used
* 6 : Deprecated, use EKF2_EV_CTRL instead
* 7 : Deprecated, use EKF2_GPS_CTRL instead
* 3 : Deprecated, use EKF2_EV_CTRL instead
*
@ -616,10 +616,10 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
* @bit 0 unused
* @bit 1 use optical flow
* @bit 2 inhibit IMU bias estimation
* @bit 3 vision position fusion
* @bit 4 vision yaw fusion
* @bit 3 unused
* @bit 4 unused
* @bit 5 multi-rotor drag fusion
* @bit 6 rotate external vision
* @bit 6 unused
* @bit 7 unused
* @bit 8 unused
* @reboot_required true
@ -841,7 +841,7 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f);
* @unit rad
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f);
PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f);
/**
* Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum

18
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
View File

@ -137,12 +137,12 @@ void EkfWrapper::setFlowOffset(const Vector3f &offset)
void EkfWrapper::enableExternalVisionPositionFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_POS;
_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::HPOS);
}
void EkfWrapper::disableExternalVisionPositionFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_POS;
_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::HPOS);
}
bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
@ -167,12 +167,12 @@ bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
void EkfWrapper::enableExternalVisionHeadingFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_YAW;
_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::YAW);
}
void EkfWrapper::disableExternalVisionHeadingFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_YAW;
_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::YAW);
}
bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
@ -180,16 +180,6 @@ bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
return _ekf->control_status_flags().ev_yaw;
}
void EkfWrapper::enableExternalVisionAlignment()
{
_ekf_params->fusion_mode |= SensorFusionMask::ROTATE_EXT_VIS;
}
void EkfWrapper::disableExternalVisionAlignment()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::ROTATE_EXT_VIS;
}
bool EkfWrapper::isIntendingMagHeadingFusion() const
{
return _ekf->control_status_flags().mag_hdg;

3
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
View File

@ -98,9 +98,6 @@ public:
bool isIntendingMag3DFusion() const;
void setMagFuseTypeNone();
void enableExternalVisionAlignment();
void disableExternalVisionAlignment();
bool isWindVelocityEstimated() const;
void enableTerrainRngFusion();

20
src/modules/ekf2/test/sensor_simulator/vio.cpp
View File

@ -8,6 +8,7 @@ namespace sensor
Vio::Vio(std::shared_ptr<Ekf> ekf): Sensor(ekf)
{
_vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
_vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
}
Vio::~Vio()
@ -32,7 +33,7 @@ void Vio::setVelocityVariance(const Vector3f &velVar)
void Vio::setPositionVariance(const Vector3f &posVar)
{
_vio_data.posVar = posVar;
_vio_data.position_var = posVar;
}
void Vio::setAngularVariance(float angVar)
@ -70,16 +71,27 @@ void Vio::setVelocityFrameToLocalNED()
_vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
}
void Vio::setPositionFrameToLocalNED()
{
_vio_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
}
void Vio::setPositionFrameToLocalFRD()
{
_vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
}
extVisionSample Vio::dataAtRest()
{
extVisionSample vio_data;
vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f};;
vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f};
vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};
vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.position_var = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.velocity_var = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.orientation_var(2) = 0.05f;
vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
return vio_data;
}

3
src/modules/ekf2/test/sensor_simulator/vio.h
View File

@ -63,6 +63,9 @@ public:
void setVelocityFrameToLocalFRD();
void setVelocityFrameToBody();
void setPositionFrameToLocalNED();
void setPositionFrameToLocalFRD();
extVisionSample dataAtRest();
private:

12
src/modules/ekf2/test/test_EKF_externalVision.cpp
View File

@ -78,6 +78,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
{
_sensor_simulator.runSeconds(_tilt_align_time); // Let the tilt align
_ekf_wrapper.enableExternalVisionPositionFusion();
_sensor_simulator._vio.setPositionFrameToLocalNED();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(2);
@ -147,7 +148,6 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
// WHEN: Vision frame is rotate +90°. The reported heading is -90°
Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, math::radians(-90.0f)));
_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
_ekf_wrapper.enableExternalVisionAlignment();
const Vector3f simulated_velocity_in_vision_frame(0.3f, -1.0f, 0.4f);
const Vector3f simulated_velocity_in_ekf_frame =
@ -179,6 +179,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
const Vector3f simulated_position(8.3f, -1.0f, 0.0f);
_sensor_simulator._vio.setPosition(simulated_position);
_sensor_simulator._vio.setPositionFrameToLocalNED();
_ekf_wrapper.enableExternalVisionPositionFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runMicroseconds(2e5);
@ -197,7 +198,6 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
// WHEN: Vision frame is rotate +90°. The reported heading is -90°
Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, math::radians(-90.0f)));
_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
_ekf_wrapper.enableExternalVisionAlignment();
const Vector3f simulated_position_in_vision_frame(8.3f, -1.0f, 0.0f);
const Vector3f simulated_position_in_ekf_frame =
@ -236,7 +236,6 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
// WHEN: Vision frame is rotate +90°. The reported heading is -90°
Quatf externalVisionFrameOffset(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
_sensor_simulator._vio.setOrientation(externalVisionFrameOffset.inversed());
_ekf_wrapper.enableExternalVisionAlignment();
// Simulate high uncertainty on vision x axis which is in this case
// the y EKF frame axis
@ -254,9 +253,9 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
EXPECT_TRUE(velVar_new(1) > velVar_new(0));
// THEN: the frame offset should be estimated correctly
Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
estimatedExternalVisionFrameOffset.canonical()));
// Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
// EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
// estimatedExternalVisionFrameOffset.canonical()));
}
TEST_F(EkfExternalVisionTest, velocityFrameBody)
@ -340,7 +339,6 @@ TEST_F(EkfExternalVisionTest, positionFrameLocal)
// WHEN: using EV yaw fusion and rotate EV is set
Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, 0.f));
_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
_ekf_wrapper.enableExternalVisionAlignment(); // ROTATE_EV
_ekf_wrapper.enableExternalVisionHeadingFusion(); // EV_YAW
// AND WHEN: using EV position aiding

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

@ -149,7 +149,7 @@ void LoggedTopics::add_default_topics()
add_topic("estimator_baro_bias", 500);
add_topic("estimator_gnss_hgt_bias", 500);
add_topic("estimator_rng_hgt_bias", 500);
add_topic("estimator_ev_hgt_bias", 500);
add_topic("estimator_ev_pos_bias", 500);
add_topic("estimator_event_flags", 0);
add_topic("estimator_gps_status", 1000);
add_topic("estimator_innovation_test_ratios", 500);
@ -160,13 +160,12 @@ void LoggedTopics::add_default_topics()
add_topic("estimator_states", 1000);
add_topic("estimator_status", 200);
add_topic("estimator_status_flags", 0);
add_topic("estimator_visual_odometry_aligned", 200);
add_topic("yaw_estimator_status", 1000);
add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_ev_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_ev_pos_bias", 500, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES);
@ -177,7 +176,6 @@ void LoggedTopics::add_default_topics()
add_optional_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_airspeed", 100, MAX_ESTIMATOR_INSTANCES);
@ -248,7 +246,7 @@ void LoggedTopics::add_default_topics()
add_topic("estimator_baro_bias");
add_topic("estimator_gnss_hgt_bias");
add_topic("estimator_rng_hgt_bias");
add_topic("estimator_ev_hgt_bias");
add_topic("estimator_ev_pos_bias");
add_topic("estimator_event_flags");
add_topic("estimator_gps_status");
add_topic("estimator_innovation_test_ratios");
@ -259,7 +257,6 @@ void LoggedTopics::add_default_topics()
add_topic("estimator_states");
add_topic("estimator_status");
add_topic("estimator_status_flags");
add_topic("estimator_visual_odometry_aligned");
add_topic("vehicle_attitude");
add_topic("vehicle_global_position");
add_topic("vehicle_local_position");
@ -281,7 +278,6 @@ void LoggedTopics::add_default_topics()
add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES);
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_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES);

2
src/modules/mavlink/mavlink_main.h
View File

@ -502,7 +502,6 @@ public:
bool hash_check_enabled() const { return _param_mav_hash_chk_en.get(); }
bool forward_heartbeats_enabled() const { return _param_mav_hb_forw_en.get(); }
bool odometry_loopback_enabled() const { return _param_mav_odom_lp.get(); }
bool failure_injection_enabled() const { return _param_sys_failure_injection_enabled.get(); }
@ -675,7 +674,6 @@ private:
(ParamBool<px4::params::MAV_FWDEXTSP>) _param_mav_fwdextsp,
(ParamBool<px4::params::MAV_HASH_CHK_EN>) _param_mav_hash_chk_en,
(ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en,
(ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp,
(ParamInt<px4::params::MAV_RADIO_TOUT>) _param_mav_radio_timeout,
(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl,
(ParamBool<px4::params::SYS_FAILURE_EN>) _param_sys_failure_injection_enabled

11
src/modules/mavlink/mavlink_params.c
View File

@ -142,17 +142,6 @@ PARAM_DEFINE_INT32(MAV_HASH_CHK_EN, 1);
*/
PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);
/**
* Activate ODOMETRY loopback.
*
* If set, it gets the data from 'vehicle_visual_odometry' instead of 'vehicle_odometry'
* serving as a loopback of the received ODOMETRY messages on the Mavlink receiver.
*
* @boolean
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_ODOM_LP, 0);
/**
* Timeout in seconds for the RADIO_STATUS reports coming in
*

32
src/modules/mavlink/streams/ODOMETRY.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -49,42 +49,20 @@ public:
unsigned get_size() override
{
if (_mavlink->odometry_loopback_enabled()) {
return _vodom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
} else {
return _odom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
return _vehicle_odometry_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _odom_sub{ORB_ID(vehicle_odometry)};
uORB::Subscription _vodom_sub{ORB_ID(vehicle_visual_odometry)};
uORB::Subscription _vehicle_odometry_sub{ORB_ID(vehicle_odometry)};
bool send() override
{
vehicle_odometry_s odom;
// check if it is to send visual odometry loopback or not
bool odom_updated = false;
mavlink_odometry_t msg{};
if (_mavlink->odometry_loopback_enabled()) {
odom_updated = _vodom_sub.update(&odom);
// source: external vision system
msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION;
} else {
odom_updated = _odom_sub.update(&odom);
// source: PX4 estimator
msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT;
}
if (odom_updated) {
if (_vehicle_odometry_sub.update(&odom)) {
mavlink_odometry_t msg{};
msg.time_usec = odom.timestamp_sample;
// set the frame_id according to the local frame of the data