vpKalmanFilter.cpp

/****************************************************************************
 *
 * $Id: vpKalmanFilter.cpp 5060 2014-12-12 18:31:03Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Kalman filtering.
 *
 * Authors:
 * Eric Marchand
 * Fabien Spindler
 *
 *****************************************************************************/


#include <visp/vpKalmanFilter.h>

#include <math.h>
#include <stdlib.h>

void
vpKalmanFilter::init(unsigned int size_state_vector, unsigned int size_measure_vector,
                     unsigned int n_signal)
{
  this->size_state = size_state_vector;
  this->size_measure = size_measure_vector ;
  this->nsignal = n_signal ;
  F.resize(size_state*nsignal, size_state*nsignal) ;
  H.resize(size_measure*nsignal,  size_state*nsignal) ;

  R.resize(size_measure*nsignal, size_measure*nsignal) ;
  Q.resize(size_state*nsignal, size_state*nsignal) ;

  Xest.resize(size_state*nsignal) ; Xest = 0;
  Xpre.resize(size_state*nsignal) ; Xpre = 0 ;

  Pest.resize(size_state*nsignal, size_state*nsignal) ; Pest = 0 ;

  I.resize(size_state*nsignal, size_state*nsignal) ;
  //  init_done = false ;
  iter = 0 ;
  dt = -1 ;
}

vpKalmanFilter::vpKalmanFilter()
  : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false),
    Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
{
}

vpKalmanFilter::vpKalmanFilter(unsigned int n_signal)
  : iter(0), size_state(0), size_measure(0), nsignal(n_signal), verbose_mode(false),
    Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
{
}

vpKalmanFilter::vpKalmanFilter(unsigned int size_state_vector, unsigned int size_measure_vector, unsigned int n_signal)
  : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false),
    Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
{
  init( size_state_vector, size_measure_vector, n_signal) ;
}

void
vpKalmanFilter::prediction()
{
  if (Xest.getRows() != size_state*nsignal) {
    std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()
          <<" " << size_state*nsignal<<  std::endl ;
    std::cout << " Error : Filter non initialized " << std::endl;
    exit(1) ;
  }

//   if (!init_done) {
//     std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()<<" " << size_state<<  std::endl ;
//     std::cout << " Error : Filter non initialized " << std::endl;
//     exit(1) ;
//     return;
//   }
  
  if (verbose_mode) {
    std::cout << "F = " << std::endl <<  F << std::endl ;
    std::cout << "Xest = "<< std::endl  << Xest << std::endl  ;  
  }
  // Prediction
  // Bar-Shalom  5.2.3.2
  Xpre = F*Xest  ;
  if (verbose_mode) {
    std::cout << "Xpre = "<< std::endl  << Xpre << std::endl  ;
    std::cout << "Q = "<< std::endl  << Q << std::endl  ;  
    std::cout << "Pest " << std::endl << Pest << std::endl ;
  }
  // Bar-Shalom  5.2.3.5
  Ppre = F*Pest*F.t() + Q ;

  // Matrice de covariance de l'erreur de prediction
  if (verbose_mode) 
    std::cout << "Ppre " << std::endl << Ppre << std::endl ;
}

void
vpKalmanFilter::filtering(vpColVector &z)
{
  if (verbose_mode)
    std::cout << "z " << std::endl << z << std::endl ;
  // Bar-Shalom  5.2.3.11
  vpMatrix S =  H*Ppre*H.t() + R ;
  if (verbose_mode)
    std::cout << "S " << std::endl << S << std::endl ;

  W = (Ppre * H.t())* (S).inverseByLU() ;
  if (verbose_mode)
    std::cout << "W " << std::endl << W << std::endl ;
  // Bar-Shalom  5.2.3.15
  Pest = Ppre - W*S*W.t() ;
  if (verbose_mode)
    std::cout << "Pest " << std::endl << Pest << std::endl ;

  if (0) {
    // Bar-Shalom  5.2.3.16
    // numeriquement plus stable que  5.2.3.15
    vpMatrix  Pestinv  ;
    Pestinv = Ppre.inverseByLU()  + H.t() * R.inverseByLU()*H ;
    Pest =   Pestinv.inverseByLU() ;
  }
  // Bar-Shalom  5.2.3.12 5.2.3.13 5.2.3.7
  Xest = Xpre + (W*(z - (H*Xpre))) ;
  if (verbose_mode)
    std::cout << "Xest " << std::endl << Xest << std::endl ;
  
  iter++ ;
}


