From 118d5b88b26421289319f9dbd458ca33b3a2d7fd Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 29 Jun 2016 17:17:37 +1000 Subject: [PATCH] AP_NavEKF2: Clean up output observer and reduce pos vel time constant Previous Time constant was too large leading to poor tracking of EKF states in the presence of bad quality IMU data. --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 9 ++------- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 4fc82b7bc6..587f766a4a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -458,7 +458,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @Range: -1 100 // @Increment: 10 // @User: Advanced - AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, 50), + AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, 10), // @Param: MAGE_P_NSE // @DisplayName: Earth magnetic field process noise (gauss/s) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 04aaa3f291..fcb0ea7373 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -642,16 +642,11 @@ void NavEKF2_core::calcOutputStates() // convert time constant from centi-seconds to seconds float tauPosVel = 0.01f*(float)frontend->_tauVelPosOutput; - // calculate a position correction that will be applied to the output state history + // calculate a position and velocity correction that will be applied to the output state history // to track the EKF position states with the specified time constant float velPosGain = dtEkfAvg / constrain_float(tauPosVel, dtEkfAvg, 10.0f); Vector3f posDelta = (stateStruct.position - outputDataDelayed.position) * velPosGain; - - // calculate a velocity correction that will be applied to the output state history - // to track the EKF velocity states with the specified time constant - Vector3f velDelta; - float velGain = dtEkfAvg / constrain_float(tauPosVel, dtEkfAvg, 10.0f); - velDelta = (stateStruct.velocity - outputDataDelayed.velocity) * velGain; + Vector3f velDelta = (stateStruct.velocity - outputDataDelayed.velocity) * velPosGain; // 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