fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 22:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move GPS blending from ekf2 to sensors module

Browse Source 
- new sensors work item that subscribes to N x sensor_gps and publishes vehicle_gps_position
 - blending is now configurable with SENS_GPS_MASK and SENS_GPS_TAU

Co-authored-by: Jacob Crabill <jacob@volans-i.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
This commit is contained in:
Jacob Dahl
2020-09-25 23:28:31 -04:00
committed by GitHub
co-authored by Jacob Crabill Jacob Dahl
parent e792c46f20
commit a24488328f
27 changed files with 1147 additions and 861 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+13 -633
View File
@@ -154,7 +154,6 @@ EKF2::EKF2(bool replay_mode):
// advertise immediately to ensure consistent uORB instance numbering
_att_pub.advertise();
_blended_gps_pub.advertise();
_ekf2_timestamps_pub.advertise();
_ekf_gps_drift_pub.advertise();
_estimator_innovation_test_ratios_pub.advertise();
@@ -458,87 +457,18 @@ void EKF2::Run()
}
}
// read gps1 data if available
bool gps1_updated = _gps_subs[0].updated();
if (gps1_updated) {
if (_vehicle_gps_position_sub.updated()) {
vehicle_gps_position_s gps;
if (_gps_subs[0].copy(&gps)) {
fillGpsMsgWithVehicleGpsPosData(_gps_state[0], gps);
_gps_alttitude_ellipsoid[0] = gps.alt_ellipsoid;
}
}
if (_vehicle_gps_position_sub.copy(&gps)) {
gps_message gps_msg{};
// check for second GPS receiver data
bool gps2_updated = _gps_subs[1].updated();
fillGpsMsgWithVehicleGpsPosData(gps_msg, gps);
if (gps2_updated) {
vehicle_gps_position_s gps;
_ekf.setGpsData(gps_msg);
if (_gps_subs[1].copy(&gps)) {
fillGpsMsgWithVehicleGpsPosData(_gps_state[1], gps);
_gps_alttitude_ellipsoid[1] = gps.alt_ellipsoid;
}
}
if ((_param_ekf2_gps_mask.get() == 0) && gps1_updated) {
// When GPS blending is disabled we always use the first receiver instance
_ekf.setGpsData(_gps_state[0]);
} else if ((_param_ekf2_gps_mask.get() > 0) && (gps1_updated || gps2_updated)) {
// blend dual receivers if available
// calculate blending weights
if (!blend_gps_data()) {
// handle case where the blended states cannot be updated
// Only use selected receiver data if it has been updated
_gps_new_output_data = (gps1_updated && _gps_select_index == 0) ||
(gps2_updated && _gps_select_index == 1);
// Reset relative position offsets to zero
_NE_pos_offset_m[0].zero();
_NE_pos_offset_m[1].zero();
_hgt_offset_mm[0] = _hgt_offset_mm[1] = 0.0f;
}
if (_gps_new_output_data) {
// correct the _gps_state data for steady state offsets and write to _gps_output
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]);
// 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.heading = _gps_output[_gps_select_index].yaw;
gps.heading_offset = _gps_output[_gps_select_index].yaw_offset;
gps.selected = _gps_select_index;
// Publish to the EKF blended GPS topic
_blended_gps_pub.publish(gps);
// clear flag to avoid re-use of the same data
_gps_new_output_data = false;
_gps_time_usec = gps_msg.time_usec;
_gps_alttitude_ellipsoid = gps.alt_ellipsoid;
}
}
@@ -1400,570 +1330,20 @@ void EKF2::publish_wind_estimate(const hrt_abstime &timestamp)
}
}
bool EKF2::blend_gps_data()
{
// zero the blend weights
memset(&_blend_weights, 0, sizeof(_blend_weights));
/*
* If both receivers have the same update rate, use the oldest non-zero time.
* If two receivers with different update rates are used, use the slowest.
* If time difference is excessive, use newest to prevent a disconnected receiver
* from blocking updates.
*/
// Calculate the time step for each receiver with some filtering to reduce the effects of jitter
// Find the largest and smallest time step.
float dt_max = 0.0f;
float dt_min = 0.3f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
float raw_dt = 1e-6f * (float)(_gps_state[i].time_usec - _time_prev_us[i]);
if (raw_dt > 0.0f && raw_dt < 0.3f) {
_gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i];
}
if (_gps_dt[i] > dt_max) {
dt_max = _gps_dt[i];
_gps_slowest_index = i;
}
if (_gps_dt[i] < dt_min) {
dt_min = _gps_dt[i];
}
}
// Find the receiver that is last be updated
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;
_gps_newest_index = i;
}
if ((_gps_state[i].time_usec < min_us) && (_gps_state[i].time_usec > 0)) {
min_us = _gps_state[i].time_usec;
_gps_oldest_index = i;
}
}
if ((max_us - min_us) > 300000) {
// A receiver has timed out so fall out of blending
if (_gps_state[0].time_usec > _gps_state[1].time_usec) {
_gps_select_index = 0;
} else {
_gps_select_index = 1;
}
return false;
}
// One receiver has lost 3D fix, fall out of blending
if (_gps_state[0].fix_type > 2 && _gps_state[1].fix_type < 3) {
_gps_select_index = 0;
return false;
} else if (_gps_state[1].fix_type > 2 && _gps_state[0].fix_type < 3) {
_gps_select_index = 1;
return false;
}
/*
* If the largest dt is less than 20% greater than the smallest, then we have receivers
* running at the same rate then we wait until we have two messages with an arrival time
* difference that is less than 50% of the smallest time step and use the time stamp from
* the newest data.
* Else we have two receivers at different update rates and use the slowest receiver
* as the timing reference.
*/
if ((dt_max - dt_min) < 0.2f * dt_min) {
// both receivers assumed to be running at the same rate
if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) {
// data arrival within a short time window enables the two measurements to be blended
_gps_time_ref_index = _gps_newest_index;
_gps_new_output_data = true;
}
} else {
// both receivers running at different rates
_gps_time_ref_index = _gps_slowest_index;
if (_gps_state[_gps_time_ref_index].time_usec > _time_prev_us[_gps_time_ref_index]) {
// blend data at the rate of the slower receiver
_gps_new_output_data = true;
}
}
if (_gps_new_output_data) {
_gps_blended_state.time_usec = _gps_state[_gps_time_ref_index].time_usec;
// calculate the sum squared speed accuracy across all GPS sensors
float speed_accuracy_sum_sq = 0.0f;
if (_param_ekf2_gps_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 (_param_ekf2_gps_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 (_param_ekf2_gps_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 && (_param_ekf2_gps_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 && (_param_ekf2_gps_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 && (_param_ekf2_gps_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;
}
// 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;
}
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 solution.
*/
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.setZero();
_gps_blended_state.vel_ned_valid = true;
_gps_blended_state.nsats = 0;
_gps_blended_state.pdop = 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 += _gps_state[i].vel_ned * _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].pdop > 0
&& _gps_state[i].pdop < _gps_blended_state.pdop) {
_gps_blended_state.pdop = _gps_state[i].pdop;
}
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;
// Take GPS heading from the highest weighted receiver that is publishing a valid .heading value
uint8_t gps_best_yaw_index = 0;
best_weight = 0.0f;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (PX4_ISFINITE(_gps_state[i].yaw) && (_blend_weights[i] > best_weight)) {
best_weight = _blend_weights[i];
gps_best_yaw_index = i;
}
}
_gps_blended_state.yaw = _gps_state[gps_best_yaw_index].yaw;
_gps_blended_state.yaw_offset = _gps_state[gps_best_yaw_index].yaw_offset;
}
/*
* 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
// 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(_param_ekf2_gps_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
alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].time_usec - _time_prev_us[i]),
0.0f, 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 = _gps_state[i].vel_ned;
_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].pdop = _gps_state[i].pdop;
_gps_output[i].nsats = _gps_state[i].nsats;
_gps_output[i].vel_ned_valid = _gps_state[i].vel_ned_valid;
_gps_output[i].yaw = _gps_state[i].yaw;
_gps_output[i].yaw_offset = _gps_state[i].yaw_offset;
}
}
/*
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 = _gps_blended_state.vel_ned;
_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].pdop = _gps_blended_state.pdop;
_gps_output[GPS_BLENDED_INSTANCE].nsats = _gps_blended_state.nsats;
_gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid = _gps_blended_state.vel_ned_valid;
_gps_output[GPS_BLENDED_INSTANCE].yaw = _gps_blended_state.yaw;
_gps_output[GPS_BLENDED_INSTANCE].yaw_offset = _gps_blended_state.yaw_offset;
}
float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
{
float height_diff = static_cast<float>(_gps_alttitude_ellipsoid[0]) * 1e-3f - amsl_hgt;
float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;
if (_gps_alttitude_ellipsoid_previous_timestamp[0] == 0) {
if (_gps_alttitude_ellipsoid_previous_timestamp == 0) {
_wgs84_hgt_offset = height_diff;
_gps_alttitude_ellipsoid_previous_timestamp[0] = _gps_state[0].time_usec;
_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
} else if (_gps_state[0].time_usec != _gps_alttitude_ellipsoid_previous_timestamp[0]) {
} else if (_gps_time_usec != _gps_alttitude_ellipsoid_previous_timestamp) {
// apply a 10 second first order low pass filter to baro offset
float dt = 1e-6f * static_cast<float>(_gps_state[0].time_usec - _gps_alttitude_ellipsoid_previous_timestamp[0]);
_gps_alttitude_ellipsoid_previous_timestamp[0] = _gps_state[0].time_usec;
float dt = 1e-6f * (_gps_time_usec - _gps_alttitude_ellipsoid_previous_timestamp);
_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset);
_wgs84_hgt_offset += dt * constrain(offset_rate_correction, -0.1f, 0.1f);
}
src/modules/ekf2/EKF2.hpp
+5 -79
View File
@@ -60,7 +60,6 @@
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/ekf_gps_drift.h>
#include <uORB/topics/ekf_gps_position.h>
#include <uORB/topics/estimator_innovations.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/estimator_states.h>
@@ -85,15 +84,6 @@
#include "Utility/PreFlightChecker.hpp"
// 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
class EKF2 final : public ModuleBase<EKF2>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
@@ -135,38 +125,6 @@ private:
void publish_wind_estimate(const hrt_abstime &timestamp);
void publish_yaw_estimator_status(const hrt_abstime &timestamp);
/*
* Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
* receiver solutions. 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 solution.
*/
bool blend_gps_data();
/*
* 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();
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
@@ -197,34 +155,11 @@ private:
// Used to control saving of mag declination to be used on next startup
bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved
// set pose/velocity as invalid if standard deviation is bigger than max_std_dev
// TODO: the user should be allowed to set these values by a parameter
static constexpr float ep_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated position
static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation
//static constexpr float ev_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated velocity
bool _had_valid_terrain{false}; ///< true if at any time there was a valid terrain estimate
// 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
uint8_t _gps_time_ref_index =
0; ///< index of the receiver that is used as the timing reference for the blending update
uint8_t _gps_oldest_index = 0; ///< index of the physical receiver with the oldest data
uint8_t _gps_newest_index = 0; ///< index of the physical receiver with the newest data
uint8_t _gps_slowest_index = 0; ///< index of the physical receiver with the slowest update rate
float _gps_dt[GPS_MAX_RECEIVERS] = {}; ///< average time step in seconds.
bool _gps_new_output_data = false; ///< true if there is new output data for the EKF
bool _had_valid_terrain = false; ///< true if at any time there was a valid terrain estimate
int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt
uint64_t _gps_time_usec{0};
int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
bool _imu_bias_reset_request{false};
@@ -242,6 +177,7 @@ private:
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
@@ -255,16 +191,12 @@ private:
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
int _range_finder_sub_index = -1; // index for downward-facing range finder subscription
// because we can have multiple GPS instances
uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}};
sensor_selection_s _sensor_selection{};
vehicle_land_detected_s _vehicle_land_detected{};
vehicle_status_s _vehicle_status{};
uORB::Publication<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::Publication<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
uORB::Publication<ekf_gps_position_s> _blended_gps_pub{ORB_ID(ekf_gps_position)};
uORB::Publication<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::Publication<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::Publication<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
@@ -516,12 +448,6 @@ private:
(ParamExtFloat<px4::params::EKF2_PCOEF_Z>)
_param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis
// GPS blending
(ParamInt<px4::params::EKF2_GPS_MASK>)
_param_ekf2_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
(ParamFloat<px4::params::EKF2_GPS_TAU>)
_param_ekf2_gps_tau, ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
// Test used to determine if the vehicle is static or moving
(ParamExtFloat<px4::params::EKF2_MOVE_TEST>)
_param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
src/modules/ekf2/ekf2_params.c
-31
View File
@@ -1375,37 +1375,6 @@ PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f);
*/
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);
/**
* Vehicle movement test threshold
*