Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpLinearKalmanFilterInstantiation.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35
41#include <visp3/core/vpDebug.h>
42#include <visp3/core/vpException.h>
43#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
44
45#include <math.h>
46#include <stdlib.h>
47
170void vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal, vpColVector &sigma_state,
171 vpColVector &sigma_measure, double rho, double delta_t)
172{
173 switch (model) {
175 initStateConstVelWithColoredNoise_MeasureVel(n_signal, sigma_state, sigma_measure, rho);
176 break;
178 initStateConstVel_MeasurePos(n_signal, sigma_state, sigma_measure, delta_t);
179 break;
181 initStateConstAccWithColoredNoise_MeasureVel(n_signal, sigma_state, sigma_measure, rho, delta_t);
182 break;
183 case unknown:
184 vpERROR_TRACE("Kalman state model is not set");
185 throw(vpException(vpException::notInitialized, "Kalman state model is not set"));
186 }
187}
188
285 vpColVector &sigma_measure, double delta_t)
286{
287 // init_done = true ;
289
290 init(size_state, size_measure, n_signal);
291
292 iter = 0;
293 Pest = 0;
294 Xest = 0;
295 F = 0;
296 H = 0;
297 R = 0;
298 Q = 0;
299 this->dt = delta_t;
300
301 double dt2 = dt * dt;
302 double dt3 = dt2 * dt;
303
304 for (unsigned int i = 0; i < size_measure * n_signal; i++) {
305 // State model
306 // | 1 dt |
307 // F = | |
308 // | 0 1 |
309
310 F[2 * i][2 * i] = 1;
311 F[2 * i][2 * i + 1] = dt;
312 F[2 * i + 1][2 * i + 1] = 1;
313
314 // Measure model
315 H[i][2 * i] = 1;
316 H[i][2 * i + 1] = 0;
317
318 double sR = sigma_measure[i];
319 double sQ = sigma_state[2 * i]; // sigma_state[2*i+1] is not used
320
321 // Measure noise
322 R[i][i] = sR;
323
324 // State covariance matrix 6.2.2.12
325 Q[2 * i][2 * i] = sQ * dt3 / 3;
326 Q[2 * i][2 * i + 1] = sQ * dt2 / 2;
327 Q[2 * i + 1][2 * i] = sQ * dt2 / 2;
328 Q[2 * i + 1][2 * i + 1] = sQ * dt;
329
330 Pest[2 * i][2 * i] = sR;
331 Pest[2 * i][2 * i + 1] = sR / (2 * dt);
332 Pest[2 * i + 1][2 * i] = sR / (2 * dt);
333 Pest[2 * i + 1][2 * i + 1] = sQ * 2 * dt / 3.0 + sR / (2 * dt2);
334 }
335}
336
491 vpColVector &sigma_state,
492 vpColVector &sigma_measure,
493 double rho)
494{
495 if ((rho < 0) || (rho >= 1)) {
496 vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho);
497 throw(vpException(vpException::badValue, "Bad rho value"));
498 }
499
501
502 init(size_state, size_measure, n_signal);
503
504 iter = 0;
505 Pest = 0;
506 Xest = 0;
507 F = 0;
508 H = 0;
509 R = 0;
510 Q = 0;
511
512 for (unsigned int i = 0; i < size_measure * n_signal; i++) {
513 // State model
514 // | 1 1 |
515 // F = | |
516 // | 0 rho |
517
518 F[2 * i][2 * i] = 1;
519 F[2 * i][2 * i + 1] = 1;
520 F[2 * i + 1][2 * i + 1] = rho;
521
522 // Measure model
523 H[i][2 * i] = 1;
524 H[i][2 * i + 1] = 0;
525
526 double sR = sigma_measure[i];
527 double sQ = sigma_state[2 * i + 1]; // sigma_state[2*i] is not used
528
529 // Measure noise
530 R[i][i] = sR;
531
532 // State covariance matrix
533 Q[2 * i][2 * i] = 0;
534 Q[2 * i][2 * i + 1] = 0;
535 Q[2 * i + 1][2 * i] = 0;
536 Q[2 * i + 1][2 * i + 1] = sQ;
537
538 Pest[2 * i][2 * i] = sR;
539 Pest[2 * i][2 * i + 1] = 0.;
540 Pest[2 * i + 1][2 * i] = 0;
541 Pest[2 * i + 1][2 * i + 1] = sQ / (1 - rho * rho);
542 }
543}
544
711 vpColVector &sigma_state,
712 vpColVector &sigma_measure,
713 double rho, double delta_t)
714{
715 if ((rho < 0) || (rho >= 1)) {
716 vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho);
717 throw(vpException(vpException::badValue, "Bad rho value"));
718 }
720
721 init(size_state, size_measure, n_signal);
722
723 iter = 0;
724 Pest = 0;
725 Xest = 0;
726 F = 0;
727 H = 0;
728 R = 0;
729 Q = 0;
730 this->dt = delta_t;
731 // initialise les matrices decrivant les modeles
732 for (unsigned int i = 0; i < size_measure * nsignal; i++) {
733 // State model
734 // | 1 1 dt |
735 // F = | o rho 0 |
736 // | 0 0 1 |
737
738 F[3 * i][3 * i] = 1;
739 F[3 * i][3 * i + 1] = 1;
740 F[3 * i][3 * i + 2] = dt;
741 F[3 * i + 1][3 * i + 1] = rho;
742 F[3 * i + 2][3 * i + 2] = 1;
743
744 // Measure model
745 H[i][3 * i] = 1;
746 H[i][3 * i + 1] = 0;
747 H[i][3 * i + 2] = 0;
748
749 double sR = sigma_measure[i];
750 double sQ1 = sigma_state[3 * i + 1];
751 double sQ2 = sigma_state[3 * i + 2];
752
753 // Measure noise
754 R[i][i] = sR;
755
756 // State covariance matrix
757 Q[3 * i + 1][3 * i + 1] = sQ1;
758 Q[3 * i + 2][3 * i + 2] = sQ2;
759
760 Pest[3 * i][3 * i] = sR;
761 Pest[3 * i][3 * i + 1] = 0.;
762 Pest[3 * i][3 * i + 2] = sR / dt;
763 Pest[3 * i + 1][3 * i + 1] = sQ1 / (1 - rho * rho);
764 Pest[3 * i + 1][3 * i + 2] = -rho * sQ1 / ((1 - rho * rho) * dt);
765 Pest[3 * i + 2][3 * i + 2] = (2 * sR + sQ1 / (1 - rho * rho)) / (dt * dt);
766 // complete the lower triangle
767 Pest[3 * i + 1][3 * i] = Pest[3 * i][3 * i + 1];
768 Pest[3 * i + 2][3 * i] = Pest[3 * i][3 * i + 2];
769 Pest[3 * i + 2][3 * i + 1] = Pest[3 * i + 1][3 * i + 2];
770 }
771}
772
787{
788 if (nsignal < 1) {
789 vpERROR_TRACE("Bad signal number. You need to initialize the Kalman filter");
790 throw(vpException(vpException::notInitialized, "Bad signal number"));
791 }
792
793 // Specific initialization of the filter that depends on the state model
794 if (iter == 0) {
795 Xest = 0;
796 switch (model) {
800 for (unsigned int i = 0; i < size_measure * nsignal; i++) {
801 Xest[size_state * i] = z[i];
802 }
803 prediction();
804 // init_done = true;
805 break;
806 case unknown:
807 vpERROR_TRACE("Kalman state model is not set");
808 throw(vpException(vpException::notInitialized, "Kalman state model is not set"));
809 }
810 iter++;
811
812 return;
813 } else if (iter == 1) {
815 for (unsigned int i = 0; i < size_measure * nsignal; i++) {
816 double z_prev = Xest[size_state * i]; // Previous mesured position
817 Xest[size_state * i] = z[i];
818 Xest[size_state * i + 1] = (z[i] - z_prev) / dt;
819 }
820 prediction();
821 iter++;
822
823 return;
824 }
825 }
826
827 filtering(z);
828 prediction();
829}
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition vpException.h:86
unsigned int size_state
Size of the state vector .
long iter
Filter step or iteration. When set to zero, initialize the filter.
vpMatrix R
Measurement noise covariance matrix .
unsigned int nsignal
Number of signal to filter.
vpMatrix Q
Process noise covariance matrix .
void filtering(const vpColVector &z)
vpColVector Xest
unsigned int size_measure
Size of the measure vector .
vpMatrix H
Matrix that describes the evolution of the measurements.
void init(unsigned int size_state, unsigned int size_measure, unsigned int n_signal)
void initStateConstVel_MeasurePos(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double dt)
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
void initStateConstAccWithColoredNoise_MeasureVel(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
void initStateConstVelWithColoredNoise_MeasureVel(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho)
#define vpERROR_TRACE
Definition vpDebug.h:388