Kalman.cpp

#include <StateEstimator/Kalman.hh>

#include <Base/Common/BIASpragma.hh>

using namespace BIAS;
using namespace std;



Kalman::Kalman()
{
  SystemStateDim_=MeasurementDim_=0;
}

Kalman::Kalman(const Matrix<double>& SystemMatrix,
               const Matrix<double>& MeasurementMatrix,
               const Vector<double>& InitialSystemState,
               const Matrix<double>& InitialSystemCovariance)
{
  Init(SystemMatrix, MeasurementMatrix, InitialSystemState,
       InitialSystemCovariance);
}

Kalman::~Kalman()
{}

void Kalman::Init(const Matrix<double>& SystemMatrix,
                  const Matrix<double>& MeasurementMatrix,
                  const Vector<double>& InitialSystemState,
                  const Matrix<double>& InitialSystemCovariance)
{
  State_ = InitialSystemState;
  Covariance_ = InitialSystemCovariance;
  SystemStateDim_ = InitialSystemState.size();
  MeasurementDim_ = MeasurementMatrix.num_rows();
  
  A_ = SystemMatrix;
  AT_ = A_.Transpose();
  H_ = MeasurementMatrix;
  HT_ = H_.Transpose();

  Identity_.newsize(SystemStateDim_, SystemStateDim_);
  Identity_.SetIdentity();

  BIASASSERT(A_.num_cols()==SystemStateDim_);
  BIASASSERT(A_.num_rows()==SystemStateDim_);

  BIASASSERT(H_.num_cols()==SystemStateDim_);
}

void Kalman::Predict(const Matrix<double>& ProcessCovariance)
{
  BIASASSERT(A_.num_cols()!=0);
  BIASASSERT(A_.num_rows()!=0);
  State_ = A_ * State_;
  Covariance_ = A_ * Covariance_ * AT_ + ProcessCovariance;
}

void Kalman::Predict(const Matrix<double>& SystemMatrix,
                     const Matrix<double>& ProcessCovariance)
{
  A_=SystemMatrix;

  BIASASSERT(A_.num_cols()==SystemStateDim_);
  BIASASSERT(A_.num_rows()==SystemStateDim_);

  AT_=A_.Transpose();

  Predict(ProcessCovariance);
}

void Kalman::Update(const Vector<double>& Measurement, 
                    const Matrix<double>& MeasurementCovariance)
{
  BIASASSERT(Measurement.size()==MeasurementDim_);
  BIASASSERT(MeasurementCovariance.num_cols()==MeasurementDim_);
  BIASASSERT(MeasurementCovariance.num_rows()==MeasurementDim_);

  Matrix<double> denominator;
  denominator = H_ * Covariance_ * HT_ + MeasurementCovariance;
  Matrix<double> inv;
  inv = svd_.Invert(denominator); 

  Matrix<double> K;
  K = Covariance_ * HT_ * inv;
  
  State_ = State_ + K*(Measurement - H_*State_);

  Covariance_ = (Identity_ - K * H_) * Covariance_;
}