Biorbd Release_1.13.0
Loading...
Searching...
No Matches
KalmanRecons.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/KalmanRecons.h"
3
4#include "BiorbdModel.h"
5#include "RigidBody/GeneralizedAcceleration.h"
6#include "RigidBody/GeneralizedCoordinates.h"
7#include "RigidBody/GeneralizedVelocity.h"
8#include "Utils/Matrix.h"
9#include "Utils/Vector.h"
10
11using namespace BIORBD_NAMESPACE;
12
14 : m_params(std::make_shared<KalmanParam>()),
15 m_Te(std::make_shared<double>(1.0 / (m_params->acquisitionFrequency()))),
16 m_nbDof(std::make_shared<size_t>()),
17 m_nMeasure(std::make_shared<size_t>()),
18 m_xp(std::make_shared<utils::Vector>()),
19 m_A(std::make_shared<utils::Matrix>()),
20 m_Q(std::make_shared<utils::Matrix>()),
21 m_R(std::make_shared<utils::Matrix>()),
22 m_Pp(std::make_shared<utils::Matrix>()) {}
23
25 Model &model,
26 size_t nbMeasure,
27 KalmanParam params)
28 : m_params(std::make_shared<KalmanParam>(params)),
29 m_Te(std::make_shared<double>(1.0 / (m_params->acquisitionFrequency()))),
30 m_nbDof(std::make_shared<size_t>(model.dof_count)),
31 m_nMeasure(std::make_shared<size_t>(nbMeasure)),
32 m_xp(std::make_shared<utils::Vector>()),
33 m_A(std::make_shared<utils::Matrix>()),
34 m_Q(std::make_shared<utils::Matrix>()),
35 m_R(std::make_shared<utils::Matrix>()),
36 m_Pp(std::make_shared<utils::Matrix>()) {}
37
39
41 *m_params = *other.m_params;
42 *m_Te = *other.m_Te;
43 *m_nbDof = *other.m_nbDof;
44 *m_nMeasure = *other.m_nMeasure;
45 *m_xp = *other.m_xp;
46 *m_A = *other.m_A;
47 *m_Q = *other.m_Q;
48 *m_R = *other.m_R;
49 *m_Pp = *other.m_Pp;
50}
51
53 utils::Vector measure,
54 const utils::Vector &projectedMeasure,
55 const utils::Matrix &Hessian,
56 const std::vector<size_t> &occlusion) {
57 // Prediction
58 const utils::Vector &xkm(*m_A * *m_xp);
59 const utils::Matrix &Pkm(*m_A * *m_Pp * m_A->transpose() + *m_Q);
60
61 // Correction
62 utils::Matrix InvTp((Hessian * Pkm * Hessian.transpose() + *m_R).inverse());
63 manageOcclusionDuringIteration(InvTp, measure, occlusion);
64 const utils::Matrix &K(Pkm * Hessian.transpose() * InvTp); // Gain
65
66 *m_xp = xkm + K * (measure - projectedMeasure); // New estimated state
67 const RigidBodyDynamics::Math::MatrixNd &temp(
68 utils::Matrix::Identity(3 * *m_nbDof, 3 * *m_nbDof) - K * Hessian);
69 *m_Pp = temp * Pkm * temp.transpose() + K * *m_R * K.transpose();
70}
71
73 utils::Matrix &InvTp,
74 utils::Vector &measure,
75 const std::vector<size_t> &occlusion) {
76 for (size_t i = 0; i < occlusion.size(); ++i)
77 for (size_t j = occlusion[i]; j < occlusion[i] + 1; ++j) {
78 InvTp(j, j) = 0; // Artifact due to the fact that m_R has a value
79 // at (j:j+2,j:j+2)
80 measure(j) = 0;
81 }
82}
83
88 if (Q != nullptr) {
89 *Q = m_xp->block(0, 0, *m_nbDof, 1);
90 }
91
92 if (Qdot != nullptr) {
93 *Qdot = m_xp->block(*m_nbDof, 0, *m_nbDof, 1);
94 }
95
96 if (Qddot != nullptr) {
97 *Qddot = m_xp->block(2 * *m_nbDof, 0, *m_nbDof, 1);
98 }
99}
100
102rigidbody::KalmanRecons::evolutionMatrix(const size_t nQ, size_t n, double Te) {
103 // m : number of degrees of freedom
104 // n : order of Taylor development
105 // Te : 1 / (acquisition frequency)
106
107 n += 1;
108 utils::Matrix A(utils::Matrix::Identity(nQ * n, nQ * n));
109 double c = 1;
110 for (size_t i = 2; i < n + 1; ++i) {
111 size_t j = (i - 1) * nQ;
112 c = c / (i - 1);
113
114 for (size_t cmp = 0; cmp < nQ * n - j; ++cmp) {
115 A(0 + cmp, j + cmp) +=
116 c * static_cast<double>(std::pow(Te, (static_cast<double>(i) - 1.0)));
117 }
118 }
119
120 return A;
121}
122
124 const size_t nbQ,
125 double Te) {
126 // Find the coefficients values
127 double c1 = 1.0 / 20.0 * pow(Te, 5);
128 double c2 = 1.0 / 8.0 * pow(Te, 4);
129 double c3 = 1.0 / 6.0 * pow(Te, 3);
130 double c4 = 1.0 / 3.0 * pow(Te, 3);
131 double c5 = 1.0 / 2.0 * pow(Te, 2);
132 double c6 = Te;
133
134 // Ouput matrix
135 utils::Matrix Q(utils::Matrix::Zero(3 * nbQ, 3 * nbQ));
136 for (size_t j = 0; j < nbQ; ++j) {
137 Q(j, j) = c1;
138 Q(j, nbQ + j) = c2;
139 Q(j, 2 * nbQ + j) = c3;
140 Q(nbQ + j, j) = c2;
141 Q(nbQ + j, nbQ + j) = c4;
142 Q(nbQ + j, 2 * nbQ + j) = c5;
143 Q(2 * nbQ + j, j) = c3;
144 Q(2 * nbQ + j, nbQ + j) = c5;
145 Q(2 * nbQ + j, 2 * nbQ + j) = c6;
146 }
147
148 return Q;
149}
150
151// Methods during the initialization
153 // Declaration of the evolution matrix
155
156 // Process Noise Matrix
158
159 // Matrix of the noise on the measurement
160 *m_R = measurementNoiseMatrix(*m_nMeasure, m_params->noiseFactor());
161
162 // Initialize the state
164
165 // Matrix Pp
166 *m_Pp = initCovariance(*m_nbDof, m_params->errorFactor());
167}
168
170 const size_t nbT,
171 double val) {
172 utils::Matrix R(utils::Matrix::Zero(nbT, nbT));
173 for (size_t i = 0; i < nbT; ++i) {
174 R(i, i) = val;
175 }
176 return R;
177}
178
180 const size_t nbQ) {
181 return rigidbody::GeneralizedCoordinates::Zero(3 * nbQ); // Q, Qdot, Qddot
182}
183
188 if (Q != nullptr) {
189 m_xp->block(0, 0, *m_nbDof, 1) = *Q;
190 }
191
192 if (Qdot != nullptr) {
193 m_xp->block(*m_nbDof, 0, *m_nbDof, 1) = *Qdot;
194 }
195
196 if (Qddot != nullptr) {
197 m_xp->block(2 * *m_nbDof, 0, *m_nbDof, 1) = *Qddot;
198 }
199}
200
202 const size_t nbQ,
203 double val) {
204 utils::Matrix Pp(utils::Matrix::Zero(3 * nbQ, 3 * nbQ));
205 for (size_t i = 0; i < 3 * nbQ; ++i) {
206 Pp(i, i) = val;
207 }
208 return Pp;
209}
210
212 double frequency,
213 double noiseFactor,
214 double errorFactor)
215 : m_acquisitionFrequency(frequency),
216 m_noiseFactor(noiseFactor),
217 m_errorFactor(errorFactor) {}
218
220 return m_acquisitionFrequency;
221}
222
223double rigidbody::KalmanParam::noiseFactor() const { return m_noiseFactor; }
224
225double rigidbody::KalmanParam::errorFactor() const { return m_errorFactor; }
The actual musculoskeletal model that holds everything in biorbd.
Parameters of the reconstruction.
double errorFactor() const
Return the error factor.
double acquisitionFrequency() const
Return the acquisition frequency.
double noiseFactor() const
Return the noise factor.
KalmanParam(double frequency=100, double noiseFactor=1e-10, double errorFactor=1e-5)
Set the Kalman filter parameters.
Class Kinematic reconstruction algorithm using an Extended Kalman Filter.
virtual void manageOcclusionDuringIteration(utils::Matrix &InvTp, utils::Vector &measure, const std::vector< size_t > &occlusion)
Manage the occlusion during the iteration.
std::shared_ptr< utils::Matrix > m_Pp
Covariance matrix.
void iteration(utils::Vector measure, const utils::Vector &projectedMeasure, const utils::Matrix &Hessian, const std::vector< size_t > &occlusion=std::vector< size_t >())
Compute an iteration of the Kalman filter.
std::shared_ptr< size_t > m_nMeasure
Number of measurements.
utils::Matrix measurementNoiseMatrix(const size_t nbT, double val)
Matrix of the noise on the measurements.
GeneralizedCoordinates initState(const size_t nbQ)
Initialize the states.
std::shared_ptr< size_t > m_nbDof
Number of states.
utils::Matrix initCovariance(const size_t nbQ, double val)
Returns a initialized covianriance matrix.
virtual void initialize()
Initialization of the filter.
virtual ~KalmanRecons()
Destroy class properly.
void DeepCopy(const KalmanRecons &other)
Deep copy of Kalman reconstruction.
std::shared_ptr< utils::Vector > m_xp
State vector.
utils::Matrix processNoiseMatrix(const size_t nbQ, double Te)
Process the noise matrix.
void getState(GeneralizedCoordinates *Q=nullptr, GeneralizedVelocity *Qdot=nullptr, GeneralizedAcceleration *Qddot=nullptr)
Get the state (Q, Qdot, Qddot).
std::shared_ptr< KalmanParam > m_params
The parameters of the Kalman filter.
std::shared_ptr< utils::Matrix > m_Q
Noise matrix.
void setInitState(const GeneralizedCoordinates *Q=nullptr, const GeneralizedVelocity *Qdot=nullptr, const GeneralizedAcceleration *Qddot=nullptr)
Set the initial guess of the reconstruction.
std::shared_ptr< utils::Matrix > m_A
Evolution matrix.
utils::Matrix evolutionMatrix(const size_t m, size_t n, double Te)
Create the evolution matrix.
std::shared_ptr< double > m_Te
Inherent parameter to the frequency.
std::shared_ptr< utils::Matrix > m_R
Matrix of the noise on the measurements.
A wrapper for the Eigen::MatrixXd.
Definition Matrix.h:18
Wrapper of the Eigen VectorXd.
Definition Vector.h:21