Updates to UKF

include/robot_localization/filter_base.h

@@ -333,14 +333,16 @@ class FilterBase
     //! @brief Method for settings bounds on acceleration values derived from controls
     //! @param[in] state - The current state variable (e.g., linear X velocity)
     //! @param[in] control - The current control commanded velocity corresponding to the state variable
+    //! @param[in] delta - the timestep of the predict for which these terms are computed
     //! @param[in] accelerationLimit - Limit for acceleration (regardless of driving direction)
     //! @param[in] accelerationGain - Gain applied to acceleration control error
     //! @param[in] decelerationLimit - Limit for deceleration (moving towards zero, regardless of driving direction)
     //! @param[in] decelerationGain - Gain applied to deceleration control error
     //! @return a usable acceleration estimate for the control vector
     //!
-    inline double computeControlAcceleration(const double state, const double control, const double accelerationLimit,
-      const double accelerationGain, const double decelerationLimit, const double decelerationGain)
+    inline double computeControlAcceleration(const double state, const double control, const double delta,
+      const double accelerationLimit, const double accelerationGain, const double decelerationLimit,
+      const double decelerationGain)
     {
       FB_DEBUG("---------- FilterBase::computeControlAcceleration ----------\n");
 
@@ -357,7 +359,7 @@ class FilterBase
         gain = decelerationGain;
       }
 
-      const double finalAccel = std::min(std::max(gain * error, -limit), limit);
+      const double finalAccel = std::min(std::max(gain * error / delta, -limit), limit);
 
       FB_DEBUG("Control value: " << control << "\n" <<
                "State value: " << state << "\n" <<

src/filter_base.cpp

@@ -367,7 +367,7 @@ namespace RobotLocalization
         if (controlUpdateVector_[controlInd])
         {
           controlAcceleration_(controlInd) = computeControlAcceleration(state_(controlInd + POSITION_V_OFFSET),
-            (timedOut ? 0.0 : latestControl_(controlInd)), accelerationLimits_[controlInd],
+            (timedOut ? 0.0 : latestControl_(controlInd)), predictionDelta, accelerationLimits_[controlInd],
             accelerationGains_[controlInd], decelerationLimits_[controlInd], decelerationGains_[controlInd]);
         }
       }

src/ukf.cpp

@@ -292,8 +292,9 @@ namespace RobotLocalization
     {
       state_.noalias() += kalmanGainSubset * innovationSubset;
 
-      // (6) Compute the new estimate error covariance P = P - (K * P_zz * K')
-      estimateErrorCovariance_.noalias() -= (kalmanGainSubset * predictedMeasCovar * kalmanGainSubset.transpose());
+      // (6) Compute the new estimate error covariance P = P - (K * ( P_zz + R ) * K')
+      estimateErrorCovariance_.noalias() -= (kalmanGainSubset * (predictedMeasCovar + measurementCovarianceSubset)*
+                                    kalmanGainSubset.transpose());
 
       wrapStateAngles();
 
@@ -453,6 +454,18 @@ namespace RobotLocalization
     transferFunction_(StateMemberVy, StateMemberAy) = delta;
     transferFunction_(StateMemberVz, StateMemberAz) = delta;
 
+    // Apply control terms, which are actually accelerations
+    sigmaPoint(StateMemberVroll) += controlAcceleration_(ControlMemberVroll) * delta;
+    sigmaPoint(StateMemberVpitch) += controlAcceleration_(ControlMemberVpitch) * delta;
+    sigmaPoint(StateMemberVyaw) += controlAcceleration_(ControlMemberVyaw) * delta;
+
+    sigmaPoint(StateMemberAx) = (controlUpdateVector_[ControlMemberVx] ?
+      controlAcceleration_(ControlMemberVx) : sigmaPoint(StateMemberAx));
+    sigmaPoint(StateMemberAy) = (controlUpdateVector_[ControlMemberVy] ?
+      controlAcceleration_(ControlMemberVy) : sigmaPoint(StateMemberAy));
+    sigmaPoint(StateMemberAz) = (controlUpdateVector_[ControlMemberVz] ?
+      controlAcceleration_(ControlMemberVz) : sigmaPoint(StateMemberAz));
+
     sigmaPoint.applyOnTheLeft(transferFunction_);
   }
 }  // namespace RobotLocalization