vpKalmanFilter.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * 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 <visp3/core/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(const 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