From fc65939f0e855f8cc791b24f70831a67da1af9ed Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 Jul 2018 02:18:30 +1000 Subject: [PATCH] ekf2: Add support for use of multiple GPS receivers (#9765) --- msg/CMakeLists.txt | 3 +- msg/ekf_gps_position.msg | 16 + src/modules/ekf2/ekf2_main.cpp | 692 +++++++++++++++++++++++++++++++-- src/modules/ekf2/ekf2_params.c | 31 ++ src/modules/logger/logger.cpp | 1 + 5 files changed, 711 insertions(+), 32 deletions(-) create mode 100644 msg/ekf_gps_position.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 5632214188..45997df367 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -52,6 +52,7 @@ set(msg_files distance_sensor.msg ekf2_innovations.msg ekf2_timestamps.msg + ekf_gps_position.msg esc_report.msg esc_status.msg estimator_status.msg @@ -127,9 +128,9 @@ set(msg_files vehicle_magnetometer.msg vehicle_rates_setpoint.msg vehicle_roi.msg - vehicle_trajectory_waypoint.msg vehicle_status.msg vehicle_status_flags.msg + vehicle_trajectory_waypoint.msg vtol_vehicle_status.msg wind_estimate.msg vehicle_constraints.msg diff --git a/msg/ekf_gps_position.msg b/msg/ekf_gps_position.msg new file mode 100644 index 0000000000..1c0a3a4ec9 --- /dev/null +++ b/msg/ekf_gps_position.msg @@ -0,0 +1,16 @@ +# EKF blended position in WGS84 coordinates. +int32 lat # Latitude in 1E-7 degrees +int32 lon # Longitude in 1E-7 degrees +int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) +int32 alt_ellipsoid # Altitude in 1E-3 meters bove Ellipsoid, (millimetres) +float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) +uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. +float32 eph # GPS horizontal position accuracy (metres) +float32 epv # GPS vertical position accuracy (metres) +float32 vel_m_s # GPS ground speed, (metres/sec) +float32 vel_n_m_s # GPS North velocity, (metres/sec) +float32 vel_e_m_s # GPS East velocity, (metres/sec) +float32 vel_d_m_s # GPS Down velocity, (metres/sec) +bool vel_ned_valid # True if NED velocity is valid +uint8 satellites_used # Number of satellites used +uint8 selected # GPS selection: 0: GPS1, 1: GPS2. 2: GPS1+GPS2 blend diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 42ae8d73dc..deab06f862 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -55,22 +55,32 @@ #include #include #include +#include #include +#include #include #include #include #include #include +#include #include #include #include #include #include +#include #include #include -#include -#include -#include + +// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm +#define BLEND_MASK_USE_SPD_ACC 1 +#define BLEND_MASK_USE_HPOS_ACC 2 +#define BLEND_MASK_USE_VPOS_ACC 4 + +// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution +#define GPS_MAX_RECEIVERS 2 +#define GPS_BLENDED_INSTANCE 2 using math::constrain; using namespace time_literals; @@ -115,6 +125,38 @@ private: const Vector3f get_vel_body_wind(); + /* + * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical receiver solutions + * with weights are calculated in calc_gps_blend_weights(). This internal state cannot be used directly by estimators + * because if physical receivers have significant position differences, variation in receiver estimated accuracy will + * cause undesirable variation in the position soution. + */ + bool calc_gps_blend_weights(); + + /* + * Calculate internal states used to blend GPS data from multiple receivers using weightings calculated + * by calc_blend_weights() + * States are written to _gps_state and _gps_blended_state class variables + */ + void update_gps_blend_states(); + + /* + * The location in _gps_blended_state will move around as the relative accuracy changes. + * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is + * calculated. + */ + void update_gps_offsets(); + + /* + * Apply the steady state physical receiver offsets calculated by update_gps_offsets(). + */ + void apply_gps_offsets(); + + /* + Calculate GPS output that is a blend of the offset corrected physical receiver data + */ + void calc_gps_blend_output(); + bool _replay_mode = false; ///< true when we use replay data from a log // time slip monitoring @@ -170,11 +212,22 @@ private: const float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; ///< preflight velocity innovation spike limit (m/sec) const float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; ///< preflight position innovation spike limit (m) + // GPS blending and switching + gps_message _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS + gps_message _gps_blended_state{}; ///< internal state data for the blended GPS + gps_message _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS + Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS] = {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) + float _hgt_offset_mm[GPS_MAX_RECEIVERS] = {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm) + Vector3f _blended_antenna_offset = {}; ///< blended antenna offset + float _blend_weights[GPS_MAX_RECEIVERS] = {}; ///< blend weight for each GPS. The blend weights must sum to 1.0 across all instances. + uint64_t _time_prev_us[GPS_MAX_RECEIVERS] = {}; ///< the previous value of time_us for that GPS instance - used to detect new data. + uint8_t _gps_best_index = 0; ///< index of the physical receiver with the lowest reported error + uint8_t _gps_select_index = 0; ///< 0 = GPS1, 1 = GPS2, 2 = blended + int _airdata_sub{-1}; int _airspeed_sub{-1}; int _ev_att_sub{-1}; int _ev_pos_sub{-1}; - int _gps_sub{-1}; int _landing_target_pose_sub{-1}; int _magnetometer_sub{-1}; int _optical_flow_sub{-1}; @@ -185,15 +238,20 @@ private: int _vehicle_land_detected_sub{-1}; // because we can have several distance sensor instances with different orientations - int _range_finder_subs[ORB_MULTI_MAX_INSTANCES]; + int _range_finder_subs[ORB_MULTI_MAX_INSTANCES] {}; int _range_finder_sub_index = -1; // index for downward-facing range finder subscription + // because we can have multiple GPS instances + int _gps_subs[ORB_MULTI_MAX_INSTANCES] {}; + int _gps_orb_instance{-1}; + orb_advert_t _att_pub{nullptr}; orb_advert_t _wind_pub{nullptr}; orb_advert_t _estimator_status_pub{nullptr}; orb_advert_t _estimator_innovations_pub{nullptr}; orb_advert_t _ekf2_timestamps_pub{nullptr}; orb_advert_t _sensor_bias_pub{nullptr}; + orb_advert_t _blended_gps_pub{nullptr}; uORB::Publication _vehicle_local_position_pub; uORB::Publication _vehicle_global_position_pub; @@ -401,7 +459,13 @@ private: (ParamFloat) _K_pstatic_coef_y, ///< static pressure position error coefficient along the Y body axis (ParamFloat) - _K_pstatic_coef_z ///< static pressure position error coefficient along the Z body axis + _K_pstatic_coef_z, ///< static pressure position error coefficient along the Z body axis + + // GPS blending + (ParamInt) + _gps_blend_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio + (ParamFloat) + _gps_blend_tau ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec) ) }; @@ -508,7 +572,6 @@ Ekf2::Ekf2(): _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude)); _ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); - _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _landing_target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose)); _magnetometer_sub = orb_subscribe(ORB_ID(vehicle_magnetometer)); _optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); @@ -519,6 +582,7 @@ Ekf2::Ekf2(): _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + _gps_subs[i] = orb_subscribe_multi(ORB_ID(vehicle_gps_position), i); _range_finder_subs[i] = orb_subscribe_multi(ORB_ID(distance_sensor), i); } @@ -535,7 +599,6 @@ Ekf2::~Ekf2() orb_unsubscribe(_airspeed_sub); orb_unsubscribe(_ev_att_sub); orb_unsubscribe(_ev_pos_sub); - orb_unsubscribe(_gps_sub); orb_unsubscribe(_landing_target_pose_sub); orb_unsubscribe(_magnetometer_sub); orb_unsubscribe(_optical_flow_sub); @@ -548,6 +611,8 @@ Ekf2::~Ekf2() for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { orb_unsubscribe(_range_finder_subs[i]); _range_finder_subs[i] = -1; + orb_unsubscribe(_gps_subs[i]); + _gps_subs[i] = -1; } } @@ -847,38 +912,131 @@ void Ekf2::run() } } - // read gps data if available - bool gps_updated = false; - orb_check(_gps_sub, &gps_updated); + // read gps1 data if available + bool gps1_updated = false; + orb_check(_gps_subs[0], &gps1_updated); - if (gps_updated) { + if (gps1_updated) { vehicle_gps_position_s gps; - if (orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps) == PX4_OK) { - struct gps_message gps_msg; - gps_msg.time_usec = gps.timestamp; - gps_msg.lat = gps.lat; - gps_msg.lon = gps.lon; - gps_msg.alt = gps.alt; - gps_msg.fix_type = gps.fix_type; - gps_msg.eph = gps.eph; - gps_msg.epv = gps.epv; - gps_msg.sacc = gps.s_variance_m_s; - gps_msg.vel_m_s = gps.vel_m_s; - gps_msg.vel_ned[0] = gps.vel_n_m_s; - gps_msg.vel_ned[1] = gps.vel_e_m_s; - gps_msg.vel_ned[2] = gps.vel_d_m_s; - gps_msg.vel_ned_valid = gps.vel_ned_valid; - gps_msg.nsats = gps.satellites_used; + if (orb_copy(ORB_ID(vehicle_gps_position), _gps_subs[0], &gps) == PX4_OK) { + _gps_state[0].time_usec = gps.timestamp; + _gps_state[0].lat = gps.lat; + _gps_state[0].lon = gps.lon; + _gps_state[0].alt = gps.alt; + _gps_state[0].fix_type = gps.fix_type; + _gps_state[0].eph = gps.eph; + _gps_state[0].epv = gps.epv; + _gps_state[0].sacc = gps.s_variance_m_s; + _gps_state[0].vel_m_s = gps.vel_m_s; + _gps_state[0].vel_ned[0] = gps.vel_n_m_s; + _gps_state[0].vel_ned[1] = gps.vel_e_m_s; + _gps_state[0].vel_ned[2] = gps.vel_d_m_s; + _gps_state[0].vel_ned_valid = gps.vel_ned_valid; + _gps_state[0].nsats = gps.satellites_used; //TODO: add gdop to gps topic - gps_msg.gdop = 0.0f; - - _ekf.setGpsData(gps.timestamp, &gps_msg); + _gps_state[0].gdop = 0.0f; ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } } + // check for second GPS receiver data + bool gps2_updated = false; + orb_check(_gps_subs[1], &gps2_updated); + + if (gps2_updated) { + vehicle_gps_position_s gps; + + if (orb_copy(ORB_ID(vehicle_gps_position), _gps_subs[1], &gps) == PX4_OK) { + _gps_state[1].time_usec = gps.timestamp; + _gps_state[1].lat = gps.lat; + _gps_state[1].lon = gps.lon; + _gps_state[1].alt = gps.alt; + _gps_state[1].fix_type = gps.fix_type; + _gps_state[1].eph = gps.eph; + _gps_state[1].epv = gps.epv; + _gps_state[1].sacc = gps.s_variance_m_s; + _gps_state[1].vel_m_s = gps.vel_m_s; + _gps_state[1].vel_ned[0] = gps.vel_n_m_s; + _gps_state[1].vel_ned[1] = gps.vel_e_m_s; + _gps_state[1].vel_ned[2] = gps.vel_d_m_s; + _gps_state[1].vel_ned_valid = gps.vel_ned_valid; + _gps_state[1].nsats = gps.satellites_used; + //TODO: add gdop to gps topic + _gps_state[1].gdop = 0.0f; + } + } + + if ((_gps_blend_mask.get() == 0) && gps1_updated) { + // When GPS blending is disabled we always use the first receiver instance + _ekf.setGpsData(_gps_state[0].time_usec, &_gps_state[0]); + + } else if ((_gps_blend_mask.get() > 0) && (gps1_updated || gps2_updated)) { + // blend dual receivers if available + + // calculate blending weights + if (calc_gps_blend_weights()) { + // With updated weights we can calculate a blended GPS solution and + // offsets for each physical receiver + update_gps_blend_states(); + update_gps_offsets(); + _gps_select_index = 2; + + } else { + // handle case where the blended states cannot be updated + if (_gps_state[0].fix_type > _gps_state[0].fix_type) { + // GPS 1 has the best fix status so use that + _gps_select_index = 0; + + } else if (_gps_state[1].fix_type > _gps_state[0].fix_type) { + // GPS 2 has the best fix status so use that + _gps_select_index = 1; + + } else if (_gps_select_index == 2) { + // use last receiver we received data from + if (gps1_updated) { + _gps_select_index = 0; + + } else if (gps2_updated) { + _gps_select_index = 1; + } + } + } + + // correct the physical receiver data for steady state offsets + apply_gps_offsets(); + + // calculate a blended output from the offset corrected receiver data + if (_gps_select_index == 2) { + calc_gps_blend_output(); + } + + // write selected GPS to EKF + _ekf.setGpsData(_gps_output[_gps_select_index].time_usec, &_gps_output[_gps_select_index]); + + // log blended solution as a third GPS instance + ekf_gps_position_s gps; + gps.timestamp = _gps_output[_gps_select_index].time_usec; + gps.lat = _gps_output[_gps_select_index].lat; + gps.lon = _gps_output[_gps_select_index].lon; + gps.alt = _gps_output[_gps_select_index].alt; + gps.fix_type = _gps_output[_gps_select_index].fix_type; + gps.eph = _gps_output[_gps_select_index].eph; + gps.epv = _gps_output[_gps_select_index].epv; + gps.s_variance_m_s = _gps_output[_gps_select_index].sacc; + gps.vel_m_s = _gps_output[_gps_select_index].vel_m_s; + gps.vel_n_m_s = _gps_output[_gps_select_index].vel_ned[0]; + gps.vel_e_m_s = _gps_output[_gps_select_index].vel_ned[1]; + gps.vel_d_m_s = _gps_output[_gps_select_index].vel_ned[2]; + gps.vel_ned_valid = _gps_output[_gps_select_index].vel_ned_valid; + gps.satellites_used = _gps_output[_gps_select_index].nsats; + gps.selected = _gps_select_index; + + // Publish to the EKF blended GPS topic + orb_publish_auto(ORB_ID(ekf_gps_position), &_blended_gps_pub, &gps, &_gps_orb_instance, ORB_PRIO_LOW); + } + bool airspeed_updated = false; orb_check(_airspeed_sub, &airspeed_updated); @@ -1544,6 +1702,478 @@ const Vector3f Ekf2::get_vel_body_wind() return R_to_body * v_wind_comp; } +/* + calculate the weightings used to blend GPS location and velocity data +*/ +bool Ekf2::calc_gps_blend_weights() +{ + // zero the blend weights + memset(&_blend_weights, 0, sizeof(_blend_weights)); + + // Use the oldest non-zero time, but if time difference is excessive, use newest to prevent a disconnected receiver from blocking updates + uint64_t max_us = 0; // newest non-zero system time of arrival of a GPS message + uint64_t min_us = -1; // oldest non-zero system time of arrival of a GPS message + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + // Find largest and smallest times + if (_gps_state[i].time_usec > max_us) { + max_us = _gps_state[i].time_usec; + } + + if ((_gps_state[i].time_usec < min_us) && (_gps_state[i].time_usec > 0)) { + min_us = _gps_state[i].time_usec; + } + } + + if ((max_us - min_us) < 300000) { + // data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated + _gps_blended_state.time_usec = min_us; + + } else { + // receiver data has timed out so fail out of blending + return false; + } + + // calculate the sum squared speed accuracy across all GPS sensors + float speed_accuracy_sum_sq = 0.0f; + + if (_gps_blend_mask.get() & BLEND_MASK_USE_SPD_ACC) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].sacc > 0.0f) { + speed_accuracy_sum_sq += _gps_state[i].sacc * _gps_state[i].sacc; + + } else { + // not all receivers support this metric so set it to zero and don't use it + speed_accuracy_sum_sq = 0.0f; + break; + } + } + } + + // calculate the sum squared horizontal position accuracy across all GPS sensors + float horizontal_accuracy_sum_sq = 0.0f; + + if (_gps_blend_mask.get() & BLEND_MASK_USE_HPOS_ACC) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph > 0.0f) { + horizontal_accuracy_sum_sq += _gps_state[i].eph * _gps_state[i].eph; + + } else { + // not all receivers support this metric so set it to zero and don't use it + horizontal_accuracy_sum_sq = 0.0f; + break; + } + } + } + + // calculate the sum squared vertical position accuracy across all GPS sensors + float vertical_accuracy_sum_sq = 0.0f; + + if (_gps_blend_mask.get() & BLEND_MASK_USE_VPOS_ACC) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv > 0.0f) { + vertical_accuracy_sum_sq += _gps_state[i].epv * _gps_state[i].epv; + + } else { + // not all receivers support this metric so set it to zero and don't use it + vertical_accuracy_sum_sq = 0.0f; + break; + } + } + } + + // Check if we can do blending using reported accuracy + bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f + || speed_accuracy_sum_sq > 0.0f); + + // if we can't do blending using reported accuracy, return false and hard switch logic will be used instead + if (!can_do_blending) { + return false; + } + + float sum_of_all_weights = 0.0f; + + // calculate a weighting using the reported speed accuracy + float spd_blend_weights[GPS_MAX_RECEIVERS] = {}; + + if (speed_accuracy_sum_sq > 0.0f && (_gps_blend_mask.get() & BLEND_MASK_USE_SPD_ACC)) { + // calculate the weights using the inverse of the variances + float sum_of_spd_weights = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].sacc >= 0.001f) { + spd_blend_weights[i] = 1.0f / (_gps_state[i].sacc * _gps_state[i].sacc); + sum_of_spd_weights += spd_blend_weights[i]; + } + } + + // normalise the weights + if (sum_of_spd_weights > 0.0f) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + spd_blend_weights[i] = spd_blend_weights[i] / sum_of_spd_weights; + } + + sum_of_all_weights += 1.0f; + } + } + + // calculate a weighting using the reported horizontal position + float hpos_blend_weights[GPS_MAX_RECEIVERS] = {}; + + if (horizontal_accuracy_sum_sq > 0.0f && (_gps_blend_mask.get() & BLEND_MASK_USE_HPOS_ACC)) { + // calculate the weights using the inverse of the variances + float sum_of_hpos_weights = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph >= 0.001f) { + hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (_gps_state[i].eph * _gps_state[i].eph); + sum_of_hpos_weights += hpos_blend_weights[i]; + } + } + + // normalise the weights + if (sum_of_hpos_weights > 0.0f) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights; + } + + sum_of_all_weights += 1.0f; + } + } + + // calculate a weighting using the reported vertical position accuracy + float vpos_blend_weights[GPS_MAX_RECEIVERS] = {}; + + if (vertical_accuracy_sum_sq > 0.0f && (_gps_blend_mask.get() & BLEND_MASK_USE_VPOS_ACC)) { + // calculate the weights using the inverse of the variances + float sum_of_vpos_weights = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv >= 0.001f) { + vpos_blend_weights[i] = vertical_accuracy_sum_sq / (_gps_state[i].epv * _gps_state[i].epv); + sum_of_vpos_weights += vpos_blend_weights[i]; + } + } + + // normalise the weights + if (sum_of_vpos_weights > 0.0f) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + vpos_blend_weights[i] = vpos_blend_weights[i] / sum_of_vpos_weights; + } + + sum_of_all_weights += 1.0f; + }; + } + + // calculate an overall weight + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + _blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights; + } + + return true; +} + +/* + * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical receiver solutions + * with weights are calculated in calc_gps_blend_weights(). This internal state cannot be used directly by estimators + * because if physical receivers have significant position differences, variation in receiver estimated accuracy will + * cause undesirable variation in the position soution. +*/ +void Ekf2::update_gps_blend_states() +{ + // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver. + _gps_blended_state.time_usec = 0; + _gps_blended_state.lat = 0; + _gps_blended_state.lon = 0; + _gps_blended_state.alt = 0; + _gps_blended_state.fix_type = 0; + _gps_blended_state.eph = FLT_MAX; + _gps_blended_state.epv = FLT_MAX; + _gps_blended_state.sacc = FLT_MAX; + _gps_blended_state.vel_m_s = 0.0f; + _gps_blended_state.vel_ned[0] = 0.0f; + _gps_blended_state.vel_ned[1] = 0.0f; + _gps_blended_state.vel_ned[2] = 0.0f; + _gps_blended_state.vel_ned_valid = true; + _gps_blended_state.nsats = 0; + _gps_blended_state.gdop = FLT_MAX; + + _blended_antenna_offset.zero(); + + // combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights() + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + // blend the timing data + _gps_blended_state.time_usec += (uint64_t)((double)_gps_state[i].time_usec * (double)_blend_weights[i]); + + // use the highest status + if (_gps_state[i].fix_type > _gps_blended_state.fix_type) { + _gps_blended_state.fix_type = _gps_state[i].fix_type; + } + + // calculate a blended average speed and velocity vector + _gps_blended_state.vel_m_s += _gps_state[i].vel_m_s * _blend_weights[i]; + _gps_blended_state.vel_ned[0] += _gps_state[i].vel_ned[0] * _blend_weights[i]; + _gps_blended_state.vel_ned[1] += _gps_state[i].vel_ned[1] * _blend_weights[i]; + _gps_blended_state.vel_ned[2] += _gps_state[i].vel_ned[2] * _blend_weights[i]; + + // Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers + // If any receiver contributing has an invalid velocity, then report blended velocity as invalid + if (_blend_weights[i] > 0.0f) { + + if (_gps_state[i].eph > 0.0f + && _gps_state[i].eph < _gps_blended_state.eph) { + _gps_blended_state.eph = _gps_state[i].eph; + } + + if (_gps_state[i].epv > 0.0f + && _gps_state[i].epv < _gps_blended_state.epv) { + _gps_blended_state.epv = _gps_state[i].epv; + } + + if (_gps_state[i].sacc > 0.0f + && _gps_state[i].sacc < _gps_blended_state.sacc) { + _gps_blended_state.sacc = _gps_state[i].sacc; + } + + if (_gps_state[i].gdop > 0 + && _gps_state[i].gdop < _gps_blended_state.gdop) { + _gps_blended_state.gdop = _gps_state[i].gdop; + } + + if (_gps_state[i].nsats > 0 + && _gps_state[i].nsats > _gps_blended_state.nsats) { + _gps_blended_state.nsats = _gps_state[i].nsats; + } + + if (!_gps_state[i].vel_ned_valid) { + _gps_blended_state.vel_ned_valid = false; + } + + } + + // TODO read parameters for individual GPS antenna positions and blend + // Vector3f temp_antenna_offset = _antenna_offset[i]; + // temp_antenna_offset *= _blend_weights[i]; + // _blended_antenna_offset += temp_antenna_offset; + + } + + /* + * Calculate the instantaneous weighted average location using available GPS instances and store in _gps_state. + * This is statistically the most likely location, but may not be stable enough for direct use by the EKF. + */ + + // Use the GPS with the highest weighting as the reference position + float best_weight = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_blend_weights[i] > best_weight) { + best_weight = _blend_weights[i]; + _gps_best_index = i; + _gps_blended_state.lat = _gps_state[i].lat; + _gps_blended_state.lon = _gps_state[i].lon; + _gps_blended_state.alt = _gps_state[i].alt; + } + } + + // Convert each GPS position to a local NEU offset relative to the reference position + Vector2f blended_NE_offset_m; + blended_NE_offset_m.zero(); + float blended_alt_offset_mm = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if ((_blend_weights[i] > 0.0f) && (i != _gps_best_index)) { + // calculate the horizontal offset + Vector2f horiz_offset{}; + get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7), + (_gps_blended_state.lon / 1.0e7), (_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + &horiz_offset(0), &horiz_offset(1)); + + // sum weighted offsets + blended_NE_offset_m += horiz_offset * _blend_weights[i]; + + // calculate vertical offset + float vert_offset = (float)(_gps_state[i].alt - _gps_blended_state.alt); + + // sum weighted offsets + blended_alt_offset_mm += vert_offset * _blend_weights[i]; + } + } + + // Add the sum of weighted offsets to the reference position to obtain the blended position + double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7; + double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7; + double lat_deg_res, lon_deg_res; + add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, + &lon_deg_res); + _gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res); + _gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res); + _gps_blended_state.alt += (int32_t)blended_alt_offset_mm; + +} + +/* + * The location in _gps_blended_state will move around as the relative accuracy changes. + * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is + * calculated. +*/ +void Ekf2::update_gps_offsets() +{ + + // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset + // Increase the filter time constant proportional to the inverse of the weighting + // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering + float alpha[GPS_MAX_RECEIVERS] = {}; + float omega_lpf = 1.0f / fmaxf(_gps_blend_tau.get(), 1.0f); + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].time_usec - _time_prev_us[i] > 0) { + // calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter + float min_alpha = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].time_usec - _time_prev_us[i]), + 0.0f, 1.0f); + + // scale the filter coefficient so that time constant is inversely proprtional to weighting + if (_blend_weights[i] > min_alpha) { + alpha[i] = min_alpha / _blend_weights[i]; + + } else { + alpha[i] = 1.0f; + } + + _time_prev_us[i] = _gps_state[i].time_usec; + } + } + + // Calculate a filtered position delta for each GPS relative to the blended solution state + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + Vector2f offset; + get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + (_gps_blended_state.lat / 1.0e7), (_gps_blended_state.lon / 1.0e7), &offset(0), &offset(1)); + _NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]); + _hgt_offset_mm[i] = (float)(_gps_blended_state.alt - _gps_state[i].alt) * alpha[i] + + _hgt_offset_mm[i] * (1.0f - alpha[i]); + } + + // calculate offset limits from the largest difference between receivers + Vector2f max_ne_offset{}; + float max_alt_offset = 0; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + for (uint8_t j = i; j < GPS_MAX_RECEIVERS; j++) { + if (i != j) { + Vector2f offset; + get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + (_gps_state[j].lat / 1.0e7), (_gps_state[j].lon / 1.0e7), &offset(0), &offset(1)); + max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0))); + max_ne_offset(1) = fmaxf(max_ne_offset(1), fabsf(offset(1))); + max_alt_offset = fmaxf(max_alt_offset, fabsf((float)(_gps_state[i].alt - _gps_state[j].alt))); + } + } + } + + // apply offset limits + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + _NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0)); + _NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1)); + _hgt_offset_mm[i] = constrain(_hgt_offset_mm[i], -max_alt_offset, max_alt_offset); + } + +} + + +/* + * Apply the steady state physical receiver offsets calculated by update_gps_offsets(). +*/ +void Ekf2::apply_gps_offsets() +{ + // calculate offset corrected output for each physical GPS. + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + // Add the sum of weighted offsets to the reference position to obtain the blended position + double lat_deg_now = (double)_gps_state[i].lat * 1.0e-7; + double lon_deg_now = (double)_gps_state[i].lon * 1.0e-7; + double lat_deg_res, lon_deg_res; + add_vector_to_global_position(lat_deg_now, lon_deg_now, _NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1), &lat_deg_res, + &lon_deg_res); + _gps_output[i].lat = (int32_t)(1.0E7 * lat_deg_res); + _gps_output[i].lon = (int32_t)(1.0E7 * lon_deg_res); + _gps_output[i].alt = _gps_state[i].alt + (int32_t)_hgt_offset_mm[i]; + + // other receiver data is used uncorrected + _gps_output[i].time_usec = _gps_state[i].time_usec; + _gps_output[i].fix_type = _gps_state[i].fix_type; + _gps_output[i].vel_m_s = _gps_state[i].vel_m_s; + _gps_output[i].vel_ned[0] = _gps_state[i].vel_ned[0]; + _gps_output[i].vel_ned[1] = _gps_state[i].vel_ned[1]; + _gps_output[i].vel_ned[2] = _gps_state[i].vel_ned[2]; + _gps_output[i].eph = _gps_state[i].eph; + _gps_output[i].epv = _gps_state[i].epv; + _gps_output[i].sacc = _gps_state[i].sacc; + _gps_output[i].gdop = _gps_state[i].gdop; + _gps_output[i].nsats = _gps_state[i].nsats; + _gps_output[i].vel_ned_valid = _gps_state[i].vel_ned_valid; + } +} + +/* + Calculate GPS output that is a blend of the offset corrected physical receiver data +*/ +void Ekf2::calc_gps_blend_output() +{ + // Convert each GPS position to a local NEU offset relative to the reference position + // which is defined as the positon of the blended solution calculated from non offset corrected data + Vector2f blended_NE_offset_m; + blended_NE_offset_m.zero(); + float blended_alt_offset_mm = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_blend_weights[i] > 0.0f) { + // calculate the horizontal offset + Vector2f horiz_offset{}; + get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7), + (_gps_blended_state.lon / 1.0e7), + (_gps_output[i].lat / 1.0e7), + (_gps_output[i].lon / 1.0e7), + &horiz_offset(0), + &horiz_offset(1)); + + // sum weighted offsets + blended_NE_offset_m += horiz_offset * _blend_weights[i]; + + // calculate vertical offset + float vert_offset = (float)(_gps_output[i].alt - _gps_blended_state.alt); + + // sum weighted offsets + blended_alt_offset_mm += vert_offset * _blend_weights[i]; + } + } + + // Add the sum of weighted offsets to the reference position to obtain the blended position + double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7; + double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7; + double lat_deg_res, lon_deg_res; + add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, + &lon_deg_res); + _gps_output[GPS_BLENDED_INSTANCE].lat = (int32_t)(1.0E7 * lat_deg_res); + _gps_output[GPS_BLENDED_INSTANCE].lon = (int32_t)(1.0E7 * lon_deg_res); + _gps_output[GPS_BLENDED_INSTANCE].alt = _gps_blended_state.alt + (int32_t)blended_alt_offset_mm; + + // Copy remaining data from internal states to output + _gps_output[GPS_BLENDED_INSTANCE].time_usec = _gps_blended_state.time_usec; + _gps_output[GPS_BLENDED_INSTANCE].fix_type = _gps_blended_state.fix_type; + _gps_output[GPS_BLENDED_INSTANCE].vel_m_s = _gps_blended_state.vel_m_s; + _gps_output[GPS_BLENDED_INSTANCE].vel_ned[0] = _gps_blended_state.vel_ned[0]; + _gps_output[GPS_BLENDED_INSTANCE].vel_ned[1] = _gps_blended_state.vel_ned[1]; + _gps_output[GPS_BLENDED_INSTANCE].vel_ned[2] = _gps_blended_state.vel_ned[2]; + _gps_output[GPS_BLENDED_INSTANCE].eph = _gps_blended_state.eph; + _gps_output[GPS_BLENDED_INSTANCE].epv = _gps_blended_state.epv; + _gps_output[GPS_BLENDED_INSTANCE].sacc = _gps_blended_state.sacc; + _gps_output[GPS_BLENDED_INSTANCE].gdop = _gps_blended_state.gdop; + _gps_output[GPS_BLENDED_INSTANCE].nsats = _gps_blended_state.nsats; + _gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid = _gps_blended_state.vel_ned_valid; + +} + + Ekf2 *Ekf2::instantiate(int argc, char *argv[]) { Ekf2 *instance = new Ekf2(); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 9bce2cd2ee..d026a21793 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1268,3 +1268,34 @@ PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f); * @decimal 2 */ PARAM_DEFINE_FLOAT(EKF2_ABL_TAU, 0.5f); + +/** + * Multi GPS Blending Control Mask. + * + * Set bits in the following positions to set which GPS accuracy metrics will be used to calculate the blending weight. Set to zero to disable and always used first GPS instance. + * 0 : Set to true to use speed accuracy + * 1 : Set to true to use horizontal position accuracy + * 2 : Set to true to use vertical position accuracy + * + * @group EKF2 + * @min 0 + * @max 7 + * @bit 0 use speed accuracy + * @bit 1 use hpos accuracy + * @bit 2 use vpos accuracy + */ +PARAM_DEFINE_INT32(EKF2_GPS_MASK, 0); + +/** + * Multi GPS Blending Time Constant + * + * Sets the longest time constant that will be applied to the calculation of GPS position and height offsets used to correct data from multiple GPS data for steady state position differences. + * + * + * @group EKF2 + * @min 1.0 + * @max 100.0 + * @unit s + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_TAU, 10.0f); diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index e33ef63eb2..f451fe9f08 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -687,6 +687,7 @@ void Logger::add_estimator_replay_topics() { // for estimator replay (need to be at full rate) add_topic("ekf2_timestamps"); + add_topic("ekf_gps_position"); // current EKF2 subscriptions add_topic("airspeed");