ExampleKalman.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 ExampleKalman.cpp
    @relates Kalman
    @ingroup g_examples    
    @ingroup g_stateestimation
    @brief  simple example for kalman filter usage
    @author woelk 04/2006 */

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

using namespace BIAS;
using namespace std;

const double meas_noise = 2.0;
const double process_noise = 1.0;

/** computes the next ground truth system state */
Vector<double> Process(const Matrix<double>& A, const Vector<double>& state, 
                       const Matrix<double>& process_covariance)
{
  Vector<double> res;
  res = A * state;
  const int dim = res.Size();
  Vector<double> noise(dim);
  static Random random;
  for (int i=0; i<dim; i++){
    noise[i] = 
      random.GetNormalDistributed(0.0, sqrt(process_covariance[i][i]));
  }
  return res + noise;
}

/** computes noisy measuremenst using the ground truth system state */
void GenerateNoisyMeasurements(const Vector<double>& state, 
                               const Matrix<double>& H,
                               Vector<double>& meas, 
                               Matrix<double>& meas_cov)
{
  Vector<double> gt_meas = H * state;
  static Random random;
  const int meas_dim = meas.Size();
  for (int r=0; r<meas_dim; r++){
    double std_dev = random.GetUniformDistributed(0.5, meas_noise*3);
    meas_cov[r][r] = std_dev * std_dev;
    meas[r] = gt_meas[r] + 
      random.GetNormalDistributed(0.0, std_dev);
    //cout << "meas gt: "<<gt_meas[r]<<" noisy: "<<meas[r]<<endl;
  }

}

int main(int /*argc*/, char ** /*argv*/ )
{
  const int state_dim = 2;
  const int meas_dim = 2;
  // the system matrix
  const Matrix<double> A(state_dim, state_dim, "1.1 0.1 -0.2 1.1");
  // the measurement matrix measurement = H * system_state 
  const Matrix<double> H(meas_dim, state_dim, "1.0 0.0 0.0 1.0");
  //cout << "H: "<<H<<endl;
  //cout << "A: "<<A<<endl;

  Random random;
  Vector<double> pred(state_dim), meas(meas_dim), update(state_dim), 
    state(state_dim);
  Matrix<double> pred_cov(state_dim, state_dim), 
    meas_cov(meas_dim, meas_dim), 
    update_cov(state_dim, state_dim),
    process_covariance(state_dim, state_dim);

  // fill process_covariance randomly
  double std_dev;
  for (int r=0; r<state_dim; r++){
    std_dev = random.GetUniformDistributed(0.5, process_noise);
    process_covariance[r][r] = std_dev * std_dev;
  }

  // initialize the system state and covariance matrix
  pred_cov.SetZero();
  for (int r=0; r<state_dim; r++){
    state[r] = random.GetUniformDistributed(-5.0, 5.0);
    std_dev = random.GetUniformDistributed(0.5, meas_noise);
    pred_cov[r][r] = std_dev * std_dev;
    pred[r] = state[r] + 
      random.GetNormalDistributed(0.0, std_dev);
  }
  cout << "# ground truth system state: " << state
       << "\n# intial system state guess: " << pred 
       << "\n# covariance matrix of initial system state guess: "
       <<pred_cov<<endl;

  Kalman kafi;
  kafi.Init(A, H, pred, pred_cov);

  const int num_cycles=100;
  cout <<"# time\tstate\t\tpred\t\tpred_cov\tmeas\t\tmeas-cov\tup\t\tup-cov\n";
  for (int i=0; i<num_cycles; i++){
    // predict the ground truth
    state = Process(A, state, process_covariance);

    // predict the estimator
    kafi.Predict(process_covariance);
    kafi.GetState(pred);
    kafi.GetCovariance(pred_cov);

    // generate some synthetic measurements
    GenerateNoisyMeasurements(state, H, meas, meas_cov);

    // update
    kafi.Update(meas, meas_cov);
    kafi.GetState(update);
    kafi.GetCovariance(update_cov);

    cout <<i;
    for (int d=0; d<state_dim; d++){
      cout <<"\t"<<state[d] << "\t" << pred[d] 
           <<"\t"<<sqrt(pred_cov[d][d])<< "\t";
      if (d<meas_dim){
        cout << meas[d] << "\t" << sqrt(meas_cov[d][d]) << "\t" ;
      } else {
        cout << "nan" << "\t" << "nan" << "\t";
      }
      cout << update[d] << "\t" 
           << sqrt(update_cov[d][d]);
    }
    cout << endl;
  }

  return 0;
}