|
Biorbd Release_1.13.0
|
Class Kinematic reconstruction algorithm using an Extended Kalman Filter. More...
#include <KalmanRecons.h>
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. | |
Class Kinematic reconstruction algorithm using an Extended Kalman Filter.
Definition at line 66 of file KalmanRecons.h.
| rigidbody::KalmanRecons::KalmanRecons | ( | ) |
Kalman reconstruction.
Definition at line 13 of file KalmanRecons.cpp.
| rigidbody::KalmanRecons::KalmanRecons | ( | Model & | model, |
| size_t | nbMeasure, | ||
| KalmanParam | params = KalmanParam() ) |
Kalman reconstruction.
| model | The joint model |
| nbMeasure | The number of measurements |
| params | The Kalman filter parameters |
Definition at line 24 of file KalmanRecons.cpp.
|
virtual |
Destroy class properly.
Definition at line 38 of file KalmanRecons.cpp.
| void rigidbody::KalmanRecons::DeepCopy | ( | const KalmanRecons & | other | ) |
Deep copy of Kalman reconstruction.
| other | The Kalman reconstruction to copy |
Definition at line 40 of file KalmanRecons.cpp.
|
protected |
Create the evolution matrix.
| m | The number of degrees of freedom |
| n | The order of the Taylor development |
| Te | Is equal to \(\frac{1}{\text{Acquisition frequency}}\) |
Definition at line 102 of file KalmanRecons.cpp.
| void rigidbody::KalmanRecons::getState | ( | rigidbody::GeneralizedCoordinates * | Q = nullptr, |
| rigidbody::GeneralizedVelocity * | Qdot = nullptr, | ||
| rigidbody::GeneralizedAcceleration * | Qddot = nullptr ) |
Get the state (Q, Qdot, Qddot).
| Q | The generalized coordinates |
| Qdot | The generalized velocities |
| Qddot | The generalized accelerations |
Definition at line 84 of file KalmanRecons.cpp.
|
protected |
Returns a initialized covianriance matrix.
| nbQ | The number of degrees-of-freedom |
| val | The initial value to fill the matrix with |
Definition at line 201 of file KalmanRecons.cpp.
|
protectedvirtual |
Initialization of the filter.
Reimplemented in BIORBD_NAMESPACE::rigidbody::KalmanReconsMarkers.
Definition at line 152 of file KalmanRecons.cpp.
|
protected |
Initialize the states.
| nbQ | The number of degrees-of-freedom |
Definition at line 179 of file KalmanRecons.cpp.
|
protected |
Compute an iteration of the Kalman filter.
| measure | The vector actual measurement to track |
| projectedMeasure | The projected measurement from the update step of the filter |
| Hessian | The hessian matrix |
| occlusion | The vector where occlusionsoccurs |
Definition at line 52 of file KalmanRecons.cpp.
|
protectedvirtual |
Manage the occlusion during the iteration.
| InvTp | The inverse of the Tp matrix |
| measure | The vector actual measurement to track |
| occlusion | The vector where occlusions occurs |
Reimplemented in BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU, and BIORBD_NAMESPACE::rigidbody::KalmanReconsMarkers.
Definition at line 72 of file KalmanRecons.cpp.
|
protected |
Matrix of the noise on the measurements.
| nbT | The number of measurements |
| val | The noise level |
Definition at line 169 of file KalmanRecons.cpp.
|
protected |
Process the noise matrix.
| nbQ | The number of degrees-of-freedom |
| Te | Is equal to \(\frac{1}{\text{Acquisition frequency}}\) |
Definition at line 123 of file KalmanRecons.cpp.
|
pure virtual |
Proceed to one iteration of the Kalman filter.
Implemented in BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU, and BIORBD_NAMESPACE::rigidbody::KalmanReconsMarkers.
| void rigidbody::KalmanRecons::setInitState | ( | const GeneralizedCoordinates * | Q = nullptr, |
| const GeneralizedVelocity * | Qdot = nullptr, | ||
| const GeneralizedAcceleration * | Qddot = nullptr ) |
Set the initial guess of the reconstruction.
| Q | The generalized coordinates |
| Qdot | The generalized velocities |
| Qddot | The generalized accelerations |
Definition at line 184 of file KalmanRecons.cpp.
|
protected |
Evolution matrix.
Definition at line 204 of file KalmanRecons.h.
|
protected |
Number of states.
Definition at line 199 of file KalmanRecons.h.
|
protected |
Number of measurements.
Definition at line 200 of file KalmanRecons.h.
|
protected |
The parameters of the Kalman filter.
Definition at line 197 of file KalmanRecons.h.
|
protected |
Covariance matrix.
Definition at line 208 of file KalmanRecons.h.
|
protected |
Noise matrix.
Definition at line 205 of file KalmanRecons.h.
|
protected |
Matrix of the noise on the measurements.
Definition at line 207 of file KalmanRecons.h.
|
protected |
Inherent parameter to the frequency.
Definition at line 198 of file KalmanRecons.h.
|
protected |
State vector.
Definition at line 203 of file KalmanRecons.h.