From c7817eaca1e0cfc890d5c564ced1e2368776bf47 Mon Sep 17 00:00:00 2001 From: chobits Date: Mon, 18 May 2020 14:22:20 +0900 Subject: [PATCH] AP_NavEKF3: support VISION_SPEED_ESTIMATE Co-authored-by: Randy Mackay --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 15 ++++++ libraries/AP_NavEKF3/AP_NavEKF3.h | 10 ++++ libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 18 +++++++ .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 51 +++++++++++++++++-- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 9 ++++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 25 +++++++++ 7 files changed, 126 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index c35c5f88c6..f57120a64d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -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_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 { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 4da11db92c..d93eeaf1ef 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -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"); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index c607406cd4..edaef69ff2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -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 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 {