#if 0


void
vpKalmanFilter::initFilterCteAcceleration(double dt,
                      vpColVector &Z0,
                      vpColVector &Z1,
                      vpColVector &Z2,
                      vpColVector  &sigma_noise,
                      vpColVector &sigma_state )
{
  this->dt = dt ;

  double dt2 = dt*dt ;
  double dt3 = dt2*dt ;
  double dt4 = dt3*dt ;
  double dt5 = dt4*dt ;

  //init_done = true ;

  Pest =0 ;
  // initialise les matrices decrivant les modeles
  for (int i=0;  i < size_measure ;  i++ )
  {
    // modele sur l'etat

    //         | 1  dt  dt2/2 |
    //     F = | 0   1   dt   |
    //         | 0   0    1   |

    F[3*i][3*i] = 1 ;
    F[3*i][3*i+1] = dt ;
    F[3*i][3*i+2] = dt*dt/2 ;
    F[3*i+1][3*i+1] = 1 ;
    F[3*i+1][3*i+2] = dt ;
    F[3*i+2][3*i+2] = 1 ;


    // modele sur la mesure
    H[i][3*i] = 1 ;
    H[i][3*i+1] = 0 ;
    H[i][3*i+2] = 0 ;

    double sR = sigma_noise[i] ;
    double sQ = sigma_state[i] ;

    // bruit de mesure
    R[i][i] = (sR) ;

    // bruit d'etat 6.2.3.9
    Q[3*i  ][3*i  ] =  sQ * dt5/20;
    Q[3*i  ][3*i+1] =  sQ * dt4/8;
    Q[3*i  ][3*i+2] =  sQ * dt3/6 ;

    Q[3*i+1][3*i  ] = sQ * dt4/8 ;
    Q[3*i+1][3*i+1] = sQ * dt3/3 ;
    Q[3*i+1][3*i+2] = sQ * dt2/2 ;

    Q[3*i+2][3*i  ] = sQ * dt3/6 ;
    Q[3*i+2][3*i+1] = sQ * dt2/2.0 ;
    Q[3*i+2][3*i+2] = sQ * dt ;


    // Initialisation pour la matrice de covariance sur l'etat

    Pest[3*i  ][3*i  ] = sR ;
    Pest[3*i  ][3*i+1] = 1.5/dt*sR ;
    Pest[3*i  ][3*i+2] = sR/(dt2) ;

    Pest[3*i+1][3*i  ] = 1.5/dt*sR ;
    Pest[3*i+1][3*i+1] = dt3/3*sQ + 13/(2*dt2)*sR ;
    Pest[3*i+1][3*i+2] = 9*dt2*sQ/40.0 +6/dt3*sR ;

    Pest[3*i+2][3*i  ] = sR/(dt2) ;
    Pest[3*i+2][3*i+1] = 9*dt2*sQ/40.0 +6/dt3*sR ;
    Pest[3*i+2][3*i+2] = 23*dt/30.0*sQ+6.0/dt4*sR ;


    // Initialisation pour l'etat

    Xest[3*i] = Z2[i] ;
    Xest[3*i+1] = ( 1.5 *Z2[i] - Z1[i] -0.5*Z0[i] ) /( 2*dt ) ;
    Xest[3*i+2] = ( Z2[i] - 2*Z1[i] + Z0[i] ) /( dt*dt ) ;

  }
}

