Visual Servoing Platform  version 3.1.0
vpLinearKalmanFilterInstantiation.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Kalman filtering.
33  *
34  * Authors:
35  * Eric Marchand
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
45 #include <visp3/core/vpDebug.h>
46 #include <visp3/core/vpException.h>
47 #include <visp3/core/vpLinearKalmanFilterInstantiation.h>
48 
49 #include <math.h>
50 #include <stdlib.h>
51 
174 void vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal, vpColVector &sigma_state,
175  vpColVector &sigma_measure, double rho, double delta_t)
176 {
177  switch (model) {
179  initStateConstVelWithColoredNoise_MeasureVel(n_signal, sigma_state, sigma_measure, rho);
180  break;
182  initStateConstVel_MeasurePos(n_signal, sigma_state, sigma_measure, delta_t);
183  break;
185  initStateConstAccWithColoredNoise_MeasureVel(n_signal, sigma_state, sigma_measure, rho, delta_t);
186  break;
187  case unknown:
188  vpERROR_TRACE("Kalman state model is not set");
189  throw(vpException(vpException::notInitialized, "Kalman state model is not set"));
190  }
191 }
192 
289  vpColVector &sigma_measure, double delta_t)
290 {
291  // init_done = true ;
293 
294  init(size_state, size_measure, n_signal);
295 
296  iter = 0;
297  Pest = 0;
298  Xest = 0;
299  F = 0;
300  H = 0;
301  R = 0;
302  Q = 0;
303  this->dt = delta_t;
304 
305  double dt2 = dt * dt;
306  double dt3 = dt2 * dt;
307 
308  for (unsigned int i = 0; i < size_measure * n_signal; i++) {
309  // State model
310  // | 1 dt |
311  // F = | |
312  // | 0 1 |
313 
314  F[2 * i][2 * i] = 1;
315  F[2 * i][2 * i + 1] = dt;
316  F[2 * i + 1][2 * i + 1] = 1;
317 
318  // Measure model
319  H[i][2 * i] = 1;
320  H[i][2 * i + 1] = 0;
321 
322  double sR = sigma_measure[i];
323  double sQ = sigma_state[2 * i]; // sigma_state[2*i+1] is not used
324 
325  // Measure noise
326  R[i][i] = sR;
327 
328  // State covariance matrix 6.2.2.12
329  Q[2 * i][2 * i] = sQ * dt3 / 3;
330  Q[2 * i][2 * i + 1] = sQ * dt2 / 2;
331  Q[2 * i + 1][2 * i] = sQ * dt2 / 2;
332  Q[2 * i + 1][2 * i + 1] = sQ * dt;
333 
334  Pest[2 * i][2 * i] = sR;
335  Pest[2 * i][2 * i + 1] = sR / (2 * dt);
336  Pest[2 * i + 1][2 * i] = sR / (2 * dt);
337  Pest[2 * i + 1][2 * i + 1] = sQ * 2 * dt / 3.0 + sR / (2 * dt2);
338  }
339 }
340 
495  vpColVector &sigma_state,
496  vpColVector &sigma_measure,
497  double rho)
498 {
499  if ((rho < 0) || (rho >= 1)) {
500  vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho);
501  throw(vpException(vpException::badValue, "Bad rho value"));
502  }
503 
505 
506  init(size_state, size_measure, n_signal);
507 
508  iter = 0;
509  Pest = 0;
510  Xest = 0;
511  F = 0;
512  H = 0;
513  R = 0;
514  Q = 0;
515 
516  for (unsigned int i = 0; i < size_measure * n_signal; i++) {
517  // State model
518  // | 1 1 |
519  // F = | |
520  // | 0 rho |
521 
522  F[2 * i][2 * i] = 1;
523  F[2 * i][2 * i + 1] = 1;
524  F[2 * i + 1][2 * i + 1] = rho;
525 
526  // Measure model
527  H[i][2 * i] = 1;
528  H[i][2 * i + 1] = 0;
529 
530  double sR = sigma_measure[i];
531  double sQ = sigma_state[2 * i + 1]; // sigma_state[2*i] is not used
532 
533  // Measure noise
534  R[i][i] = sR;
535 
536  // State covariance matrix
537  Q[2 * i][2 * i] = 0;
538  Q[2 * i][2 * i + 1] = 0;
539  Q[2 * i + 1][2 * i] = 0;
540  Q[2 * i + 1][2 * i + 1] = sQ;
541 
542  Pest[2 * i][2 * i] = sR;
543  Pest[2 * i][2 * i + 1] = 0.;
544  Pest[2 * i + 1][2 * i] = 0;
545  Pest[2 * i + 1][2 * i + 1] = sQ / (1 - rho * rho);
546  }
547 }
548 
715  vpColVector &sigma_state,
716  vpColVector &sigma_measure,
717  double rho, double delta_t)
718 {
719  if ((rho < 0) || (rho >= 1)) {
720  vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho);
721  throw(vpException(vpException::badValue, "Bad rho value"));
722  }
724 
725  init(size_state, size_measure, n_signal);
726 
727  iter = 0;
728  Pest = 0;
729  Xest = 0;
730  F = 0;
731  H = 0;
732  R = 0;
733  Q = 0;
734  this->dt = delta_t;
735  // initialise les matrices decrivant les modeles
736  for (unsigned int i = 0; i < size_measure * nsignal; i++) {
737  // State model
738  // | 1 1 dt |
739  // F = | o rho 0 |
740  // | 0 0 1 |
741 
742  F[3 * i][3 * i] = 1;
743  F[3 * i][3 * i + 1] = 1;
744  F[3 * i][3 * i + 2] = dt;
745  F[3 * i + 1][3 * i + 1] = rho;
746  F[3 * i + 2][3 * i + 2] = 1;
747 
748  // Measure model
749  H[i][3 * i] = 1;
750  H[i][3 * i + 1] = 0;
751  H[i][3 * i + 2] = 0;
752 
753  double sR = sigma_measure[i];
754  double sQ1 = sigma_state[3 * i + 1];
755  double sQ2 = sigma_state[3 * i + 2];
756 
757  // Measure noise
758  R[i][i] = sR;
759 
760  // State covariance matrix
761  Q[3 * i + 1][3 * i + 1] = sQ1;
762  Q[3 * i + 2][3 * i + 2] = sQ2;
763 
764  Pest[3 * i][3 * i] = sR;
765  Pest[3 * i][3 * i + 1] = 0.;
766  Pest[3 * i][3 * i + 2] = sR / dt;
767  Pest[3 * i + 1][3 * i + 1] = sQ1 / (1 - rho * rho);
768  Pest[3 * i + 1][3 * i + 2] = -rho * sQ1 / ((1 - rho * rho) * dt);
769  Pest[3 * i + 2][3 * i + 2] = (2 * sR + sQ1 / (1 - rho * rho)) / (dt * dt);
770  // complete the lower triangle
771  Pest[3 * i + 1][3 * i] = Pest[3 * i][3 * i + 1];
772  Pest[3 * i + 2][3 * i] = Pest[3 * i][3 * i + 2];
773  Pest[3 * i + 2][3 * i + 1] = Pest[3 * i + 1][3 * i + 2];
774  }
775 }
776 
791 {
792  if (nsignal < 1) {
793  vpERROR_TRACE("Bad signal number. You need to initialize the Kalman filter");
794  throw(vpException(vpException::notInitialized, "Bad signal number"));
795  }
796 
797  // Specific initialization of the filter that depends on the state model
798  if (iter == 0) {
799  Xest = 0;
800  switch (model) {
804  for (unsigned int i = 0; i < size_measure * nsignal; i++) {
805  Xest[size_state * i] = z[i];
806  }
807  prediction();
808  // init_done = true;
809  break;
810  case unknown:
811  vpERROR_TRACE("Kalman state model is not set");
812  throw(vpException(vpException::notInitialized, "Kalman state model is not set"));
813  break;
814  }
815  iter++;
816 
817  return;
818  } else if (iter == 1) {
820  for (unsigned int i = 0; i < size_measure * nsignal; i++) {
821  double z_prev = Xest[size_state * i]; // Previous mesured position
822  // std::cout << "Mesure pre: " << z_prev << std::endl;
823  Xest[size_state * i] = z[i];
824  Xest[size_state * i + 1] = (z[i] - z_prev) / dt;
825  }
826  prediction();
827  iter++;
828 
829  return;
830  }
831  }
832 
833  filtering(z);
834  prediction();
835 }
void initStateConstAccWithColoredNoise_MeasureVel(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
long iter
Filter step or iteration. When set to zero, initialize the filter.
#define vpERROR_TRACE
Definition: vpDebug.h:393
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:71
void initStateConstVelWithColoredNoise_MeasureVel(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho)
vpColVector Xest
void filtering(const vpColVector &z)
unsigned int nsignal
Number of signal to filter.
void init(unsigned int size_state, unsigned int size_measure, unsigned int n_signal)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void initStateConstVel_MeasurePos(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double dt)
vpMatrix H
Matrix that describes the evolution of the measurements.
vpMatrix Q
Process noise covariance matrix .