ExampleIEKF.cpp

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


/** @example ExampleIEKF.cpp
    @relates IteratedExtendedKalman
    @ingroup g_examples
    @ingroup g_stateestimation
    @brief  Example using the iterated extended kalman filter
    @author MIP
*/

#include <StateEstimator/IteratedExtendedKalman.hh>
#include <Base/Math/Random.hh>

#include <fstream>

using namespace std;
using namespace BIAS;
/** \cond HIDDEN_SYMBOLS */
/// specialized kalman filter for example
class OwnKF : public IteratedExtendedKalman {

public:

  OwnKF() : IteratedExtendedKalman(100, 1e-8, true) {
  }

  /// specialization for state transition function
  /// simple one dimesional movement (position + speed)
  virtual void StateTransition(const BIAS::Vector<double>& state,
                               const BIAS::Vector<double>& control,
                               BIAS::Vector<double>& prediction)
  {
    /// newPos = olsPos + deltaT*oldVel
    prediction[0] = state[0] + control[0]*state[1];
    /// newVel = oldVel
    prediction[1] = state[1];
  }

  /// specialization for jacobian matrices for state transition
  virtual void GetTransitionJacobian(const BIAS::Vector<double>& state,
                                     const BIAS::Vector<double>& control,
                                     BIAS::Matrix<double>& transJacobian,
                                     BIAS::Matrix<double>& transErrorJacobian)
  {
    /// delta_StateTransition/delta_state
    transJacobian[0][0] = 1;
    transJacobian[0][1] = control[0];
    transJacobian[1][0] = 0;
    transJacobian[1][1] = 1;

    /// delta_StateTransition/delta_processNoise
    /// process noise additive for position and speed
    transErrorJacobian[0][0] = 1;
    transErrorJacobian[0][1] = 0;
    transErrorJacobian[1][0] = 0;
    transErrorJacobian[1][1] = 1;
  }

  /// specialization for measurement prediction function
  virtual void MeasurementPrediction(const BIAS::Vector<double>& state,
                                     BIAS::Vector<double>& measurePred,
                                     const unsigned int index=1)
  {
    switch(index) {
    case 1:
      /// first observation = sin(position)
      measurePred[0] = state[0]*state[0]*state[0];
      break;
    case 2:
      /// second observation = velocity^3
      measurePred[0] = state[1]*state[1]*state[1]+10*state[1];
      break;
    default:
      BIASERR("Unknown measurement called!");
      BIASABORT;
    }
  }

  /// specialization for measurement prediction jacobian
  virtual void GetMeasureJacobian(const BIAS::Vector<double> &state,
                                  BIAS::Matrix<double> &measJacobian,
                                  BIAS::Matrix<double> &measErrorJacobian,
                                  const unsigned int index=1)
  {
    switch(index) {
    case 1:
      /// first observation = position^3, no velocity -> jac = [3*pos | 0]
      measJacobian[0][0] = 3*state[0];
      measJacobian[0][1] = 0;
      /// additive noise
      measErrorJacobian[0][0] = 1;
      break;
    case 2:
      /// first observation = no position, vel^3 + 10*vel -> jac = [0|3*vel+10]
      measJacobian[0][0] = 0;
      measJacobian[0][1] = 3*state[1]+10;
      /// additive noise
      measErrorJacobian[0][0] = 1;
      break;
    default:
      BIASERR("Unknown measurement called!");
      BIASABORT;
    }
  }
};
/** \endcond */

