ViSP  2.8.0
vpLinearKalmanFilterInstantiation.cpp
1 /****************************************************************************
2  *
3  * $Id: vpLinearKalmanFilterInstantiation.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Description:
34  * Kalman filtering.
35  *
36  * Authors:
37  * Eric Marchand
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
47 #include <visp/vpLinearKalmanFilterInstantiation.h>
48 #include <visp/vpDebug.h>
49 #include <visp/vpException.h>
50 
51 #include <math.h>
52 #include <stdlib.h>
53 
176 void
178  vpColVector &sigma_state,
179  vpColVector &sigma_measure,
180  double rho, double dt)
181 {
182  switch (model) {
184  initStateConstVelWithColoredNoise_MeasureVel(nsignal, sigma_state,
185  sigma_measure, rho);
186  break;
188  initStateConstVel_MeasurePos(nsignal, sigma_state,
189  sigma_measure, dt);
190  break;
192  initStateConstAccWithColoredNoise_MeasureVel(nsignal, sigma_state,
193  sigma_measure, rho, dt);
194  break;
195  case unknown:
196  vpERROR_TRACE("Kalman state model is not set") ;
197  throw(vpException(vpException::notInitialized, "Kalman state model is not set")) ;
198  }
199 }
200 
298 void
300  vpColVector &sigma_state,
301  vpColVector &sigma_measure,
302  double dt )
303 {
304  // init_done = true ;
306 
307  init(size_state, size_measure, nsignal);
308 
309  iter = 0;
310  Pest = 0;
311  Xest = 0;
312  F = 0;
313  H = 0;
314  R = 0;
315  Q = 0;
316  this->dt = dt ;
317 
318  double dt2 = dt*dt ;
319  double dt3 = dt2*dt ;
320 
321  for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
322  // State model
323  // | 1 dt |
324  // F = | |
325  // | 0 1 |
326 
327  F[2*i][2*i] = 1 ;
328  F[2*i][2*i+1] = dt ;
329  F[2*i+1][2*i+1] = 1 ;
330 
331  // Measure model
332  H[i][2*i] = 1 ;
333  H[i][2*i+1] = 0 ;
334 
335  double sR = sigma_measure[i] ;
336  double sQ = sigma_state[2*i] ; // sigma_state[2*i+1] is not used
337 
338  // Measure noise
339  R[i][i] = sR ;
340 
341  // State covariance matrix 6.2.2.12
342  Q[2*i][2*i] = sQ * dt3/3;
343  Q[2*i][2*i+1] = sQ * dt2/2;
344  Q[2*i+1][2*i] = sQ * dt2/2;
345  Q[2*i+1][2*i+1] = sQ * dt;
346 
347  Pest[2*i][2*i] = sR ;
348  Pest[2*i][2*i+1] = sR/(2*dt) ;
349  Pest[2*i+1][2*i] = sR/(2*dt) ;
350  Pest[2*i+1][2*i+1] = sQ*2*dt/3.0+ sR/(2*dt2) ;
351  }
352 }
353 
507 void
509  vpColVector &sigma_state,
510  vpColVector &sigma_measure,
511  double rho)
512 {
513  if ((rho < 0) || (rho >= 1)) {
514  vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho) ;
515  throw(vpException(vpException::badValue, "Bad rho value")) ;
516  }
517 
519 
520  init(size_state, size_measure, nsignal);
521 
522  iter = 0;
523  Pest = 0;
524  Xest = 0;
525  F = 0;
526  H = 0;
527  R = 0;
528  Q = 0;
529 
530  for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
531  // State model
532  // | 1 1 |
533  // F = | |
534  // | 0 rho |
535 
536  F[2*i][2*i] = 1 ;
537  F[2*i][2*i+1] = 1 ;
538  F[2*i+1][2*i+1] = rho ;
539 
540  // Measure model
541  H[i][2*i] = 1 ;
542  H[i][2*i+1] = 0 ;
543 
544  double sR = sigma_measure[i] ;
545  double sQ = sigma_state[2*i+1] ; // sigma_state[2*i] is not used
546 
547  // Measure noise
548  R[i][i] = sR ;
549 
550  // State covariance matrix
551  Q[2*i][2*i] = 0 ;
552  Q[2*i][2*i+1] = 0;
553  Q[2*i+1][2*i] = 0;
554  Q[2*i+1][2*i+1] = sQ ;
555 
556  Pest[2*i][2*i] = sR ;
557  Pest[2*i][2*i+1] = 0. ;
558  Pest[2*i+1][2*i] = 0 ;
559  Pest[2*i+1][2*i+1] = sQ/(1-rho*rho) ;
560  }
561 }
562 
730 void
732  vpColVector &sigma_state,
733  vpColVector &sigma_measure,
734  double rho,
735  double dt)
736 {
737  if ((rho < 0) || (rho >= 1)) {
738  vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho) ;
739  throw(vpException(vpException::badValue, "Bad rho value")) ;
740  }
742 
743  init(size_state, size_measure, nsignal);
744 
745  iter = 0;
746  Pest = 0;
747  Xest = 0;
748  F = 0;
749  H = 0;
750  R = 0;
751  Q = 0;
752  this->dt = dt;
753  // initialise les matrices decrivant les modeles
754  for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
755  // State model
756  // | 1 1 dt |
757  // F = | o rho 0 |
758  // | 0 0 1 |
759 
760  F[3*i][3*i] = 1 ;
761  F[3*i][3*i+1] = 1 ;
762  F[3*i][3*i+2] = dt ;
763  F[3*i+1][3*i+1] = rho ;
764  F[3*i+2][3*i+2] = 1 ;
765 
766  // Measure model
767  H[i][3*i] = 1 ;
768  H[i][3*i+1] = 0 ;
769  H[i][3*i+2] = 0 ;
770 
771  double sR = sigma_measure[i] ;
772  double sQ1 = sigma_state[3*i+1] ;
773  double sQ2 = sigma_state[3*i+2] ;
774 
775  // Measure noise
776  R[i][i] = sR ;
777 
778  // State covariance matrix
779  Q[3*i+1][3*i+1] = sQ1;
780  Q[3*i+2][3*i+2] = sQ2;
781 
782  Pest[3*i][3*i] = sR ;
783  Pest[3*i][3*i+1] = 0. ;
784  Pest[3*i][3*i+2] = sR/dt ;
785  Pest[3*i+1][3*i+1] = sQ1/(1-rho*rho) ;
786  Pest[3*i+1][3*i+2] = -rho*sQ1/((1-rho*rho)*dt) ;
787  Pest[3*i+2][3*i+2] = (2*sR+sQ1/(1-rho*rho))/(dt*dt) ;
788  // complete the lower triangle
789  Pest[3*i+1][3*i] = Pest[3*i][3*i+1];
790  Pest[3*i+2][3*i] = Pest[3*i][3*i+2];
791  Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2];
792  }
793 }
794 
808 void
810 {
811  if (nsignal < 1) {
812  vpERROR_TRACE("Bad signal number. You need to initialize the Kalman filter") ;
814  "Bad signal number")) ;
815  }
816 
817  // Specific initialization of the filter that depends on the state model
818  if (iter == 0) {
819  Xest = 0;
820  switch (model) {
824  for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
825  Xest[size_state*i] = z[i] ;
826  }
827  prediction();
828  // init_done = true;
829  break;
830  case unknown:
831  vpERROR_TRACE("Kalman state model is not set") ;
832  throw(vpException(vpException::notInitialized, "Kalman state model is not set")) ;
833  break;
834  }
835  iter ++;
836 
837  return;
838  }
839  else if (iter == 1) {
841  for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
842  double z_prev = Xest[size_state*i]; // Previous mesured position
843  // std::cout << "Mesure pre: " << z_prev << std::endl;
844  Xest[size_state*i] = z[i] ;
845  Xest[size_state*i+1] = (z[i] - z_prev) / dt ;
846  }
847  prediction();
848  iter ++;
849 
850  return;
851  }
852  }
853 
854  filtering(z);
855  prediction();
856 
857 }
void initStateConstAccWithColoredNoise_MeasureVel(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
void filtering(vpColVector &z)
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
#define vpERROR_TRACE
Definition: vpDebug.h:379
long iter
Filter step or iteration. When set to zero, initialize the filter.
unsigned int size_state
Size of the state vector .
vpMatrix R
Measurement noise covariance matrix .
unsigned int size_measure
Size of the measure vector .
error that can be emited by ViSP classes.
Definition: vpException.h:75
void initStateConstVelWithColoredNoise_MeasureVel(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho)
vpColVector Xest
unsigned int nsignal
Number of signal to filter.
void init(unsigned int size_state, unsigned int size_measure, unsigned int nsignal)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void initStateConstVel_MeasurePos(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double dt)
vpMatrix F
Transition matrix that describes the evolution of the state.
vpMatrix H
Matrix that describes the evolution of the measurements.
vpMatrix Q
Process noise covariance matrix .