Program Listing for File joint_state.h

↰ Return to documentation for file (states/joint_state.h)

#pragma once
#include <spdlog/spdlog.h>

#include "lupnt/core/definitions.h"
#include "lupnt/dynamics/dynamics.h"
#include "lupnt/filters/filter.h"
#include "lupnt/states/state.h"

namespace lupnt {

  // define Params
  enum EstType { ESTIMATED, CONSIDERED, FIXED };

  class JointState {
  private:
    std::vector<State> states_;
    std::vector<Ptr<Dynamics>> dynamics_;
    std::vector<ParamState> params_;
    std::vector<std::vector<EstType>> est_types_;
    std::vector<Ptr<ProcessNoiseFunction>> proc_noise_;
    MatXd est_matrix_;  // K x N matrix where M is the number of estimated parameters and N is the
                        // number of states (N = K + M)
    MatXd consider_matrix_;  // M x N matrix where M is the number of consider parameters and N is
                             // the number of states
    std::vector<std::vector<std::pair<double, double>>>
        tauqs_;  // Time constants and sigmas for first-order Gauss Markov

    double tau_default_ = 1e12;  // Default time constant for first-order Gauss-Markov

    int size_ = 0;
    int state_size_ = 0;
    int param_size_ = 0;
    int considered_param_size_ = 0;
    int estimated_param_size_ = 0;
    int fixed_param_size_ = 0;
    int stm_size_ = 0;

  public:
    int GetSize() const { return state_size_ + param_size_; };
    int GetStateSize() const { return state_size_; };
    int GetParamSize() const { return param_size_; };
    int GetStmSize() const { return stm_size_; };
    int GetEstimatedParamSize() const { return estimated_param_size_; };
    int GetConsideredParamSize() const { return considered_param_size_; };
    int GetFixedParamSize() const { return fixed_param_size_; };
    State GetState() const;
    State GetStmState() const;
    ParamState GetParams() const;

    void Add(const State& state, Ptr<Dynamics>&& dynamics,
             Ptr<ProcessNoiseFunction>&& proc_noise = nullptr,
             const ParamState& param = ParamState(0), std::vector<EstType> est_types = {},
             std::vector<std::pair<double, double>> tauq = {});

    // void Add(State state, Ptr<DynamicsWithParams> dynamics,
    //                          Ptr<FilterProcessNoiseFunction> proc_noise = nullptr);

    FilterDynamicsFunction GetDynamicsFunction();
    ProcessNoiseFunction GetProcessNoiseFunction();
    MatXd GetProcessNoiseMappingMatrix();
  };

}  // namespace lupnt