Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/sot/core/kalman.hh	Lines:	0	9	0.0 %
Date:	2023-03-13 12:09:37	Branches:	0	6	0.0 %


/*
 * Copyright 2010, 2011, 2012
 * Nicolas Mansard,
 * François Bleibel,
 * Olivier Stasse,
 * Florent Lamiraux
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_KALMAN_H
#define __SOT_KALMAN_H

/* -------------------------------------------------------------------------- */
/* --- INCLUDE -------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/linear-algebra.h>

#include <Eigen/LU>

/* -------------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------------ */
/* -------------------------------------------------------------------------- */

#if defined(WIN32)
#if defined(kalman_EXPORTS)
#define SOT_KALMAN_EXPORT __declspec(dllexport)
#else
#define SOT_KALMAN_EXPORT __declspec(dllimport)
#endif
#else
#define SOT_KALMAN_EXPORT
#endif

/* -------------------------------------------------------------------------- */
/* --- CLASSE --------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

namespace dynamicgraph {
namespace sot {

class SOT_KALMAN_EXPORT Kalman : public Entity {
 public:
  static const std::string CLASS_NAME;
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }

 protected:
  unsigned int size_state;
  unsigned int size_measure;
  double dt;

 public:
  SignalPtr<Vector, int> measureSIN;          // y
  SignalPtr<Matrix, int> modelTransitionSIN;  // F
  SignalPtr<Matrix, int> modelMeasureSIN;     // H
  SignalPtr<Matrix, int> noiseTransitionSIN;  // Q
  SignalPtr<Matrix, int> noiseMeasureSIN;     // R

  SignalPtr<Vector, int> statePredictedSIN;        // x_{k|k-1}
  SignalPtr<Vector, int> observationPredictedSIN;  // y_pred = h (x_{k|k-1})
  SignalTimeDependent<Matrix, int> varianceUpdateSOUT;  // P
  SignalTimeDependent<Vector, int> stateUpdateSOUT;     // X_est

  SignalTimeDependent<Matrix, int> gainSINTERN;        // K
  SignalTimeDependent<Matrix, int> innovationSINTERN;  // S

 public:
  virtual std::string getDocString() const {
    return "Implementation of extended Kalman filter     \n"
           "\n"
           "  Dynamics of the system:                    \n"
           "\n"
           "    x = f (x   , u   ) + w       (state)      \n"
           "     k      k-1   k-1     k-1                 \n"
           "\n"
           "    y = h (x ) + v               (observation)\n"
           "     k      k     k                           \n"
           "\n"
           "  Prediction:\n"
           "\n"
           "    ^          ^                       \n"
           "    x     = f (x       , u   )     (state) \n"
           "     k|k-1      k-1|k-1   k-1          \n"
           "\n"
           "                           T           \n"
           "    P     = F    P        F    + Q (covariance)\n"
           "     k|k-1   k-1  k-1|k-1  k-1         \n"
           "\n"
           "  with\n"
           "           \\                         \n"
           "           d f  ^                         \n"
           "    F    = --- (x       , u   )           \n"
           "     k-1   \\     k-1|k-1   k-1            \n"
           "           d x                         \n"
           "\n"
           "         \\                             \n"
           "         d h  ^                          \n"
           "    H  = --- (x       )                     \n"
           "     k   \\     k-1|k-1                      \n"
           "         d x                             \n"

           "  Update:\n"
           "\n"
           "                ^                            \n"
           "    z = y  - h (x     )             (innovation)\n"
           "     k   k       k|k-1                       \n"
           "                   T                          \n"
           "    S = H  P      H  + R            (innovation covariance)\n"
           "     k   k  k|k-1  k                          \n"
           "                T  -1                         \n"
           "    K = P      H  S                 (Kalman gain)\n"
           "     k   k|k-1  k  k                          \n"
           "    ^     ^                                   \n"
           "    x   = x      + K  z             (state)   \n"
           "     k|k   k|k-1    k  k                      \n"
           "\n"
           "    P   =(I - K  H ) P                        \n"
           "     k|k       k  k   k|k-1                   \n"
           "\n"
           "  Signals\n"
           "    - input(vector)::x_pred:  state prediction\n"
           "                                                         ^\n"
           "    - input(vector)::y_pred:  observation prediction: h (x     )\n"
           "                                                          k|k-1\n"
           "    - input(matrix)::F:       partial derivative wrt x of f\n"
           "    - input(vector)::y:       measure         \n"
           "    - input(matrix)::H:       partial derivative wrt x of h\n"
           "    - input(matrix)::Q:       variance of noise w\n"
           "                                                 k-1\n"
           "    - input(matrix)::R:       variance of noise v\n"
           "                                                 k\n"
           "    - output(matrix)::P_pred: variance of prediction\n"
           "                                               ^\n"
           "    - output(vector)::x_est:  state estimation x\n"
           "                                                k|k\n";
  }

 protected:
  Matrix &computeVarianceUpdate(Matrix &P_k_k, const int &time);
  Vector &computeStateUpdate(Vector &x_est, const int &time);

  void setStateEstimation(const Vector &x0) {
    stateEstimation_ = x0;
    stateUpdateSOUT.recompute(0);
  }

  void setStateVariance(const Matrix &P0) {
    stateVariance_ = P0;
    varianceUpdateSOUT.recompute(0);
  }
  // Current state estimation
  // ^
  // x
  //  k-1|k-1
  Vector stateEstimation_;
  // Variance of current state estimation
  // P
  //  k-1|k-1
  Matrix stateVariance_;

  //                          ^
  // Innovation: z  = y  - H  x
  //              k    k    k  k|k-1
  Vector z_;

  // F    P
  //  k-1  k-1|k-1
  Matrix FP_;

  // Variance prediction
  // P
  //  k|k-1
  Matrix Pk_k_1_;

  // Innovation covariance
  Matrix S_;

  // Kalman Gain
  Matrix K_;

 public:
  Kalman(const std::string &name);
  /* --- Entity --- */
  void display(std::ostream &os) const;
};

}  // namespace sot
}  // namespace dynamicgraph

