IteratedExtendedKalman.cpp

#include "IteratedExtendedKalman.hh"

using namespace std;
using namespace BIAS;

IteratedExtendedKalman::IteratedExtendedKalman()
  : bUseStable_(false), maxIters_(20), iterEps_(1e-4)
{
  //defaults indicating nothing was set
  systemStateDim_ = 0;
  processNoiseDim_ = 0;
}

IteratedExtendedKalman::IteratedExtendedKalman(unsigned int maxIters,
                                               double iterEps,
                                               bool bUseStable)
  : bUseStable_(bUseStable), maxIters_(maxIters), iterEps_(iterEps)
{
  //defaults indicating no state was set
  systemStateDim_ = 0;
  processNoiseDim_ = 0;
}

void IteratedExtendedKalman::Init(const BIAS::Vector<double>& initialState,
                                  const BIAS::Matrix<double>& initialCov,
                                  const BIAS::Matrix<double>& processNoiseCov)
{ 
  statePrior_ = initialState;
  statePost_ = initialState;
  state_ = initialState;

  covariancePrior_ = initialCov; 
  covariancePost_ = initialCov; 
  covariance_ = initialCov;

  Q_ = processNoiseCov;

  processNoiseDim_ = processNoiseCov.num_rows();
  systemStateDim_ = initialState.size();

  Identity_ = Matrix<double>(systemStateDim_, systemStateDim_);
  Identity_.SetIdentity();

  A_.newsize(systemStateDim_, systemStateDim_);
  W_.newsize(systemStateDim_, processNoiseDim_);
}

void IteratedExtendedKalman::Predict(const BIAS::Vector<double>& control)
{
#ifdef BIAS_DEBUG
  BIASASSERT(systemStateDim_ != 0);
  BIASASSERT(processNoiseDim_ != 0);
#endif //BIAS_DEBUG

  //resize jacobians if necessary
  if (((unsigned int) A_.num_rows() != systemStateDim_) || 
      ((unsigned int) A_.num_cols() != systemStateDim_))
    A_.newsize(systemStateDim_, systemStateDim_);
  if (((unsigned int) W_.num_rows() != systemStateDim_) ||
      ((unsigned int) W_.num_cols() != processNoiseDim_))
    W_.newsize(systemStateDim_, processNoiseDim_);

  //get predicted state
  StateTransition(state_, control, statePrior_);
  //get prediction jacobians
  GetTransitionJacobian(state_, control, A_, W_);
  AT_ = A_.Transpose();
  WT_ = W_.Transpose();

#ifdef BIAS_DEBUG
  BIASASSERT((unsigned int) A_.num_cols() == systemStateDim_);
  BIASASSERT(A_.num_cols() == A_.num_rows());
  BIASASSERT(W_.num_cols() == Q_.num_rows());
  BIASASSERT((unsigned int) W_.num_rows() == systemStateDim_);
#endif //BIAS_DEBUG

  //project cov. ahead
  covariancePrior_ = A_ * covariance_ * AT_ + W_ * Q_ * WT_;

  //set actual state
  state_ = statePrior_;
  covariance_ = covariancePrior_;
}

void IteratedExtendedKalman::Update(const BIAS::Vector<double>& measurement,
                                    const BIAS::Matrix<double>& measureNoise,
                                    const unsigned int index)
{
  const unsigned int measurementDim = measurement.size();

#ifdef BIAS_DEBUG
  BIASASSERT(systemStateDim_ != 0);
  BIASASSERT(measurementDim != 0);
  BIASASSERT(measurementDim == (unsigned int)measureNoise.num_cols());
  BIASASSERT(measurementDim == (unsigned int)measureNoise.num_rows());
#endif //BIAS_DEBUG

  //resize matrices if necessary
  if (((unsigned int) H_.num_rows() != measurementDim) ||
      ((unsigned int) H_.num_cols() != systemStateDim_))
    H_.newsize(measurementDim,systemStateDim_);
  if (((unsigned int) V_.num_rows() != measurementDim)||
      (V_.num_cols() != measureNoise.num_rows()))
    V_.newsize(measurementDim,measureNoise.num_rows());
  if ((unsigned int) Identity_.num_cols() != systemStateDim_) {
    Identity_.newsize(systemStateDim_, systemStateDim_);
    Identity_.SetIdentity();
  }

  //temporary vars for update iteration...
  //copy state for iteration
  BIAS::Vector<double> state_iter(state_);
  //predicted measurement
  BIAS::Vector<double> measurePred(measurementDim);
  //difference of iterated and start state
  BIAS::Vector<double> delta_state(systemStateDim_);
  //update for single iteration
  BIAS::Vector<double> update(systemStateDim_);
  //kalman gain matrix
  BIAS::Matrix<double> K(systemStateDim_, measurementDim);
  //inverse of (H_*covariance_*HT_ + V_*measureNoise*VT_)
  BIAS::Matrix<double> invTmp(measurementDim, measurementDim);

  //cout << "iteration: ";

  for (unsigned int i=0; i<maxIters_; i++) {
    delta_state = state_ - state_iter;

    //get jacobians for observation model
    GetMeasureJacobian(state_iter, H_, V_, index);
    HT_ = H_.Transpose();
    VT_ = V_.Transpose();

    //create matrix for inversion of (H_*covariance_*HT_ + V_*measureNoise*VT_)
    invTmp = H_ * covariance_ * HT_ + V_ * measureNoise * VT_;
    invTmp = svd_.Invert(invTmp);
    //compute Kalman gain
    K = covariance_ * HT_ * invTmp;

    //update iteration state estimate...
    //call virtual measurement-function
    MeasurementPrediction(state_iter, measurePred, index);
    //correct iteration state with measurement
    update = delta_state + K*(measurement - measurePred - H_*delta_state);
    state_iter = state_iter + update;

    //cout << i << " ";

    //stop iterating if change is sufficient small
    if (update.NormL2() < iterEps_) break;
  }
  
  //cout << endl;

  //update error-covariance
  if (bUseStable_) {
    BIAS::Matrix<double> IKH = (Identity_ - K * H_);
    covariance_ = IKH * covariance_ * IKH.Transpose()
                  + K*measureNoise*K.Transpose();
  } else {
    covariance_ = (Identity_ - K * H_) * covariance_;
  }

  state_ = state_iter;
  statePost_ = state_;
  covariancePost_ = covariance_;
}