From 927186339c98911d36996e1679babc03c6d2b5ff Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 1 Jul 2016 13:39:38 +1000 Subject: [PATCH] AP_NavEKF2: Improved output predictor tracking Implement a PI feedback controller for velocity and position state tracking --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 9 ++-- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 52 ++++++++++-------------- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 2 +- 3 files changed, 26 insertions(+), 37 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index cd8f3f78a7..b66268ba3a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -454,11 +454,11 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @Param: TAU_OUTPUT // @DisplayName: Output complementary filter time constant (centi-sec) - // @Description: Sets the time constant of the output complementary filter/predictor in centi-seconds. Set to a negative number to use a computationally cheaper and less accurate method with an automatically calculated time constant. - // @Range: -1 100 - // @Increment: 10 + // @Description: Sets the time constant of the output complementary filter/predictor in centi-seconds. + // @Range: 5 30 + // @Increment: 5 // @User: Advanced - AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, 10), + AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, 20), // @Param: MAGE_P_NSE // @DisplayName: Earth magnetic field process noise (gauss/s) @@ -476,7 +476,6 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @Units: gauss/s AP_GROUPINFO("MAGB_P_NSE", 41, NavEKF2, _magBodyProcessNoise, MAGB_P_NSE_DEFAULT), - AP_GROUPEND }; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 2588434d18..a7d8db10d2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -213,8 +213,8 @@ void NavEKF2_core::InitialiseVariables() tasStoreIndex = 0; ofStoreIndex = 0; delAngCorrection.zero(); - delVelCorrection.zero(); velCorrection.zero(); + posCorrection.zero(); gpsGoodToAlign = false; gpsNotAvailable = true; motorsArmed = false; @@ -584,7 +584,7 @@ void NavEKF2_core::calcOutputStates() outputDataNew.quat.rotation_matrix(Tbn_temp); // transform body delta velocities to delta velocities in the nav frame - Vector3f delVelNav = Tbn_temp*delVelNewCorrected + delVelCorrection; + Vector3f delVelNav = Tbn_temp*delVelNewCorrected; delVelNav.z += GRAVITY_MSS*imuDataNew.delVelDT; // save velocity for use in trapezoidal integration for position calcuation @@ -594,7 +594,7 @@ void NavEKF2_core::calcOutputStates() outputDataNew.velocity += delVelNav; // apply a trapezoidal integration to velocities to calculate position - outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f) + velCorrection * imuDataNew.delVelDT; + outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f); // store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer if (runUpdates) { @@ -622,41 +622,41 @@ void NavEKF2_core::calcOutputStates() deltaAngErr.y = scaler * quatErr[2]; deltaAngErr.z = scaler * quatErr[3]; - // collect magnitude tracking error for diagnostics - outputTrackError.x = deltaAngErr.length(); - // calculate a gain that provides tight tracking of the estimator states and // adjust for changes in time delay to maintain consistent damping ratio of ~0.7 float timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms); timeDelay = fmaxf(timeDelay, dtIMUavg); - float errorGain = 0.45f / timeDelay; + float errorGain = 0.5f / timeDelay; // calculate a corrrection to the delta angle // that will cause the INS to track the EKF quaternions delAngCorrection = deltaAngErr * errorGain * dtIMUavg; + // calculate velocity and position tracking errors + Vector3f velDelta = (stateStruct.velocity - outputDataDelayed.velocity); + Vector3f posDelta = (stateStruct.position - outputDataDelayed.position); + + // collect magnitude tracking error for diagnostics + outputTrackError.x = deltaAngErr.length(); + outputTrackError.y = velDelta.length(); + outputTrackError.z = posDelta.length(); + // If the user specifes a time constant for the position and velocity states then calculate and apply the position // and velocity corrections immediately to the whole output history which takes longer to process but enables smaller // time constants to be used. Else apply the corrections to the current state only using the same time constant // used for the quaternion corrections. if (frontend->_tauVelPosOutput > 0) { // convert time constant from centi-seconds to seconds - float tauPosVel = 0.01f*(float)frontend->_tauVelPosOutput; + float tauPosVel = constrain_float(0.01f*(float)frontend->_tauVelPosOutput, 0.1f, 0.5f); // calculate a gain to track the EKF position states with the specified time constant float velPosGain = dtEkfAvg / constrain_float(tauPosVel, dtEkfAvg, 10.0f); - // calculate tracking errors - Vector3f velDelta = (stateStruct.velocity - outputDataDelayed.velocity); - Vector3f posDelta = (stateStruct.position - outputDataDelayed.position); - - // collect magnitude of tracking errors for diagnostics - outputTrackError.y = velDelta.length(); - outputTrackError.z = posDelta.length(); - - // multiply error vectors by gain to calculate a correction that will be applied to the output state history - velDelta *= velPosGain; - posDelta *= velPosGain; + // use a PI feedback to calculate a correction that will be applied to the output state history + posCorrection += posDelta * velPosGain; // I term + velCorrection += velDelta * velPosGain; // I term + velDelta *= velPosGain; // P term + posDelta *= velPosGain; // P term // loop through the output filter state history and apply the corrections to the velocity and position states // this method is too expensive to use for the attitude states due to the quaternion operations required @@ -667,10 +667,10 @@ void NavEKF2_core::calcOutputStates() outputStates = storedOutput[index]; // a constant velocity correction is applied - outputStates.velocity += velDelta; + outputStates.velocity += velDelta + velCorrection; // a constant position correction is applied - outputStates.position += posDelta; + outputStates.position += posDelta + posCorrection; // push the updated data to the buffer storedOutput[index] = outputStates; @@ -680,17 +680,7 @@ void NavEKF2_core::calcOutputStates() //outputDataDelayed = storedOutput[storedIMU.get_oldest_index()]; outputDataNew = storedOutput[storedIMU.get_youngest_index()]; - // set un-used corrections to zero - delVelCorrection.zero(); - velCorrection.zero(); - - } else { - // multiply velocity and position error by a gain to calculate the delta velocity correction required to track the EKF solution - delVelCorrection = (stateStruct.velocity - outputDataDelayed.velocity) * errorGain * dtIMUavg; - velCorrection = (stateStruct.position - outputDataDelayed.position) * errorGain; - } - } } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index dc668a914e..9df05e0d27 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -761,8 +761,8 @@ private: output_elements outputDataNew; // output state data at the current time step output_elements outputDataDelayed; // output state data at the current time step Vector3f delAngCorrection; // correction applied to delta angles used by output observer to track the EKF - Vector3f delVelCorrection; // correction applied to earth frame delta velocities used by output observer to track the EKF Vector3f velCorrection; // correction applied to velocities used by the output observer to track the EKF + Vector3f posCorrection; // correction applied to positions used by the output observer to track the EKF float innovYaw; // compass yaw angle innovation (rad) uint32_t timeTasReceived_ms; // time last TAS data was received (msec) bool gpsGoodToAlign; // true when the GPS quality can be used to initialise the navigation system