ExtendedKalman.cpp

#include "ExtendedKalman.hh"

using namespace std;
using namespace BIAS;

ExtendedKalman::ExtendedKalman()
{
  //defaults indicating nothing set
  systemStateDim_ = 0;
  measurementDim_ = 0;
}

int ExtendedKalman::Predict(const Vector<double>& control)
{
#ifdef BIAS_DEBUG
  if (systemStateDim_ == 0) {
    BIASERR("Either state- or measurement-dimension is zero!!!");
    return -1;
  }
#endif //BIAS_DEBUG

  //project the state ahead
  StateTransition(posterioState_, control, priorState_);
  
  //project cov. ahead
  covariance_ = A_ * covariance_ * AT_ + W_ * Q_ * WT_;

  return 0;
}

int ExtendedKalman::Update(const Vector<double>& measurement)
{
  measurementDim_ = measurement.size();

#ifdef BIAS_DEBUG
  if ((systemStateDim_ == 0)||(measurementDim_ == 0)) {
    BIASERR("Either state- or measurement-dimension is zero!!!");
    return -1;
  }
#endif //BIAS_DEBUG
  
  Identity_.clear();
  Identity_ = Matrix<double>(systemStateDim_, systemStateDim_);
  Identity_.SetIdentity();

  //compute Kalman gain
  {
    //create temporary matrix for inversion of (H_*covariance_*HT_ + V_*R_*VT_)
    Matrix<double> invTmp(measurementDim_, measurementDim_);//systemStateDim_, measurementDim_);
    
    invTmp = H_ * covariance_ * HT_ + V_ * R_ * VT_;
    invTmp = svd_.Invert(invTmp);
    
    K_ = covariance_ * HT_ * invTmp;
  }

  //update state estimate
  {
    Vector<double> measurePred(measurementDim_);
    //call virtual measurement-function
    MeasurementFunction(priorState_, measurePred);
    //correct state with measurement
    lastKalmanUpdate_ = K_ * (measurement - measurePred);
    posterioState_ = priorState_ + lastKalmanUpdate_;
  }

  //update error-covariance
  {
    covariance_ = (Identity_ - K_ * H_) * covariance_;
  }

  return 0;
}