accelerometersimu.hpp

#ifndef DDF_ACCELEROMETERSIMU_HPP
#define DDF_ACCELEROMETERSIMU_HPP

#include "ddfsimu/sensorsimu.hpp"
#include "ddfsimu/accelmotion.hpp"
#include "ddf/tools.hpp"

namespace jafar
{
  namespace ddfsimu
  {

    template<typename T_OBS>
    class AcceleroSimu : public SensorSimu<T_OBS>
    {
      VEC   m_biases;
      bool  m_with_biases;

    public:
      AcceleroSimu(unsigned short x_size, unsigned short z_size, bool use_biases)
        :  SensorSimu<T_OBS>(x_size, z_size),
          m_biases(z_size),
          m_with_biases(use_biases)
      {
        JFR_PRECOND(z_size <= 3, "z_size > 3");
      
        this->m_type_name = SENSOR_ACCEL;
        m_biases.clear();
  
        this->Hx.clear();
        this->Hx(0, AccelMotion::SV_AX) = 1.;
        if (m_with_biases) this->Hx(0, AccelMotion::SV_BAX) = 1.;
        if(z_size > 1) { this->Hx(1, AccelMotion::SV_AY) = 1.; if (m_with_biases) this->Hx(1, AccelMotion::SV_BAY) = 1.;}
        if(z_size > 2) { this->Hx(2, AccelMotion::SV_AZ) = 1.; if (m_with_biases) this->Hx(2, AccelMotion::SV_BAZ) = 1.;}
      }

      virtual ~AcceleroSimu() {}

      bool WithBiases() { return m_with_biases; }
      void SetBiases(VEC const& biases) { m_biases = biases; }
      void Perceive(VEC const& proc_state, time t)
      {
        VEC measured_accel(this->GetMeasureSize());

        PerceiveRet(proc_state, measured_accel);
        AddMeasurementWithNoise(t,t+this->GetProcTime(),measured_accel);
      }
  
      void PerceiveRet(VEC const& proc_state, VEC & result) const {

        // Make sure biases are zero before in proc_state before calling this function
        noalias(result) = prod(this->Hx, proc_state);
        if(m_with_biases) result += m_biases;
      }
      void PrintObservationModel() {
        JFR_DEBUG("Observation model of sensor " << this->GetID() << " (" << this->GetName() << ")" << "\n");
        PrettyPrintMatrix(this->Hx, "Hx");
      }
    };

    class AcceleroSimuUncorr : public AcceleroSimu<Bayesian_filter::Linear_uncorrelated_observe_model>
    {
      
    public:
      AcceleroSimuUncorr(unsigned short x_size, unsigned short z_size, bool with_biases = false):
        AcceleroSimu<Bayesian_filter::Linear_uncorrelated_observe_model>(x_size, z_size, with_biases){}
      virtual ~AcceleroSimuUncorr() {}
      
      void ComputeInformation(VEC const& z, VEC &i, MSYM &I) { compute_information_uncorr(*this, z, i, I); }
      void SetSensorModelVar(VEC const& var) { Zv.clear(); Zv = var; }
      void PrintObservationModel()
      {
        AcceleroSimu<Bayesian_filter::Linear_uncorrelated_observe_model>::PrintObservationModel();
        PrettyPrintVector(this->Zv, "Zv");
      }
    };

    class AcceleroSimuCorr : public AcceleroSimu<Bayesian_filter::Linear_correlated_observe_model>
    {
  
    public:
      AcceleroSimuCorr(unsigned short x_size, unsigned short z_size, bool with_biases = false):
        AcceleroSimu<Bayesian_filter::Linear_correlated_observe_model>(x_size, z_size, with_biases){}
      virtual ~AcceleroSimuCorr() {}
    
      void ComputeInformation(VEC const& z, VEC &i, MSYM &I) { compute_information_corr(*this, z, m_sp, i, I); }
      void SetSensorModelCov(MSYM const& cov) { Z = cov; }
      void PrintObservationModel()
      {
        AcceleroSimu<Bayesian_filter::Linear_correlated_observe_model>::PrintObservationModel();
        PrettyPrintMatrix(Z, "R");
      }
    };

  } // namespace ddfsimu
} // namespace jafar
#endif