GCC Code Coverage Report

Directory: ./
File: include/sot/core/kalman.hh
Date: 2024-11-13 12:35:17
Exec Total Coverage
Lines: 0 9 0.0%
Branches: 0 6 0.0%
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
200