1#define BIORBD_API_EXPORTS
2#include "RigidBody/KalmanReconsIMU.h"
6#include <rbdl/Kinematics.h>
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"
17using namespace BIORBD_NAMESPACE;
26 : rigidbody::
KalmanRecons(model, model.nbTechIMUs() * 9, params),
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) {
60 const std::vector<IMU> &IMUobs,
66 static_cast<size_t>(3 * 3 * IMUobs.size()));
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);
85 for (
size_t i = 0; i < 300; ++i) {
93 utils::Vector::Zero(*
m_nbDof * 2);
100 Model &updatedModel =
104 const std::vector<rigidbody::IMU> &zest_tp =
107 std::vector<utils::Matrix> J_tp =
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) {
120 sum += IMUobs(i * 9 + j) * IMUobs(i * 9 + j);
124#ifdef BIORBD_USE_CASADI_MATH
127 sum != 0.0 && !std::isnan(sum)
130 H.block(i * 9, 0, 9, *
m_nbDof) = J_tp[i];
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);
136 occlusionIdx.push_back(i);
141 iteration(IMUobs, zest, H, occlusionIdx);
The actual musculoskeletal model that holds everything in biorbd.
Class GeneralizedAcceleration.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
std::vector< rigidbody::IMU > technicalIMU(const GeneralizedCoordinates &Q, bool updateKin=true)
Return all the technical inertial measurement units (IMU).
std::vector< utils::Matrix > TechnicalIMUJacobian(const GeneralizedCoordinates &Q, bool updateKin=true)
Return the jacobian of the technical inertial measurement units (IMU).
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 ...
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.
KalmanRecons()
Kalman reconstruction.
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.
A wrapper for the Eigen::MatrixXd.
Wrapper of the Eigen VectorXd.