Biorbd Release_1.13.0
Loading...
Searching...
No Matches
BIORBD_NAMESPACE::rigidbody::KalmanRecons Class Referenceabstract

Class Kinematic reconstruction algorithm using an Extended Kalman Filter. More...

#include <KalmanRecons.h>

Inheritance diagram for BIORBD_NAMESPACE::rigidbody::KalmanRecons:
BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU BIORBD_NAMESPACE::rigidbody::KalmanReconsMarkers

Public Member Functions

 KalmanRecons ()
 Kalman reconstruction.
 KalmanRecons (Model &model, size_t nbMeasure, KalmanParam params=KalmanParam())
 Kalman reconstruction.
virtual ~KalmanRecons ()
 Destroy class properly.
void DeepCopy (const KalmanRecons &other)
 Deep copy of Kalman reconstruction.
void getState (GeneralizedCoordinates *Q=nullptr, GeneralizedVelocity *Qdot=nullptr, GeneralizedAcceleration *Qddot=nullptr)
 Get the state (Q, Qdot, Qddot).
void setInitState (const GeneralizedCoordinates *Q=nullptr, const GeneralizedVelocity *Qdot=nullptr, const GeneralizedAcceleration *Qddot=nullptr)
 Set the initial guess of the reconstruction.
virtual void reconstructFrame ()=0
 Proceed to one iteration of the Kalman filter.

Protected Member Functions

virtual void initialize ()
 Initialization of the filter.
utils::Matrix evolutionMatrix (const size_t m, size_t n, double Te)
 Create the evolution matrix.
utils::Matrix processNoiseMatrix (const size_t nbQ, double Te)
 Process the noise matrix.
utils::Matrix measurementNoiseMatrix (const size_t nbT, double val)
 Matrix of the noise on the measurements.
utils::Matrix initCovariance (const size_t nbQ, double val)
 Returns a initialized covianriance matrix.
GeneralizedCoordinates initState (const size_t nbQ)
 Initialize the states.
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.
virtual void manageOcclusionDuringIteration (utils::Matrix &InvTp, utils::Vector &measure, const std::vector< size_t > &occlusion)
 Manage the occlusion during the iteration.

Protected Attributes

std::shared_ptr< KalmanParam > m_params
 The parameters of the Kalman filter.
std::shared_ptr< double > m_Te
 Inherent parameter to the frequency.
std::shared_ptr< size_t > m_nbDof
 Number of states.
std::shared_ptr< size_t > m_nMeasure
 Number of measurements.
std::shared_ptr< utils::Vector > m_xp
 State vector.
std::shared_ptr< utils::Matrix > m_A
 Evolution matrix.
std::shared_ptr< utils::Matrix > m_Q
 Noise matrix.
std::shared_ptr< utils::Matrix > m_R
 Matrix of the noise on the measurements.
std::shared_ptr< utils::Matrix > m_Pp
 Covariance matrix.

Detailed Description

Class Kinematic reconstruction algorithm using an Extended Kalman Filter.

Definition at line 66 of file KalmanRecons.h.

Constructor & Destructor Documentation

◆ KalmanRecons() [1/2]

rigidbody::KalmanRecons::KalmanRecons ( )

Kalman reconstruction.

Definition at line 13 of file KalmanRecons.cpp.

◆ KalmanRecons() [2/2]

rigidbody::KalmanRecons::KalmanRecons ( Model & model,
size_t nbMeasure,
KalmanParam params = KalmanParam() )

Kalman reconstruction.

Parameters
modelThe joint model
nbMeasureThe number of measurements
paramsThe Kalman filter parameters

Definition at line 24 of file KalmanRecons.cpp.

◆ ~KalmanRecons()

rigidbody::KalmanRecons::~KalmanRecons ( )
virtual

Destroy class properly.

Definition at line 38 of file KalmanRecons.cpp.

Member Function Documentation

◆ DeepCopy()

void rigidbody::KalmanRecons::DeepCopy ( const KalmanRecons & other)

Deep copy of Kalman reconstruction.

Parameters
otherThe Kalman reconstruction to copy

Definition at line 40 of file KalmanRecons.cpp.

◆ evolutionMatrix()

utils::Matrix rigidbody::KalmanRecons::evolutionMatrix ( const size_t m,
size_t n,
double Te )
protected

Create the evolution matrix.

Parameters
mThe number of degrees of freedom
nThe order of the Taylor development
TeIs equal to \(\frac{1}{\text{Acquisition frequency}}\)
Returns
The evolution matrix assuming constant frame rate

Definition at line 102 of file KalmanRecons.cpp.

◆ getState()

void rigidbody::KalmanRecons::getState ( rigidbody::GeneralizedCoordinates * Q = nullptr,
rigidbody::GeneralizedVelocity * Qdot = nullptr,
rigidbody::GeneralizedAcceleration * Qddot = nullptr )

Get the state (Q, Qdot, Qddot).

Parameters
QThe generalized coordinates
QdotThe generalized velocities
QddotThe generalized accelerations

Definition at line 84 of file KalmanRecons.cpp.

◆ initCovariance()

utils::Matrix rigidbody::KalmanRecons::initCovariance ( const size_t nbQ,
double val )
protected

Returns a initialized covianriance matrix.

Parameters
nbQThe number of degrees-of-freedom
valThe initial value to fill the matrix with
Returns
The initial covariance matrix

