aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: support VISION_SPEED_ESTIMATE

Browse Source 
Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
chobits 2020-05-18 14:22:20 +09:00 committed by Randy Mackay
parent bdb67532b0
commit c7817eaca1
7 changed files with 126 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1337,6 +1337,21 @@ void NavEKF3::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float
}
}
/* Write velocity data from an external navigation system
* vel : velocity in NED (m)
* err : velocity error (m/s)
* timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
* delay_ms : average delay of external nav system measurements relative to inertial measurements
*/
void NavEKF3::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
}
}
}
// return data for debugging optical flow fusion
void NavEKF3::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov,
float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const

10
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -296,6 +296,16 @@ public:
*/
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
/*
* Write velocity data from an external navigation system
*
* vel : velocity in NED (m)
* err : velocity error (m/s)
* timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
* delay_ms : average delay of external nav system measurements relative to inertial measurements
*/
void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
void setTakeoffExpected(bool val);

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -581,7 +581,7 @@ void NavEKF3_core::updateFilterStatus(void)
bool doingFlowNav = (PV_AidingMode != AID_NONE) && flowDataValid;
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
bool someVertRefData = (!velTimeout && (useGpsVertVel || useExtNavVel)) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav || doingBodyVelNav;
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE)));

18
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -988,6 +988,24 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
storedExtNav.push(extNavDataNew);
}
void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
if ((timeStamp_ms - extNavVelMeasTime_ms) < 70) {
return;
}
extNavVelMeasTime_ms = timeStamp_ms - delay_ms;
useExtNavVel = true;
extNavVelNew.vel = vel;
extNavVelNew.err = err;
// Correct for the average intersampling delay due to the filter updaterate
timeStamp_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer
timeStamp_ms = MAX(timeStamp_ms,imuDataDelayed.time_ms);
extNavVelNew.time_ms = timeStamp_ms;
storedExtNavVel.push(extNavVelNew);
}
/*
update timing statistics structure
*/

