1#define BIORBD_API_EXPORTS
2#include "RigidBody/KalmanRecons.h"
4#include "BiorbdModel.h"
5#include "RigidBody/GeneralizedAcceleration.h"
6#include "RigidBody/GeneralizedCoordinates.h"
7#include "RigidBody/GeneralizedVelocity.h"
8#include "Utils/Matrix.h"
9#include "Utils/Vector.h"
11using namespace BIORBD_NAMESPACE;
15 m_Te(std::make_shared<double>(1.0 / (
m_params->acquisitionFrequency()))),
16 m_nbDof(std::make_shared<size_t>()),
18 m_xp(std::make_shared<utils::Vector>()),
19 m_A(std::make_shared<utils::Matrix>()),
20 m_Q(std::make_shared<utils::Matrix>()),
21 m_R(std::make_shared<utils::Matrix>()),
22 m_Pp(std::make_shared<utils::Matrix>()) {}
29 m_Te(std::make_shared<double>(1.0 / (
m_params->acquisitionFrequency()))),
30 m_nbDof(std::make_shared<size_t>(model.dof_count)),
31 m_nMeasure(std::make_shared<size_t>(nbMeasure)),
32 m_xp(std::make_shared<utils::Vector>()),
33 m_A(std::make_shared<utils::Matrix>()),
34 m_Q(std::make_shared<utils::Matrix>()),
35 m_R(std::make_shared<utils::Matrix>()),
36 m_Pp(std::make_shared<utils::Matrix>()) {}
56 const std::vector<size_t> &occlusion) {
66 *
m_xp = xkm + K * (measure - projectedMeasure);
67 const RigidBodyDynamics::Math::MatrixNd &temp(
68 utils::Matrix::Identity(3 * *
m_nbDof, 3 * *
m_nbDof) - K * Hessian);
69 *
m_Pp = temp * Pkm * temp.transpose() + K * *
m_R * K.transpose();
75 const std::vector<size_t> &occlusion) {
76 for (
size_t i = 0; i < occlusion.size(); ++i)
77 for (
size_t j = occlusion[i]; j < occlusion[i] + 1; ++j) {
92 if (Qdot !=
nullptr) {
96 if (Qddot !=
nullptr) {
110 for (
size_t i = 2; i < n + 1; ++i) {
111 size_t j = (i - 1) * nQ;
114 for (
size_t cmp = 0; cmp < nQ * n - j; ++cmp) {
115 A(0 + cmp, j + cmp) +=
116 c *
static_cast<double>(std::pow(Te, (
static_cast<double>(i) - 1.0)));
127 double c1 = 1.0 / 20.0 * pow(Te, 5);
128 double c2 = 1.0 / 8.0 * pow(Te, 4);
129 double c3 = 1.0 / 6.0 * pow(Te, 3);
130 double c4 = 1.0 / 3.0 * pow(Te, 3);
131 double c5 = 1.0 / 2.0 * pow(Te, 2);
136 for (
size_t j = 0; j < nbQ; ++j) {
139 Q(j, 2 * nbQ + j) = c3;
141 Q(nbQ + j, nbQ + j) = c4;
142 Q(nbQ + j, 2 * nbQ + j) = c5;
143 Q(2 * nbQ + j, j) = c3;
144 Q(2 * nbQ + j, nbQ + j) = c5;
145 Q(2 * nbQ + j, 2 * nbQ + j) = c6;
173 for (
size_t i = 0; i < nbT; ++i) {
181 return rigidbody::GeneralizedCoordinates::Zero(3 * nbQ);
192 if (Qdot !=
nullptr) {
196 if (Qddot !=
nullptr) {
205 for (
size_t i = 0; i < 3 * nbQ; ++i) {
215 : m_acquisitionFrequency(frequency),
220 return m_acquisitionFrequency;
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.
Class Kinematic reconstruction algorithm using an Extended Kalman Filter.
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.
virtual ~KalmanRecons()
Destroy class properly.
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.
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.