Biorbd Release_1.13.0
Loading...
Searching...
No Matches
KalmanReconsIMU.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/KalmanReconsIMU.h"
3
4#include <rbdl/Model.h>
5
6#include <rbdl/Kinematics.h>
7
8#include "BiorbdModel.h"
9#include "RigidBody/GeneralizedAcceleration.h"
10#include "RigidBody/GeneralizedCoordinates.h"
11#include "RigidBody/GeneralizedVelocity.h"
12#include "RigidBody/IMU.h"
13#include "Utils/Error.h"
14#include "Utils/Matrix.h"
15#include "Utils/Rotation.h"
16
17using namespace BIORBD_NAMESPACE;
18
20 : rigidbody::KalmanRecons(),
21 m_firstIteration(std::make_shared<bool>(true)) {}
22
24 Model &model,
26 : rigidbody::KalmanRecons(model, model.nbTechIMUs() * 9, params),
27 m_firstIteration(std::make_shared<bool>(true)) {
28 // Initialize the filter
29 initialize();
30}
31
37
43
45 utils::Matrix &InvTp,
46 utils::Vector &measure,
47 const std::vector<size_t> &occlusion) {
48 for (size_t i = 0; i < occlusion.size(); ++i)
49 for (size_t j = occlusion[i] * 9; j < occlusion[i] * 9 + 9; ++j) {
50 InvTp(j, j) = 0; // Artefact due to the fact that m_R has a value
51 // at (j:j+2,j:j+2)
52 measure[j] = 0;
53 }
54}
55
57
59 Model &m,
60 const std::vector<IMU> &IMUobs,
64 // Separate the IMUobs in a big vector
66 static_cast<size_t>(3 * 3 * IMUobs.size())); // Matrix 3*3 * nbIMU
67 for (size_t i = 0; i < IMUobs.size(); ++i)
68 for (size_t j = 0; j < 3; ++j) {
69 T.block(9 * i + 3 * j, 0, 3, 1) = IMUobs[i].block(0, j, 3, 1);
70 }
71
72 // Reconstruct the kinematics
73 reconstructFrame(m, T, Q, Qdot, Qddot);
74}
75
77 Model &model,
78 const utils::Vector &IMUobs,
82 // An iteration of the Kalman filter
83 if (*m_firstIteration) {
84 *m_firstIteration = false;
85 for (size_t i = 0; i < 300; ++i) {
86 // The first time, call in a recursive manner to have a decent
87 // initial position
88 reconstructFrame(model, IMUobs, nullptr, nullptr, nullptr);
89
90 // We can't use the same trick for reinitiliazing speed in pP as IMU
91 // are based on velocity
92 m_xp->block(*m_nbDof, 0, *m_nbDof * 2, 1) =
93 utils::Vector::Zero(*m_nbDof * 2);
94 }
95 }
96
97 // Projected state
98 const utils::Vector &xkm(*m_A * *m_xp);
100 Model &updatedModel =
101 static_cast<Model &>(model.UpdateKinematicsCustom(&Q_tp));
102
103 // Projected markers
104 const std::vector<rigidbody::IMU> &zest_tp =
105 updatedModel.technicalIMU(Q_tp, false);
106 // Jacobian
107 std::vector<utils::Matrix> J_tp =
108 updatedModel.TechnicalIMUJacobian(Q_tp, false);
109
110 // Create only one matrix for zest and Jacobian
112 utils::Matrix::Zero(
113 *m_nMeasure,
114 *m_nbDof * 3)); // 3*nCentrales => X,Y,Z ; 3*nbDof => Q, Qdot, Qddot
115 utils::Vector zest(utils::Vector::Zero(*m_nMeasure));
116 std::vector<size_t> occlusionIdx;
117 for (size_t i = 0; i < *m_nMeasure / 9; ++i) {
118 utils::Scalar sum = 0;
119 for (size_t j = 0; j < 9; ++j) { // Calculate the norm for the 9 components
120 sum += IMUobs(i * 9 + j) * IMUobs(i * 9 + j);
121 }
122
123 if (
124#ifdef BIORBD_USE_CASADI_MATH
125 true
126#else
127 sum != 0.0 && !std::isnan(sum)
128#endif
129 ) { // If there is an IMU (no zero or NaN)
130 H.block(i * 9, 0, 9, *m_nbDof) = J_tp[i];
131 const utils::Rotation &rot = zest_tp[i].rot();
132 for (size_t j = 0; j < 3; ++j) {
133 zest.block(i * 9 + j * 3, 0, 3, 1) = rot.block(0, j, 3, 1);
134 }
135 } else {
136 occlusionIdx.push_back(i);
137 }
138 }
139
140 // Make the filter
141 iteration(IMUobs, zest, H, occlusionIdx);
142
143 getState(Q, Qdot, Qddot);
144}
145
147 utils::Error::raise("Reconstructing kinematics for IMU needs measurements");
148}
The actual musculoskeletal model that holds everything in biorbd.
std::vector< rigidbody::IMU > technicalIMU(const GeneralizedCoordinates &Q, bool updateKin=true)
Return all the technical inertial measurement units (IMU).
Definition IMUs.cpp:100
std::vector< utils::Matrix > TechnicalIMUJacobian(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the jacobian of the technical inertial measurement units (IMU).
Definition IMUs.cpp:172
Joints & UpdateKinematicsCustom(const GeneralizedCoordinates *Q=nullptr, const GeneralizedVelocity *Qdot=nullptr, const rigidbody::GeneralizedAcceleration *Qddot=nullptr)
Update the kinematic variables such as body velocities and accelerations in the model to reflect the ...
Definition Joints.cpp:1888
Parameters of the reconstruction.
Class Kinematic reconstruction algorithm using an Extended Kalman Filter for IMU.
virtual void reconstructFrame()
This function cannot be used to reconstruct frames.
virtual void reconstructFrame(Model &model, const std::vector< IMU > &IMUobs, GeneralizedCoordinates *Q, GeneralizedVelocity *Qdot, GeneralizedAcceleration *Qddot)
Proceed to one iteration of the Kalman filter.
KalmanReconsIMU DeepCopy() const
Deep copy of the Kalman reconstruction from inertial measurement units (IMU) data.
bool first()
Return if the first iteration was done.
KalmanReconsIMU()
Initialize the Kalman filter and kalman reconstruction for inertial measurement units (IMU) data.
std::shared_ptr< bool > m_firstIteration
If first iteration was done.
virtual void manageOcclusionDuringIteration(utils::Matrix &InvTp, utils::Vector &measure, const std::vector< size_t > &occlusion)
Manage the occlusion during the iteration.
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.
std::shared_ptr< size_t > m_nbDof
Number of states.
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.
void getState(GeneralizedCoordinates *Q=nullptr, GeneralizedVelocity *Qdot=nullptr, GeneralizedAcceleration *Qddot=nullptr)
Get the state (Q, Qdot, Qddot).
std::shared_ptr< utils::Matrix > m_A
Evolution matrix.
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
A wrapper for the Eigen::MatrixXd.
Definition Matrix.h:18
Wrapper of the Eigen VectorXd.
Definition Vector.h:21