Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: implement moving origin 

this shifts EKF_origin to the current location at 1Hz, while leaving
public_origin alone. All output APIs and logging are relative to
public_origin.

The effect of this change is to remove the distortion caused by a
spherical earth, which allows the EKF to operate without errors at
very long distances from the public_origin.
This commit is contained in:
Andrew Tridgell 2021-07-10 15:17:11 +10:00 committed by Randy Mackay
parent d43e95a39a
commit 579e4566c6
6 changed files with 79 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -611,9 +611,12 @@ bool NavEKF3_core::setOrigin(const Location &loc)
validOrigin = true;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u origin set",(unsigned)imu_index);
// put origin in frontend as well to ensure it stays in sync between lanes
frontend->common_EKF_origin = EKF_origin;
if (!frontend->common_origin_valid) {
frontend->common_origin_valid = true;
// put origin in frontend as well to ensure it stays in sync between lanes
public_origin = EKF_origin;
}
return true;
}

18
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -648,16 +648,6 @@ void NavEKF3_core::readGpsData()
// Post-alignment checks
calcGpsGoodForFlight();
// see if we can get an origin from the frontend
if (!validOrigin && frontend->common_origin_valid) {
if (!setOrigin(frontend->common_EKF_origin)) {
// set an error as an attempt was made to set the origin more than once
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return;
}
}
// Read the GPS location in WGS-84 lat,long,height coordinates
const struct Location &gpsloc = gps.location(selected_gps);
@ -700,7 +690,8 @@ void NavEKF3_core::readGpsData()
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
if (validOrigin) {
gpsDataNew.pos = EKF_origin.get_distance_NE_ftype(gpsloc);
gpsDataNew.lat = gpsloc.lat;
gpsDataNew.lng = gpsloc.lng;
if ((frontend->_originHgtMode & (1<<2)) == 0) {
gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
} else {
@ -1463,15 +1454,14 @@ void NavEKF3_core::getEarthFieldTable(const Location &loc)
table_declination = radians(AP_Declination::get_declination(loc.lat*1.0e-7,
loc.lng*1.0e-7));
have_table_earth_field = true;
last_field_update_ms = imuSampleTime_ms;
}
/*
check if we should update the earth field, we update it at 1Hz
update earth field, called at 1Hz
*/
void NavEKF3_core::checkUpdateEarthField(void)
{
if (have_table_earth_field && imuSampleTime_ms - last_field_update_ms > 1000) {
if (have_table_earth_field && filterStatus.flags.using_gps) {
Location loc = EKF_origin;
loc.offset(stateStruct.position.x, stateStruct.position.y);
getEarthFieldTable(loc);

15
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -207,8 +207,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
if (PV_AidingMode != AID_NONE) {
// This is the normal mode of operation where we can use the EKF position states
// correct for the IMU offset (EKF calculations are at the IMU)
posNE.x = outputDataNew.position.x + posOffsetNED.x;
posNE.y = outputDataNew.position.y + posOffsetNED.y;
posNE = (outputDataNew.position.xy() + posOffsetNED.xy() + public_origin.get_distance_NE_ftype(EKF_origin)).tofloat();
return true;
} else {
@ -218,9 +217,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
const struct Location &gpsloc = gps.location(selected_gps);
const Vector2F tempPosNE = EKF_origin.get_distance_NE_ftype(gpsloc);
posNE.x = tempPosNE.x;
posNE.y = tempPosNE.y;
posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat();
return false;
} else if (rngBcnAlignmentStarted) {
// If we are attempting alignment using range beacon data, then report the position
@ -229,14 +226,12 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
return false;
} else {
// If no GPS fix is available, all we can do is provide the last known position
posNE.x = outputDataNew.position.x;
posNE.y = outputDataNew.position.y;
posNE = outputDataNew.position.xy().tofloat();
return false;
}
} else {
// If the origin has not been set, then we have no means of providing a relative position
posNE.x = 0.0f;
posNE.y = 0.0f;
posNE.zero();
return false;
}
}
@ -357,7 +352,7 @@ void NavEKF3_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGa
bool NavEKF3_core::getOriginLLH(struct Location &loc) const
{
if (validOrigin) {
loc = EKF_origin;
loc = public_origin;
// report internally corrected reference height if enabled
if ((frontend->_originHgtMode & (1<<2)) == 0) {
loc.alt = (int32_t)(100.0f * (float)ekfGpsRefHgt);

26
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -95,9 +95,13 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
CorrectGPSForAntennaOffset(gps_corrected);
// record the ID of the GPS for the data we are using for the reset
last_gps_idx = gps_corrected.sensor_idx;
// write to state vector and compensate for offset between last GPS measurement and the EKF time horizon
stateStruct.position.x = gps_corrected.pos.x + 0.001*gps_corrected.vel.x*(ftype(imuDataDelayed.time_ms) - ftype(gps_corrected.time_ms));
stateStruct.position.y = gps_corrected.pos.y + 0.001*gps_corrected.vel.y*(ftype(imuDataDelayed.time_ms) - ftype(gps_corrected.time_ms));
// calculate position
const Location gpsloc{gps_corrected.lat, gps_corrected.lng, 0, Location::AltFrame::ABSOLUTE};
stateStruct.position.xy() = EKF_origin.get_distance_NE_ftype(gpsloc);
// compensate for offset between last GPS measurement and the EKF time horizon. Note that this is an unusual
// time delta in that it can be both -ve and +ve
const int32_t tdiff = imuDataDelayed.time_ms - gps_corrected.time_ms;
stateStruct.position.xy() += gps_corrected.vel.xy()*0.001*tdiff;
// set the variances using the position measurement noise parameter
P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
} else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::RNGBCN) {
@ -335,8 +339,7 @@ void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
gps_data.vel -= velOffsetEarth;
Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
gps_data.pos.x -= posOffsetEarth.x;
gps_data.pos.y -= posOffsetEarth.y;
Location::offset_latlng(gps_data.lat, gps_data.lng, -posOffsetEarth.x, -posOffsetEarth.y);
gps_data.hgt += posOffsetEarth.z;
}
@ -349,6 +352,9 @@ void NavEKF3_core::CorrectExtNavForSensorOffset(ext_nav_elements &ext_nav_data)
}
ext_nav_data.corrected = true;
// external nav data is against the public_origin, so convert to offset from EKF_origin
ext_nav_data.pos.xy() += EKF_origin.get_distance_NE_ftype(public_origin);
#if HAL_VISUALODOM_ENABLED
const auto *visual_odom = dal.visualodom();
if (visual_odom == nullptr) {
@ -477,8 +483,10 @@ void NavEKF3_core::SelectVelPosFusion()
velPosObs[1] = gpsDataDelayed.vel.y;
velPosObs[2] = gpsDataDelayed.vel.z;
}
velPosObs[3] = gpsDataDelayed.pos.x;
velPosObs[4] = gpsDataDelayed.pos.y;
const Location gpsloc{gpsDataDelayed.lat, gpsDataDelayed.lng, 0, Location::AltFrame::ABSOLUTE};
const Vector2F posxy = EKF_origin.get_distance_NE_ftype(gpsloc);
velPosObs[3] = posxy.x;
velPosObs[4] = posxy.y;
#if EK3_FEATURE_EXTERNAL_NAV
} else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV)) {
// use external nav system for horizontal position
@ -517,7 +525,9 @@ void NavEKF3_core::SelectVelPosFusion()
last_gps_idx = gpsDataDelayed.sensor_idx;
// reset the position to the GPS position
ResetPositionNE(gpsDataDelayed.pos.x, gpsDataDelayed.pos.y);
const Location gpsloc{gpsDataDelayed.lat, gpsDataDelayed.lng, 0, Location::AltFrame::ABSOLUTE};
const Vector2F posxy = EKF_origin.get_distance_NE_ftype(gpsloc);
ResetPositionNE(posxy.x, posxy.y);
// If we are also using GPS as the height reference, reset the height
if (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) {

37
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -10,7 +10,8 @@
// constructor
NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend) :
frontend(_frontend),
dal(AP::dal())
dal(AP::dal()),
public_origin(frontend->common_EKF_origin)
{
firstInitTime_ms = 0;
lastInitFailReport_ms = 0;
@ -701,9 +702,13 @@ void NavEKF3_core::UpdateFilter(bool predict)
// Update the filter status
updateFilterStatus();
// check for update of earth field
if (imuSampleTime_ms - last_oneHz_ms >= 1000) {
// 1Hz tasks
last_oneHz_ms = imuSampleTime_ms;
moveEKFOrigin();
checkUpdateEarthField();
}
}
// Wind output forward from the fusion to output time horizon
calcOutputStates();
@ -2146,3 +2151,31 @@ void NavEKF3_core::verifyTiltErrorVariance()
}
}
#endif
/*
move the EKF origin to the current position at 1Hz. The public_origin doesn't move.
By moving the EKF origin we keep the distortion due to spherical
shape of the earth to a minimum.
*/
void NavEKF3_core::moveEKFOrigin(void)
{
// only move origin when we have a origin and we're using GPS
if (!frontend->common_origin_valid || !filterStatus.flags.using_gps) {
return;
}
// move the origin to the current state location
Location loc = EKF_origin;
loc.offset(stateStruct.position.x, stateStruct.position.y);
const Vector2F diffNE = loc.get_distance_NE_ftype(EKF_origin);
EKF_origin = loc;
// now fix all output states
stateStruct.position.xy() += diffNE;
outputDataNew.position.xy() += diffNE;
outputDataDelayed.position.xy() += diffNE;
for (unsigned index=0; index < imu_buffer_length; index++) {
storedOutput[index].position.xy() += diffNE;
}
}

13
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -499,7 +499,7 @@ private:
};
struct gps_elements : EKF_obs_element_t {
Vector2F pos; // horizontal North East position of the GPS antenna in local NED earth frame (m)
int32_t lat, lng; // latitude and longitude in 1e7 degrees
ftype hgt; // height of the GPS antenna in local NED earth frame (m)
Vector3F vel; // velocity of the GPS antenna in local NED earth frame (m/sec)
uint8_t sensor_idx; // unique integer identifying the GPS sensor
@ -1025,7 +1025,8 @@ private:
bool needEarthBodyVarReset; // we need to reset mag earth variances at next CovariancePrediction
bool inhibitDelAngBiasStates; // true when IMU delta angle bias states are inactive
bool gpsNotAvailable; // bool true when valid GPS data is not available
struct Location EKF_origin; // LLH origin of the NED axis system
struct Location EKF_origin; // LLH origin of the NED axis system, internal only
struct Location &public_origin; // LLH origin of the NED axis system, public functions
bool validOrigin; // true when the EKF origin is valid
ftype gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
ftype gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
@ -1403,7 +1404,13 @@ private:
bool have_table_earth_field; // true when we have initialised table_earth_field_ga
Vector3F table_earth_field_ga; // earth field from WMM tables
ftype table_declination; // declination in radians from the tables
uint32_t last_field_update_ms;
// 1Hz update
uint32_t last_oneHz_ms;
void oneHzUpdate(void);
// move EKF origin at 1Hz
void moveEKFOrigin(void);
// handle earth field updates
void getEarthFieldTable(const Location &loc);