fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 15:53:36 +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
34 changed files with 1019 additions and 553 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+17 -23
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();