1#ifndef BIORBD_RIGIDBODY_KALMAN_RECONS_H
2#define BIORBD_RIGIDBODY_KALMAN_RECONS_H
4#include "biorbdConfig.h"
9namespace BIORBD_NAMESPACE {
34 double frequency = 100,
57 double m_acquisitionFrequency;
182 const std::vector<size_t> &occlusion = std::vector<size_t>());
193 const std::vector<size_t> &occlusion);
196 std::shared_ptr<KalmanParam>
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;
The actual musculoskeletal model that holds everything in biorbd.
Class GeneralizedAcceleration.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
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.
KalmanRecons()
Kalman reconstruction.
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.
Wrapper of the Eigen VectorXd.