/*!
  \file Kalman.h
  \brief  Extended kalman filter implementation
*/

#endif


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2010, 2011, 2012
3			* Nicolas Mansard,
4			* François Bleibel,
5			* Olivier Stasse,
6			* Florent Lamiraux
7			*
8			* CNRS/AIST
9			*
10			*/
11
12			#ifndef __SOT_KALMAN_H
13			#define __SOT_KALMAN_H
14
15			/* -------------------------------------------------------------------------- */
16			/* --- INCLUDE -------------------------------------------------------------- */
17			/* -------------------------------------------------------------------------- */
18
19			#include <dynamic-graph/all-signals.h>
20			#include <dynamic-graph/entity.h>
21			#include <dynamic-graph/linear-algebra.h>
22
23			#include <Eigen/LU>
24
25			/* -------------------------------------------------------------------------- */
26			/* --- API ------------------------------------------------------------------ */
27			/* -------------------------------------------------------------------------- */
28
29			#if defined(WIN32)
30			#if defined(kalman_EXPORTS)
31			#define SOT_KALMAN_EXPORT __declspec(dllexport)
32			#else
33			#define SOT_KALMAN_EXPORT __declspec(dllimport)
34			#endif
35			#else
36			#define SOT_KALMAN_EXPORT
37			#endif
38
39			/* -------------------------------------------------------------------------- */
40			/* --- CLASSE --------------------------------------------------------------- */
41			/* -------------------------------------------------------------------------- */
42
43			namespace dynamicgraph {
44			namespace sot {
45
46			class SOT_KALMAN_EXPORT Kalman : public Entity {
47			public:
48			static const std::string CLASS_NAME;
49			virtual const std::string &getClassName(void) const { return CLASS_NAME; }
50
51			protected:
52			unsigned int size_state;
53			unsigned int size_measure;
54			double dt;
55
56			public:
57			SignalPtr<Vector, int> measureSIN; // y
58			SignalPtr<Matrix, int> modelTransitionSIN; // F
59			SignalPtr<Matrix, int> modelMeasureSIN; // H
60			SignalPtr<Matrix, int> noiseTransitionSIN; // Q
61			SignalPtr<Matrix, int> noiseMeasureSIN; // R
62
63			SignalPtr<Vector, int> statePredictedSIN; // x_{k\|k-1}
64			SignalPtr<Vector, int> observationPredictedSIN; // y_pred = h (x_{k\|k-1})
65			SignalTimeDependent<Matrix, int> varianceUpdateSOUT; // P
66			SignalTimeDependent<Vector, int> stateUpdateSOUT; // X_est
67
68			SignalTimeDependent<Matrix, int> gainSINTERN; // K
69			SignalTimeDependent<Matrix, int> innovationSINTERN; // S
70
71			public:
72			virtual std::string getDocString() const {
73			return "Implementation of extended Kalman filter \n"
74			"\n"
75			" Dynamics of the system: \n"
76			"\n"
77			" x = f (x , u ) + w (state) \n"
78			" k k-1 k-1 k-1 \n"
79			"\n"
80			" y = h (x ) + v (observation)\n"
81			" k k k \n"
82			"\n"
83			" Prediction:\n"
84			"\n"
85			" ^ ^ \n"
86			" x = f (x , u ) (state) \n"
87			" k\|k-1 k-1\|k-1 k-1 \n"
88			"\n"
89			" T \n"
90			" P = F P F + Q (covariance)\n"
91			" k\|k-1 k-1 k-1\|k-1 k-1 \n"
92			"\n"
93			" with\n"
94			" \\ \n"
95			" d f ^ \n"
96			" F = --- (x , u ) \n"
97			" k-1 \\ k-1\|k-1 k-1 \n"
98			" d x \n"
99			"\n"
100			" \\ \n"
101			" d h ^ \n"
102			" H = --- (x ) \n"
103			" k \\ k-1\|k-1 \n"
104			" d x \n"
105
106			" Update:\n"
107			"\n"
108			" ^ \n"
109			" z = y - h (x ) (innovation)\n"
110			" k k k\|k-1 \n"
111			" T \n"
112			" S = H P H + R (innovation covariance)\n"
113			" k k k\|k-1 k \n"
114			" T -1 \n"
115			" K = P H S (Kalman gain)\n"
116			" k k\|k-1 k k \n"
117			" ^ ^ \n"
118			" x = x + K z (state) \n"
119			" k\|k k\|k-1 k k \n"
120			"\n"
121			" P =(I - K H ) P \n"
122			" k\|k k k k\|k-1 \n"
123			"\n"
124			" Signals\n"
125			" - input(vector)::x_pred: state prediction\n"
126			" ^\n"
127			" - input(vector)::y_pred: observation prediction: h (x )\n"
128			" k\|k-1\n"
129			" - input(matrix)::F: partial derivative wrt x of f\n"
130			" - input(vector)::y: measure \n"
131			" - input(matrix)::H: partial derivative wrt x of h\n"
132			" - input(matrix)::Q: variance of noise w\n"
133			" k-1\n"
134			" - input(matrix)::R: variance of noise v\n"
135			" k\n"
136			" - output(matrix)::P_pred: variance of prediction\n"
137			" ^\n"
138			" - output(vector)::x_est: state estimation x\n"
139			" k\|k\n";
140			}
141
142			protected:
143			Matrix &computeVarianceUpdate(Matrix &P_k_k, const int &time);
144			Vector &computeStateUpdate(Vector &x_est, const int &time);
145
146			void setStateEstimation(const Vector &x0) {
147			stateEstimation_ = x0;
148			stateUpdateSOUT.recompute(0);
149			}
150
151			void setStateVariance(const Matrix &P0) {
152			stateVariance_ = P0;
153			varianceUpdateSOUT.recompute(0);
154			}
155			// Current state estimation
156			// ^
157			// x
158			// k-1\|k-1
159			Vector stateEstimation_;
160			// Variance of current state estimation
161			// P
162			// k-1\|k-1
163			Matrix stateVariance_;
164
165			// ^
166			// Innovation: z = y - H x
167			// k k k k\|k-1
168			Vector z_;
169
170			// F P
171			// k-1 k-1\|k-1
172			Matrix FP_;
173
174			// Variance prediction
175			// P
176			// k\|k-1
177			Matrix Pk_k_1_;
178
179			// Innovation covariance
180			Matrix S_;
181
182			// Kalman Gain
183			Matrix K_;
184
185			public:
186			Kalman(const std::string &name);
187			/* --- Entity --- */
188			void display(std::ostream &os) const;
189			};
190
191			} // namespace sot
192			} // namespace dynamicgraph
193
194			/*!
195			\file Kalman.h
196			\brief Extended kalman filter implementation
197			*/
198
199			#endif