Definition at line 201 of file KalmanRecons.cpp.

◆ initialize()

void rigidbody::KalmanRecons::initialize ( )
protectedvirtual

Initialization of the filter.

Reimplemented in BIORBD_NAMESPACE::rigidbody::KalmanReconsMarkers.

Definition at line 152 of file KalmanRecons.cpp.

◆ initState()

rigidbody::GeneralizedCoordinates rigidbody::KalmanRecons::initState ( const size_t nbQ)
protected

Initialize the states.

Parameters
nbQThe number of degrees-of-freedom
Returns
The initialized states

Definition at line 179 of file KalmanRecons.cpp.

◆ iteration()

void rigidbody::KalmanRecons::iteration ( utils::Vector measure,
const utils::Vector & projectedMeasure,
const utils::Matrix & Hessian,
const std::vector< size_t > & occlusion = std::vector<size_t>() )
protected

Compute an iteration of the Kalman filter.

Parameters
measureThe vector actual measurement to track
projectedMeasureThe projected measurement from the update step of the filter
HessianThe hessian matrix
occlusionThe vector where occlusionsoccurs

Definition at line 52 of file KalmanRecons.cpp.

◆ manageOcclusionDuringIteration()

void rigidbody::KalmanRecons::manageOcclusionDuringIteration ( utils::Matrix & InvTp,
utils::Vector & measure,
const std::vector< size_t > & occlusion )
protectedvirtual

Manage the occlusion during the iteration.

Parameters
InvTpThe inverse of the Tp matrix
measureThe vector actual measurement to track
occlusionThe vector where occlusions occurs

Reimplemented in BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU, and BIORBD_NAMESPACE::rigidbody::KalmanReconsMarkers.

Definition at line 72 of file KalmanRecons.cpp.

◆ measurementNoiseMatrix()

utils::Matrix rigidbody::KalmanRecons::measurementNoiseMatrix ( const size_t nbT,
double val )
protected

Matrix of the noise on the measurements.

Parameters
nbTThe number of measurements
valThe noise level
Returns
The matrix of the noise on the measurements

Definition at line 169 of file KalmanRecons.cpp.

◆ processNoiseMatrix()

utils::Matrix rigidbody::KalmanRecons::processNoiseMatrix ( const size_t nbQ,
double Te )
protected

Process the noise matrix.

Parameters
nbQThe number of degrees-of-freedom
TeIs equal to \(\frac{1}{\text{Acquisition frequency}}\)
Returns
The noise matrix

Definition at line 123 of file KalmanRecons.cpp.

◆ reconstructFrame()

virtual void BIORBD_NAMESPACE::rigidbody::KalmanRecons::reconstructFrame ( )
pure virtual

Proceed to one iteration of the Kalman filter.

Implemented in BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU, and BIORBD_NAMESPACE::rigidbody::KalmanReconsMarkers.

◆ setInitState()

void rigidbody::KalmanRecons::setInitState ( const GeneralizedCoordinates * Q = nullptr,
const GeneralizedVelocity * Qdot = nullptr,
const GeneralizedAcceleration * Qddot = nullptr )

Set the initial guess of the reconstruction.

Parameters
QThe generalized coordinates
QdotThe generalized velocities
QddotThe generalized accelerations

Definition at line 184 of file KalmanRecons.cpp.

Member Data Documentation

◆ m_A

std::shared_ptr<utils::Matrix> BIORBD_NAMESPACE::rigidbody::KalmanRecons::m_A
protected

Evolution matrix.

Definition at line 204 of file KalmanRecons.h.

◆ m_nbDof

std::shared_ptr<size_t> BIORBD_NAMESPACE::rigidbody::KalmanRecons::m_nbDof
protected

Number of states.

Definition at line 199 of file KalmanRecons.h.

◆ m_nMeasure

std::shared_ptr<size_t> BIORBD_NAMESPACE::rigidbody::KalmanRecons::m_nMeasure
protected

Number of measurements.

Definition at line 200 of file KalmanRecons.h.

◆ m_params

std::shared_ptr<KalmanParam> BIORBD_NAMESPACE::rigidbody::KalmanRecons::m_params
protected

The parameters of the Kalman filter.

Definition at line 197 of file KalmanRecons.h.

◆ m_Pp

std::shared_ptr<utils::Matrix> BIORBD_NAMESPACE::rigidbody::KalmanRecons::m_Pp
protected

Covariance matrix.

Definition at line 208 of file KalmanRecons.h.

◆ m_Q

std::shared_ptr<utils::Matrix> BIORBD_NAMESPACE::rigidbody::KalmanRecons::m_Q
protected

Noise matrix.

Definition at line 205 of file KalmanRecons.h.

◆ m_R

std::shared_ptr<utils::Matrix> BIORBD_NAMESPACE::rigidbody::KalmanRecons::m_R
protected

Matrix of the noise on the measurements.

Definition at line 207 of file KalmanRecons.h.

◆ m_Te

std::shared_ptr<double> BIORBD_NAMESPACE::rigidbody::KalmanRecons::m_Te
protected

Inherent parameter to the frequency.

Definition at line 198 of file KalmanRecons.h.

◆ m_xp

std::shared_ptr<utils::Vector> BIORBD_NAMESPACE::rigidbody::KalmanRecons::m_xp
protected

State vector.

Definition at line 203 of file KalmanRecons.h.


The documentation for this class was generated from the following files: