ExampleExtendedKalman.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 ExampleExtendedKalman.cpp
    @relates ExtendedKalman
    @ingroup g_examples    
    @ingroup g_stateestimation
    @brief  Example using a very simple filter for estimating a constant value. Therefore the state-
      transition-function is the identity (nothing happens during timestep) and
      the measurement-function is a direct observation of the state (identity).
    @author MIP
*/

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

#ifdef WIN32
  #include <Base/Common/W32Compat.hh>
#else //Linux
  #include <sys/time.h>
#endif

namespace BIAS {
    /** \cond HIDDEN_SYMBOLS */
  class MyKalman : public ExtendedKalman {
    
  public:
    MyKalman(){};
    virtual ~MyKalman(){};
    
    virtual void MeasurementFunction(const Vector<double>& state,
                                     Vector<double>& measurePred)
    {
      measurePred = state;
    }
    
    virtual void StateTransition(const Vector<double>& oldState,
                                 const Vector<double>& control,
                                 Vector<double>& newState)
    {
      newState = oldState;
    }
    
  };
  /** \endcond */
}

using namespace std;
using namespace BIAS;

int main(int argc, char ** argv )
{
  if (argc < 2) {
    cout << "Give number of iterations!" << endl;
    exit(0);
  }

  //randomizer for noisy data
  Random random; random.Reset();

  //'real' state to estimate
  double mean1 = 20.0;
  double mean2 = 30.5;

  //filter to use
  MyKalman filter;

  //read the number for maximum iterations to make
  int maxIter = atoi(argv[1]);

  //prepare initial state and cov-matrices
  Vector<double> state(2);
  state[0] = random.GetNormalDistributed(mean1, 2);
  state[1] = random.GetNormalDistributed(mean2, 4);
  Matrix<double> zero(2,2);
  zero.SetZero();
  Matrix<double> cov(2,2);
  cov.SetZero();
  cov[0][0] = 4.0; // = 2^2 = sigma1^1
  cov[1][1] = 16.0;// = 4^2 = simga2^2
  Matrix<double> id(2,2);
  id.SetIdentity();
  //observation and control-params (no ctrlparams needed here -> vec empty)
  Vector<double> observation(2), ctrlparam(0);

  //set initial state and cov (high uncertainty at start)
  filter.SetInitial(state, 100.0*cov);
  //set derivatives for measurement and state-transition
  filter.SetMeasureDerive(id, id); //dh/dx and dh/dv
  filter.SetStateDerive(id, zero); //df/dx and df/dv
  //set cov-matrix for measurements (diagonal matrix with squared std. deviation)
  filter.SetMeasureCov(cov);
  //a constant is estimated so there's no process noise
  filter.SetProcessCov(zero);

  double sum1 = 0, sum2 = 0;

  //do maxIter updates with normal ditributed noise (sigma1 = 2, sigma2 = 4)
  for (int i=0; i<maxIter; i++) {
    //get noisy data (constants + little noise with given sigma)
    observation[0] = 
      random.GetNormalDistributed(mean1, 2);
    sum1 += observation[0];
    observation[1] = 
      random.GetNormalDistributed(mean2, 4);
    sum2 += observation[1];
    //update prediction with observation
    filter.Update(observation);
    //predict state and cov. ahead
    filter.Predict(ctrlparam);
  }

  //get estimated state
  filter.GetState(state);

  //output results...
  //There should be only small diffenrence between the filtered result and the
  //'simple' average computation. This is because we have a stationary system
  //(estimating a constant) and use always the same cov. for updates. This way
  //the Kalman-filter can't take advantage of it's capabilities!
  cout << "should be    1: " << mean1 << "       2: " << mean2 
       << endl << flush;
  cout << "kalman state 1: " << state[0] << "       2: " << state[1] 
       << endl << flush;
  cout << "avarage:     1: " << sum1/double(maxIter) << "       2: " 
       << sum2/double(maxIter) << endl << flush;

  return 0;
}