Biorbd Release_1.13.0
Loading...
Searching...
No Matches
KalmanRecons.h
1#ifndef BIORBD_RIGIDBODY_KALMAN_RECONS_H
2#define BIORBD_RIGIDBODY_KALMAN_RECONS_H
3
4#include "biorbdConfig.h"
5
6#include <memory>
7#include <vector>
8
9namespace BIORBD_NAMESPACE {
10class Model;
11
12namespace utils {
13class Matrix;
14class Vector;
15} // namespace utils
16
17namespace rigidbody {
21
26 public:
34 double frequency = 100,
35 double noiseFactor = 1e-10,
36 double errorFactor = 1e-5);
37
42 double acquisitionFrequency() const;
43
48 double noiseFactor() const;
49
54 double errorFactor() const;
55
56 private:
57 double m_acquisitionFrequency;
58 double m_noiseFactor;
59 double m_errorFactor;
60};
61
66class BIORBD_API KalmanRecons {
67 public:
68 // Constructor
69
74
82 Model &model,
83 size_t nbMeasure,
84 KalmanParam params = KalmanParam());
85
89 virtual ~KalmanRecons();
90
95 void DeepCopy(const KalmanRecons &other);
96
103 void getState(
104 GeneralizedCoordinates *Q = nullptr,
105 GeneralizedVelocity *Qdot = nullptr,
106 GeneralizedAcceleration *Qddot = nullptr);
107
114 void setInitState(
115 const GeneralizedCoordinates *Q = nullptr,
116 const GeneralizedVelocity *Qdot = nullptr,
117 const GeneralizedAcceleration *Qddot = nullptr);
118
122 virtual void reconstructFrame() = 0;
123
124 protected:
128 virtual void initialize();
129
137 utils::Matrix evolutionMatrix(const size_t m, size_t n, double Te);
138
145 utils::Matrix processNoiseMatrix(const size_t nbQ, double Te);
146
153 utils::Matrix measurementNoiseMatrix(const size_t nbT, double val);
154
161 utils::Matrix initCovariance(const size_t nbQ, double val);
162
168 GeneralizedCoordinates initState(const size_t nbQ);
169
178 void iteration(
179 utils::Vector measure,
180 const utils::Vector &projectedMeasure,
181 const utils::Matrix &Hessian,
182 const std::vector<size_t> &occlusion = std::vector<size_t>());
183
191 utils::Matrix &InvTp,
192 utils::Vector &measure,
193 const std::vector<size_t> &occlusion);
194
195 // Variables attributes
196 std::shared_ptr<KalmanParam>
198 std::shared_ptr<double> m_Te;
199 std::shared_ptr<size_t> m_nbDof;
200 std::shared_ptr<size_t> m_nMeasure;
201
202 // Kalman filter attributes
203 std::shared_ptr<utils::Vector> m_xp;
204 std::shared_ptr<utils::Matrix> m_A;
205 std::shared_ptr<utils::Matrix> m_Q;
206 std::shared_ptr<utils::Matrix>
208 std::shared_ptr<utils::Matrix> m_Pp;
209};
210
211} // namespace rigidbody
212} // namespace BIORBD_NAMESPACE
213
214#endif // BIORBD_RIGIDBODY_KALMAN_RECONS_H
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.
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.
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.
virtual void reconstructFrame()=0
Proceed to one iteration of the Kalman filter.
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