fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 08:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: Add support for use of multiple GPS receivers (#9765)

Browse Source
This commit is contained in:
Paul Riseborough
2018-07-23 02:18:30 +10:00
committed by Daniel Agar
parent afe82ffb31
commit fc65939f0e
5 changed files with 711 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+661 -31
View File
@@ -55,22 +55,32 @@
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_innovations.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/ekf_gps_position.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_magnetometer.h>
// 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_s> _vehicle_local_position_pub;
uORB::Publication<vehicle_global_position_s> _vehicle_global_position_pub;
@@ -401,7 +459,13 @@ private:
(ParamFloat<px4::params::EKF2_PCOEF_Y>)
_K_pstatic_coef_y, ///< static pressure position error coefficient along the Y body axis
(ParamFloat<px4::params::EKF2_PCOEF_Z>)
_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<px4::params::EKF2_GPS_MASK>)
_gps_blend_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
(ParamFloat<px4::params::EKF2_GPS_TAU>)
_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();