void
vpKalmanFilter::initFilterSinger(double dt,
                 double a,
                 vpColVector &Z0,
                 vpColVector &Z1,
                 vpColVector  &sigma_noise,
                 vpColVector &sigma_state )
{
  this->dt = dt ;

  double dt2 = dt*dt ;
  double dt3 = dt2*dt ;

  double a2 = a*a ;
  double a3 = a2*a ;
  double a4 = a3*a ;

  //init_done = true ;

  Pest =0 ;
  // initialise les matrices decrivant les modeles
  for (int i=0;  i < size_measure ;  i++ )
  {
    // modele sur l'etat

    //         | 1  dt  dt2/2 |
    //     F = | 0   1   dt   |
    //         | 0   0    1   |

    F[3*i][3*i] = 1 ;
    F[3*i][3*i+1] = dt ;
    F[3*i][3*i+2] = 1/a2*(1+a*dt+exp(-a*dt)) ;
    F[3*i+1][3*i+1] = 1 ;
    F[3*i+1][3*i+2] = 1/a*(1-exp(-a*dt)) ;
    F[3*i+2][3*i+2] = exp(-a*dt) ;


    // modele sur la mesure
    H[i][3*i] = 1 ;
    H[i][3*i+1] = 0 ;
    H[i][3*i+2] = 0 ;

    double sR = sigma_noise[i] ;
    double sQ = sigma_state[i] ;

    R[i][i] = (sR) ; // bruit de mesure 1.5mm

    Q[3*i  ][3*i  ] =  sQ/a4*(1-exp(-2*a*dt)+2*a*dt+2*a3/3*dt3-2*a2*dt2-4*a*dt*exp(-a*dt) ) ;
    Q[3*i  ][3*i+1] =  sQ/a3*(1+exp(-2*a*dt)-2*exp(-a*dt)+2*a*dt*exp(-a*dt)-2*a*dt+a2*dt2 ) ;
    Q[3*i  ][3*i+2] =  sQ/a2*(1-exp(-2*a*dt)-2*a*dt*exp(-a*dt) ) ;

    Q[3*i+1][3*i  ] =  Q[3*i  ][3*i+1] ;
    Q[3*i+1][3*i+1] = sQ/a2*(4*exp(-a*dt)-3-exp(-2*a*dt)+2*a*dt ) ;
    Q[3*i+1][3*i+2] = sQ/a*(exp(-2*a*dt)+1- 2*exp(-a*dt)) ;

    Q[3*i+2][3*i  ] = Q[3*i  ][3*i+2] ;
    Q[3*i+2][3*i+1] = Q[3*i+1][3*i+2] ;
    Q[3*i+2][3*i+2] = sQ*(1-exp(-2*a*dt) ) ;


    // Initialisation pour la matrice de covariance sur l'etat
    Pest[3*i  ][3*i  ] = sR ;
    Pest[3*i  ][3*i+1] = 1/dt*sR ;
    Pest[3*i  ][3*i+2] = 0 ;

    Pest[3*i+1][3*i  ] = 1/dt*sR ;
    Pest[3*i+1][3*i+1] = 2*sR/dt2 + sQ/(a4*dt2)*(2-a2*dt2+2*a3*dt3/3.0 -2*exp(-a*dt)-2*a*dt*exp(-a*dt));
    Pest[3*i+1][3*i+2] = sQ/(a2*dt)*(exp(-a*dt)+a*dt-1)  ;

    Pest[3*i+2][3*i  ] = 0  ;
    Pest[3*i+2][3*i+1] =  Pest[3*i+1][3*i+2] ;
    Pest[3*i+2][3*i+2] = 0  ;


    // Initialisation pour l'etat

    Xest[3*i]   = Z1[i] ;
    Xest[3*i+1] = ( Z1[i] - Z0[i] ) /(dt ) ;
    Xest[3*i+2] = 0 ;

  }
}

#endif