int main(int argc, char *argv[]) {

  if (argc != 5) {
    cout << "Usage: " << argv[0] 
         << " stateHistFile truePosFile measure1File measure2File!" << endl;
    exit(-1);
  }

  OwnKF estimator;              /// IEKF estimator, see above
  unsigned int simLength = 500; /// length of dataset
  unsigned int rSeed = 0;       /// seed for randomizer

  /// ground truth position
  BIAS::Vector<double> posTrue(simLength);
  /// ground truth velocity
  BIAS::Vector<double> velTrue(simLength);
  /// ground truth for first observation
  vector< BIAS::Vector<double> > measTrue1(simLength);
  /// ground truth for second observation
  vector< BIAS::Vector<double> > measTrue2(simLength);
  /// first noisy observation
  vector< BIAS::Vector<double> > measNoise1(simLength);
  /// second noisy observation
  vector< BIAS::Vector<double> > measNoise2(simLength);
  /// history of estimated state
  vector< BIAS::Vector<double> > stateHist(simLength);
  /// history of estimated state standard deviation
  vector< BIAS::Matrix<double> > covHist(simLength);

  /// set sigma for first observation
  double sigmaM1 = 0.1;
  /// set sigma for second observation
  double sigmaM2 = 0.01;

  /// create process noise covariance
  Matrix<double> Q(2,2);
  Q[0][0] = 0.001; Q[0][1] = 0.0; Q[1][0] = 0.0; Q[1][1] = 1e-5;
  /// create initial state covariance
  Matrix<double> PI(2,2);
  PI[0][0] = 0.001; PI[0][1] = 0.0; PI[1][0] = 0.0; PI[1][1] = 1;
  /// create noise covariance for first observation
  Matrix<double> R_1(1,1); R_1[0][0] = sigmaM1;
  /// create noise covariance for second observation
  Matrix<double> R_2(1,1); R_2[0][0] = sigmaM2;

  /// create randomizer using given seed
  Random rand(rSeed);
  
  //generate ground truth
  for (unsigned int i=0; i<simLength; i++) {
    /// positions = 5 periods of the function 0.25*sin^2+1
    posTrue[i] = sin(10*M_PI*double(i)/double(simLength));
    posTrue[i] = 0.25*posTrue[i]*posTrue[i]+1.0;
    /// velocity = delta_pos/delta_i;
    velTrue[i] = 5.0*M_PI/double(simLength)
      *sin(10*M_PI*double(i)/double(simLength))
      *cos(10*M_PI*double(i)/double(simLength));
    //first observation = sin(position)
    measTrue1[i].newsize(1);
    measTrue1[i][0] = posTrue[i]*posTrue[i]*posTrue[i];
    //second observation = velocity^3
    measTrue2[i].newsize(1);
    measTrue2[i][0] = velTrue[i]*velTrue[i]*velTrue[i]+10*velTrue[i];
    //posTrue[i]*posTrue[i]*posTrue[i];
    //init state history
    stateHist[i].newsize(2);
    covHist[i] = BIAS::Matrix<double>(2,2);
  }

  //generate noisy measurements
  for (unsigned int i=0; i<simLength; i++) {
    measNoise1[i].newsize(1);
    measNoise1[i][0] = measTrue1[i][0] +
                       rand.GetNormalDistributed (0.0, sigmaM1);
    measNoise2[i].newsize(1);
    measNoise2[i][0] = measTrue2[i][0] +
                       rand.GetNormalDistributed (0.0, sigmaM2);
  }

  /// create control vector (deltaT allways 1)
  BIAS::Vector<double> control(1);
  control[0] = 1;

  /// set initial state
  Vector<double> initial(2);
  initial[0] = posTrue[0]; initial[1] = 0.0;
  estimator.Init(initial, PI, Q);

  //start estimation
  for (unsigned int i=0; i<simLength; i++) {
    /// update using first observation
    estimator.Update(measNoise1[i], R_1, 1);
    /// update using second observation
    estimator.Update(measNoise2[i], R_2, 2);
    /// record state history
    estimator.GetPosterioriState(stateHist[i]);
    estimator.GetPosterioriCovariance(covHist[i]);
    /// predict state
    estimator.Predict(control);
  }

  /// write state history and ground truth position to given files
  ofstream estFile(argv[1]);
  ofstream gtFile(argv[2]);
  ofstream meas1File(argv[3]);
  ofstream meas2File(argv[4]);
  for (unsigned int i=0; i < simLength; i++) {
    estFile << stateHist[i][0] << " " << stateHist[i][1] << " "
            << sqrt(covHist[i][0][0]) << sqrt(covHist[i][1][1]) << endl;
    gtFile << posTrue[i] << " " << velTrue[i] << endl;
    meas1File << measNoise1[i][0] << endl;
    meas2File << measNoise2[i][0] << endl;
  }
  estFile.close();
  gtFile.close();
  meas1File.close();
  meas2File.close();

  exit(0);
}