UsTK : Ultrasound ToolKit  version 2.0.1 under development (2024-05-20)
usUnscentedKalmanFilter.h
1 /****************************************************************************
2  *
3  * This file is part of the ustk software.
4  * Copyright (C) 2016 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ustk with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * This software was developed at:
17  * Inria Rennes - Bretagne Atlantique
18  * Campus Universitaire de Beaulieu
19  * 35042 Rennes Cedex
20  * France
21  *
22  * If you have questions regarding the use of this file, please contact
23  * Inria at ustk@inria.fr
24  *
25  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
26  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
27  *
28  * Author:
29  * Jason Chevrie
30  *
31  *****************************************************************************/
32 
33 
39 #ifndef __usUnscentedKalmanFilter_h_
40 #define __usUnscentedKalmanFilter_h_
41 
42 #include <visp3/core/vpConfig.h>
43 
44 #include <vector>
45 
46 #include <visp3/core/vpColVector.h>
47 #include <visp3/core/vpMatrix.h>
48 
49 
50 class VISP_EXPORT usUnscentedKalmanFilter
51 {
52 public:
53  enum NoiseType : int {ADDITIVE_NOISE, GENERIC_NOISE};
54  enum SigmaPointGenerationType : int {STANDARD_COVARIANCE, FIXED_SCALING_FACTOR, LIMITED_SPREAD};
55 
56 protected:
57 
58  unsigned int m_stateDimension;
59  unsigned int m_measureDimension;
60  unsigned int m_processNoiseDimension;
61  unsigned int m_measureNoiseDimension;
62 
63  vpColVector m_state;
64  vpMatrix m_stateCovarianceMatrix;
65 
66  NoiseType m_processNoiseType;
67  vpMatrix m_processNoiseCovarianceMatrix;
68  bool m_computeProcessNoiseCovarianceMatrixAutomatically;
69 
70  NoiseType m_measureNoiseType;
71  vpMatrix m_measureNoiseCovarianceMatrix;
72  bool m_computeMeasureNoiseCovarianceMatrixAutomatically;
73 
74  vpColVector m_measure;
75 
76  SigmaPointGenerationType m_sigmaPointsGenerationType;
77  vpMatrix m_sigmaPointsInit;
78  vpMatrix m_sigmaPointsPropagated;
79  vpMatrix m_sigmaPointsMeasure;
80  vpColVector m_sigmaPointsMeanWeights;
81  vpColVector m_sigmaPointsCovarianceWeights;
82  double m_sigmaPointsScalingFactor;
83  double m_sigmaPointsSpreadThreshold;
84 
85  vpColVector m_stateSigmaMean;
86  vpColVector m_measureSigmaMean;
87  vpMatrix m_stateSigmaCovarianceMatrix;
88  vpMatrix m_stateMeasureSigmaCovarianceMatrix;
89  vpMatrix m_measureSigmaCovarianceMatrix;
90 
91 public:
92  usUnscentedKalmanFilter();
93  virtual ~usUnscentedKalmanFilter();
94 
95  int getStateDimension() const;
96  void setStateDimension(int dim);
97 
98  int getMeasureDimension() const;
99  void setMeasureDimension(int dim);
100 
101  NoiseType getProcessNoiseType() const;
102  void setProcessNoiseType(NoiseType type);
103  int getProcessNoiseDimension() const;
104  void setProcessNoiseDimension(int dim);
105 
106  NoiseType getMeasureNoiseType() const;
107  void setMeasureNoiseType(NoiseType type);
108  int getMeasureNoiseDimension() const;
109  void setMeasureNoiseDimension(int dim);
110 
111  SigmaPointGenerationType getSigmaPointGenerationType() const;
112  void setSigmaPointGenerationType(SigmaPointGenerationType type);
113  double getSigmaPointScalingFactor() const;
114  void setSigmaPointScalingFactor(double factor);
115  double getSigmaPointSpreadThreshold() const;
116  void setSigmaPointSpreadThreshold(double threshold);
117 
118  vpColVector getState() const;
119  void setState(const vpColVector &state);
120 
121  vpMatrix getStateCovarianceMatrix() const;
122  void setStateCovarianceMatrix(const vpMatrix &mat);
123 
124  vpMatrix getProcessNoiseCovarianceMatrix() const;
125  void setProcessNoiseCovarianceMatrix(const vpMatrix &cov);
126  bool computeProcessNoiseCovarianceMatrixAutomatically() const;
127  void computeProcessNoiseCovarianceMatrixAutomatically(bool flag);
128 
129  vpMatrix getMeasureNoiseCovarianceMatrix() const;
130  void setMeasureNoiseCovarianceMatrix(const vpMatrix &cov);
131  bool computeMeasureNoiseCovarianceMatrixAutomatically() const;
132  void computeMeasureNoiseCovarianceMatrixAutomatically(bool flag);
133 
134  virtual bool checkConsistency(const vpColVector &measure);
135  virtual void computeProcessNoiseCovarianceMatrix();
136  virtual void computeMeasureNoiseCovarianceMatrix();
137  bool generateSigmaPoints();
138  bool computePropagatedSigmaPoints();
139  bool computeSigmaMeasures();
140  void computeMeansAndCovarianceMatricesFromSigmaPoints();
141  bool updateState();
142 
143  bool filter(const vpColVector &measure);
144 
145  virtual vpColVector propagateSigmaPoint(const vpColVector &sigmaPoint)=0;
146  virtual vpColVector computeMeasureFromSigmaPoint(const vpColVector &sigmaPoint)=0;
147  virtual double stateNorm(const vpColVector &state) const;
148  virtual vpColVector measureLog(const vpColVector &measure, const vpColVector &measureCenter) const;
149  virtual vpColVector stateLog(const vpColVector &state, const vpColVector &stateCenter) const;
150  virtual vpColVector stateExp(const vpColVector &state, const vpColVector &stateCenter) const;
151 };
152 
153 #endif // __usUnscentedKalmanFilter_h_
usUnscentedKalmanFilter::computeMeasureFromSigmaPoint
virtual vpColVector computeMeasureFromSigmaPoint(const vpColVector &sigmaPoint)=0
usUnscentedKalmanFilter::propagateSigmaPoint
virtual vpColVector propagateSigmaPoint(const vpColVector &sigmaPoint)=0
usUnscentedKalmanFilter::m_sigmaPointsGenerationType
SigmaPointGenerationType m_sigmaPointsGenerationType
Definition: usUnscentedKalmanFilter.h:76