KalmanRecons.h

#ifndef BIORBD_RIGIDBODY_KALMAN_RECONS_H
#define BIORBD_RIGIDBODY_KALMAN_RECONS_H
 
#include <memory>
#include <vector>
#include "biorbdConfig.h"
 
 
namespace biorbd {
class Model;
 
namespace utils {
class Matrix;
class Vector;
}
 
namespace rigidbody {
class GeneralizedCoordinates;
class GeneralizedVelocity;
class GeneralizedAcceleration;
 
class KalmanParam{
public:
    KalmanParam(
            double frequency = 100,
            double noiseFactor = 1e-10,
            double errorFactor = 1e-5);
 
    double acquisitionFrequency() const;
 
    double noiseFactor() const;
 
    double errorFactor() const;
 
private:
        double m_acquisitionFrequency; 
        double m_noiseFactor; 
        double m_errorFactor; 
};
 
class BIORBD_API KalmanRecons
{
public:
 
    // Constructor 
 
    KalmanRecons();
    
    KalmanRecons(
            biorbd::Model& model,
            unsigned int nbMeasure,
            KalmanParam params = KalmanParam());
 
    virtual ~KalmanRecons();
 
    void DeepCopy(const biorbd::rigidbody::KalmanRecons& other);
 
 
    void getState(
            biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr,
            biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr,
            biorbd::rigidbody::GeneralizedAcceleration *Qddot = nullptr);
 
    void setInitState(
            const biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr,
            const biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr,
            const biorbd::rigidbody::GeneralizedAcceleration *Qddot = nullptr);
 
    virtual void reconstructFrame() = 0;
 
protected:
    virtual void initialize();
 
    biorbd::utils::Matrix evolutionMatrix(
            const unsigned int m,
            unsigned int n,
            double Te); 
 
    biorbd::utils::Matrix processNoiseMatrix(
            const unsigned int nbQ,
            double Te);
 
    biorbd::utils::Matrix measurementNoiseMatrix(
            const unsigned int nbT,
            double val);
 
    biorbd::utils::Matrix initCovariance(
            const unsigned int nbQ,
            double val);
 
    biorbd::rigidbody::GeneralizedCoordinates initState(
            const unsigned int nbQ);
 
    void iteration(
            biorbd::utils::Vector measure,
            const biorbd::utils::Vector &projectedMeasure,
            const biorbd::utils::Matrix &Hessian,
            const std::vector<unsigned int> &occlusion = std::vector<unsigned int>());
 
    virtual void manageOcclusionDuringIteration(
            biorbd::utils::Matrix &InvTp,
            biorbd::utils::Vector &measure,
            const std::vector<unsigned int> &occlusion);
 
    // Variables attributes
    std::shared_ptr<KalmanParam> m_params; 
    std::shared_ptr<double> m_Te; 
    std::shared_ptr<unsigned int> m_nbDof; 
    std::shared_ptr<unsigned int> m_nMeasure; 
 
    // Kalman filter attributes
    std::shared_ptr<biorbd::utils::Vector> m_xp; 
    std::shared_ptr<biorbd::utils::Matrix> m_A; 
    std::shared_ptr<biorbd::utils::Matrix> m_Q; 
    std::shared_ptr<biorbd::utils::Matrix> m_R; 
    std::shared_ptr<biorbd::utils::Matrix> m_Pp; 
 
};
 
}}
 
#endif // BIORBD_RIGIDBODY_KALMAN_RECONS_H