Biorbd Release_1.13.0
Loading...
Searching...
No Matches
BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU Class Reference

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

#include <KalmanReconsIMU.h>

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

Public Member Functions

 KalmanReconsIMU ()
 Initialize the Kalman filter and kalman reconstruction for inertial measurement units (IMU) data.
 KalmanReconsIMU (Model &model, KalmanParam params=KalmanParam(100, 0.005, 1e-10))
 Initialize the Kalman filter and Kalman reconstruction for inertial measurement units (IMU) data.
KalmanReconsIMU DeepCopy () const
 Deep copy of the Kalman reconstruction from inertial measurement units (IMU) data.
void DeepCopy (const KalmanReconsIMU &other)
 Deep copy of a Kalman reconstruction from inertial measurement units (IMU) data.
virtual void reconstructFrame (Model &model, const std::vector< IMU > &IMUobs, GeneralizedCoordinates *Q, GeneralizedVelocity *Qdot, GeneralizedAcceleration *Qddot)
 Proceed to one iteration of the Kalman filter.
virtual void reconstructFrame (Model &model, const utils::Vector &IMUobs, GeneralizedCoordinates *Q, GeneralizedVelocity *Qdot, GeneralizedAcceleration *Qddot)
 Reconstruct the kinematics.
virtual void reconstructFrame ()
 This function cannot be used to reconstruct frames.
bool first ()
 Return if the first iteration was done.
Public Member Functions inherited from BIORBD_NAMESPACE::rigidbody::KalmanRecons
 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.

Protected Member Functions

virtual void manageOcclusionDuringIteration (utils::Matrix &InvTp, utils::Vector &measure, const std::vector< size_t > &occlusion)
 Manage the occlusion during the iteration.
Protected Member Functions inherited from BIORBD_NAMESPACE::rigidbody::KalmanRecons
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.

Protected Attributes

std::shared_ptr< bool > m_firstIteration
 If first iteration was done.
Protected Attributes inherited from BIORBD_NAMESPACE::rigidbody::KalmanRecons
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 for IMU.

Definition at line 20 of file KalmanReconsIMU.h.

Constructor & Destructor Documentation

◆ KalmanReconsIMU() [1/2]

rigidbody::KalmanReconsIMU::KalmanReconsIMU ( )

Initialize the Kalman filter and kalman reconstruction for inertial measurement units (IMU) data.

Definition at line 19 of file KalmanReconsIMU.cpp.

◆ KalmanReconsIMU() [2/2]

rigidbody::KalmanReconsIMU::KalmanReconsIMU ( Model & model,
rigidbody::KalmanParam params = KalmanParam(100, 0.005, 1e-10) )

Initialize the Kalman filter and Kalman reconstruction for inertial measurement units (IMU) data.

Parameters
modelThe joint model
paramsThe Kalman filter parameters

Definition at line 23 of file KalmanReconsIMU.cpp.

Member Function Documentation

◆ DeepCopy() [1/2]

rigidbody::KalmanReconsIMU rigidbody::KalmanReconsIMU::DeepCopy ( ) const

Deep copy of the Kalman reconstruction from inertial measurement units (IMU) data.

Returns
Copy of the Kalman reconstruction from IMU data

Definition at line 32 of file KalmanReconsIMU.cpp.

◆ DeepCopy() [2/2]

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

Deep copy of a Kalman reconstruction from inertial measurement units (IMU) data.

Parameters
otherThe Kalman reconstruction to copy

Definition at line 38 of file KalmanReconsIMU.cpp.

◆ first()

bool rigidbody::KalmanReconsIMU::first ( )

Return if the first iteration was done.

Returns
If the first iteration was done

Definition at line 56 of file KalmanReconsIMU.cpp.

◆ manageOcclusionDuringIteration()

void rigidbody::KalmanReconsIMU::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 from BIORBD_NAMESPACE::rigidbody::KalmanRecons.

Definition at line 44 of file KalmanReconsIMU.cpp.

◆ reconstructFrame() [1/3]

void rigidbody::KalmanReconsIMU::reconstructFrame ( )
virtual

This function cannot be used to reconstruct frames.

Implements BIORBD_NAMESPACE::rigidbody::KalmanRecons.

Definition at line 146 of file KalmanReconsIMU.cpp.

◆ reconstructFrame() [2/3]

void rigidbody::KalmanReconsIMU::reconstructFrame ( Model & model,
const std::vector< IMU > & IMUobs,
rigidbody::GeneralizedCoordinates * Q,
rigidbody::GeneralizedVelocity * Qdot,
rigidbody::GeneralizedAcceleration * Qddot )
virtual

Proceed to one iteration of the Kalman filter.

Parameters
modelThe joint model
IMUobsObserved inertial measurement unit (IMU) data
QThe generalized coordinates
QdotThe generalized velocities
QddotThe generalized accelerations

Definition at line 58 of file KalmanReconsIMU.cpp.

◆ reconstructFrame() [3/3]

void rigidbody::KalmanReconsIMU::reconstructFrame ( Model & model,
const utils::Vector & IMUobs,
rigidbody::GeneralizedCoordinates * Q,
rigidbody::GeneralizedVelocity * Qdot,
rigidbody::GeneralizedAcceleration * Qddot )
virtual

Reconstruct the kinematics.

Parameters
modelThe joint model
IMUobsObserved inertial measurement unit (IMU) data in one large column-major vector
QThe generalized coordinates
QdotThe generalized velocities
QddotThe generalized accelerations

Definition at line 76 of file KalmanReconsIMU.cpp.

Member Data Documentation

◆ m_firstIteration

std::shared_ptr<bool> BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU::m_firstIteration
protected

If first iteration was done.

Definition at line 109 of file KalmanReconsIMU.h.


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