51
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -45,6 +45,15 @@ void NavEKF3_core::ResetVelocity(void)
// clear the timeout flags and counters
velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms;
} else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && posResetSource == DEFAULT) || velResetSource == EXTNAV) {
// use external nav data as the 2nd preference
// correct for sensor position
ext_nav_vel_elements extNavVelCorrected = extNavVelDelayed;
CorrectExtNavVelForSensorOffset(extNavVelCorrected.vel);
stateStruct.velocity = extNavVelCorrected.vel;
P[5][5] = P[4][4] = P[3][3] = sq(extNavVelDelayed.err);
velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms;
} else {
stateStruct.velocity.x = 0.0f;
stateStruct.velocity.y = 0.0f;
@ -256,6 +265,8 @@ void NavEKF3_core::ResetHeight(void)
// Check that GPS vertical velocity data is available and can be used
if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0 && !frontend->inhibitGpsVertVelUse) {
stateStruct.velocity.z = gpsDataNew.vel.z;
} else if (inFlight && useExtNavVel && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
stateStruct.velocity.z = extNavVelNew.vel.z;
} else if (onGround) {
stateStruct.velocity.z = 0.0f;
}
@ -356,6 +367,25 @@ void NavEKF3_core::CorrectExtNavForSensorOffset(Vector3f &ext_position)
#endif
}
// correct external navigation earth-frame velocity using sensor body-frame offset
void NavEKF3_core::CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const
{
#if HAL_VISUALODOM_ENABLED
AP_VisualOdom *visual_odom = AP::visualodom();
if (visual_odom == nullptr) {
return;
}
const Vector3f &posOffsetBody = visual_odom->get_pos_offset() - accelPosOffset;
if (posOffsetBody.is_zero()) {
return;
}
// TODO use a filtered angular rate with a group delay that matches the sensor delay
const Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
ext_velocity += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate);
#endif
}
/********************************************************
* FUSE MEASURED_DATA *
********************************************************/
@ -374,6 +404,7 @@ void NavEKF3_core::SelectVelPosFusion()
// Check for data at the fusion time horizon
extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms);
// Read GPS data from the sensor
readGpsData();
@ -422,6 +453,18 @@ void NavEKF3_core::SelectVelPosFusion()
fusePosData = false;
}
// fuse external navigation velocity data if available
if (extNavVelToFuse && (frontend->_fusionModeGPS == 3)) {
fuseVelData = true;
// correct for external navigation sensor position
Vector3f vel_corrected = extNavVelDelayed.vel;
CorrectExtNavVelForSensorOffset(vel_corrected);
velPosObs[0] = vel_corrected.x;
velPosObs[1] = vel_corrected.y;
velPosObs[2] = vel_corrected.z;
}
// we have GPS data to fuse and a request to align the yaw using the GPS course
if (gpsYawResetRequest) {
realignYawGPS();
@ -569,6 +612,8 @@ void NavEKF3_core::FuseVelPosNED()
// use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
} else if (extNavVelToFuse) {
R_OBS[2] = R_OBS[0] = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f));
} else {
// calculate additional error in GPS velocity caused by manoeuvring
R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
@ -654,7 +699,7 @@ void NavEKF3_core::FuseVelPosNED()
// test velocity measurements
uint8_t imax = 2;
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
if (frontend->_fusionModeGPS > 0 || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) {
if ((frontend->_fusionModeGPS > 0 || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) && !useExtNavVel) {
imax = 1;
}
float innovVelSumSq = 0; // sum of squares of velocity innovations
@ -738,7 +783,7 @@ void NavEKF3_core::FuseVelPosNED()
if (fuseVelData && velHealth) {
fuseData[0] = true;
fuseData[1] = true;
if (useGpsVertVel) {
if (useGpsVertVel || useExtNavVel) {
fuseData[2] = true;
}
}
@ -1075,7 +1120,7 @@ void NavEKF3_core::selectHeightForFusion()
// If we haven't fused height data for a while, then declare the height data as being timed out
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
hgtRetryTime_ms = ((useGpsVertVel || useExtNavVel) && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) {
hgtTimeout = true;
} else {

9
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -143,6 +143,9 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
if (!storedExtNav.init(obs_buffer_length)) {
return false;
}
if (!storedExtNavVel.init(obs_buffer_length)) {
return false;
}
if(!storedIMU.init(imu_buffer_length)) {
return false;
}
@ -400,6 +403,11 @@ void NavEKF3_core::InitialiseVariables()
extNavLastPosResetTime_ms = 0;
extNavDataToFuse = false;
extNavUsedForPos = false;
memset(&extNavVelNew, 0, sizeof(extNavVelNew));
memset(&extNavVelDelayed, 0, sizeof(extNavVelDelayed));
extNavVelToFuse = false;
useExtNavVel = false;
extNavVelMeasTime_ms = 0;
// zero data buffers
storedIMU.reset();
@ -412,6 +420,7 @@ void NavEKF3_core::InitialiseVariables()
storedBodyOdm.reset();
storedWheelOdm.reset();
storedExtNav.reset();
storedExtNavVel.reset();
// initialise pre-arm message
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 still initialising");

25
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -302,6 +302,16 @@ public:
*/
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
/*
* Write velocity data from an external navigation system
*
* vel : velocity in NED (m)
* err : velocity error (m/s)
* timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
* delay_ms : average delay of external nav system measurements relative to inertial measurements
*/
void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
void setTakeoffExpected(bool val);
@ -595,6 +605,12 @@ private:
bool posReset; // true when the position measurement has been reset
};
struct ext_nav_vel_elements {
Vector3f vel; // velocity in NED (m/s)
float err; // velocity measurement error (m/s)
uint32_t time_ms; // measurement timestamp (msec)
};
// bias estimates for the IMUs that are enabled but not being used
// by this core.
struct {
@ -925,6 +941,9 @@ private:
// correct external navigation earth-frame position using sensor body-frame offset
void CorrectExtNavForSensorOffset(Vector3f &ext_position);
// correct external navigation earth-frame velocity using sensor body-frame offset
void CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const;
// Runs the IMU prediction step for an independent GSF yaw estimator algorithm
// that uses IMU, GPS horizontal velocity and optionally true airspeed data.
void runYawEstimatorPrediction(void);
@ -1336,6 +1355,12 @@ private:
uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec)
bool extNavDataToFuse; // true when there is new external nav data to fuse
bool extNavUsedForPos; // true when the external nav data is being used as a position reference.
obs_ring_buffer_t<ext_nav_vel_elements> storedExtNavVel; // external navigation velocity data buffer
ext_nav_vel_elements extNavVelNew; // external navigation velocity data at the current time horizon
ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon
uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec)
bool extNavVelToFuse; // true when there is new external navigation velocity to fuse
bool useExtNavVel; // true if external nav velocity should be used
// flags indicating severe numerical errors in innovation variance calculation for different fusion operations
struct {