test_delayed.hpp

#ifndef DDF_TEST_DELAYED_HPP
#define DDF_TEST_DELAYED_HPP

#include <iostream>
#include <fstream>
#include <BayesFilter/bayesFlt.hpp>
#include <BayesFilter/infFlt.hpp>

#include "jmath/ublasExtra.hpp"
#include "sensor.hpp"
#include "velocitysimu.hpp"
#include "gpssimu.hpp"
#include "robotsimu.hpp"
#include "upd_const_speed.hpp"
#include "ddf/tools.hpp"

namespace jafar
{
  namespace ddfsimu
  {

    namespace demo
    {

    using namespace Bayesian_filter;
    using namespace Bayesian_filter_matrix;

    /*
    * Simple Prediction model (constant speed model)
    */
    class Const_speed_predict_model : public Linear_invertable_predict_model
    {
      public:
        static const double DELTA_T;
        static const unsigned int SV_SIZE;

      public:
        Const_speed_predict_model() : Linear_invertable_predict_model(Const_speed_predict_model::SV_SIZE,3)
        {
          Fx.clear();
          Fx(0,0) = 1.; Fx(1,1) = 1.;
          Fx(2,2) = 1.; Fx(3,3) = 1.;
          Fx(4,4) = 1.; Fx(5,5) = 1.;
  
          Fx(0,1) = DELTA_T; Fx(2,3) = DELTA_T; Fx(4,5) = DELTA_T;

    jafar::jmath::ublasExtra::lu_inv(Fx, static_cast<detail::BaseDenseColMatrix &> (inv.Fx));

          G.clear();
          double tmp = DELTA_T * DELTA_T / 2.0;

          G(0,0) = tmp; G(1,0) = DELTA_T;
          G(2,1) = tmp; G(3,1) = DELTA_T;
          G(4,2) = tmp; G(5,2) = DELTA_T;
  
          q[0] = 0.5; q[1] = 0.5; q[2] = 0.5;
        }

        void Print_Prediction_Model()
        {
          PrettyPrintMatrix(Fx,"Fx");
          PrettyPrintMatrix(G,"G");
          PrettyPrintVector(q,"q");
        }
    };

    class FSStorage : public FSStorageBase<int>
    {
      public:
        InfoContainer   m_StateCntr;

      public:
        FSStorage(unsigned short sv_size, FS_STORE_TYPE fs_type): FSStorageBase<int>(sv_size, fs_type,0), m_StateCntr(sv_size){}
        FSStorage(FSStorage const& other): FSStorageBase<int>(other), m_StateCntr(other.GetSize()) {
          if (this == &other) return;
          this->m_StateCntr = other.m_StateCntr;
        }
    };

    void FillStorage(Upd_const_speed & mPred, FS_STORE_TYPE fs_type, std::vector<FSStorage> &m_storage)
    {
      FSStorage  mStore(mPred.GetSvSize(), fs_type);

      mStore.m_MkIik  = mPred.GetPrediction();
      mStore.m_StateCntr = mPred.GetInversePrediction();
      m_storage.push_back(mStore);
    }

      // nb | k | Y(k) | y(k) | P(k) | x(k) | PPred(k) | xPred(k)
    void DumpStore(std::string const& fname, std::vector<FSStorage> &m_storage)
    {
      std::vector<FSStorage>::iterator  it;
      int  nb = 0;
    
      std::ofstream  dump_file(fname.c_str(), std::ios::out|std::ios::trunc);
      dump_file << std::scientific;
  
      dump_file << "# nb | k | Y(k) | y(k) | P(k) | x(k) | v(k)" << std::endl;
  
      for(it = m_storage.begin(); it != m_storage.end(); it++, nb++)
      {
        dump_file << nb << LOGSEP << ((*it).GetTime()).to_double() << LOGSEP << (*it).m_MkIik.GetInfoMat()(0,0) << LOGSEP\
            << (*it).m_MkIik.Getinfo()(0) << LOGSEP << (*it).m_StateCntr.GetInfoMat()(0,0) << LOGSEP\
            << (*it).m_StateCntr.Getinfo()(0) << LOGSEP << (*it).m_StateCntr.Getinfo()(1) << LOGSEP;
        dump_file << std::endl;
      }
      dump_file.close();
      m_storage.clear();
    }

    void DumpHistory(std::string const& fname, Upd_const_speed & predMod, std::vector<FSStorage> &m_storage)
    {
      unsigned short buf_size = predMod.GetHistorySize();
      unsigned short nb = 0;
      ParametersGeneric   tmp;
      FSStorageBase<ParametersGeneric>   store(predMod.GetSvSize(), FS_UPDATE,tmp);

      std::ofstream  dump_file(fname.c_str(), std::ios::out|std::ios::trunc);
      dump_file << std::scientific;
  
      dump_file << "# nb | time | Yx | yx | Yv | yv | type" << std::endl;
            
      for(int i = 0; i < buf_size; i++, nb++)
      {
        store = predMod.RecallStoredState(i);
        dump_file << nb << LOGSEP << store.GetTime().to_double() << LOGSEP << store.m_MkIik.GetInfoMat()(0,0) << LOGSEP\
            << store.m_MkIik.Getinfo()(0) << LOGSEP << store.m_MkIik.GetInfoMat()(1,1) << LOGSEP << store.m_MkIik.Getinfo()(1) << LOGSEP\
            << store.GetType();
        dump_file << std::endl;
      }
      predMod.ClearHistory();
      dump_file.close();
    }
    
    // Test the delayed and asequent data fusion with one robot equipped with two sensors.
    // state vector[x vx]
    // one velocimeter (for vx) and one absolute sensor (for x, delayed)
    void test_out_of_sequence()
    {
      const unsigned short cx = 2;
      const unsigned short dim = cx/2;
              
      const unsigned short PROF_SIZE_R1 = 2;
      double accProfR1[PROF_SIZE_R1][4] = {{0.0,0.,0.,0.},
        {5.0,0.,0.,0.}};

        std::vector<FSStorage>     m_storage;
              
        VEC   v1(1);
        VEC   v2(2);
        VEC   v3_1(3), v3_2(3);
        MSYM  Y0(cx, cx);
        time  now = time::null(), horizon = time::null();
        InfoContainer  iCont(cx);
  
        // Create sensors
        VelocitySimuUncorr *vel_r1 = new VelocitySimuUncorr(cx, dim);
        v1[0] = TWOSIGMA_TO_VAR(0.03);      vel_r1->SetSensorSimVariance(v1);
        v1[0] = TWOSIGMA_TO_VAR(0.031);      vel_r1->SetSensorModelVar(v1);
        vel_r1->SetPeriod(0.1);
        vel_r1->PrintObservationModel();
  
        GPSSimuUncorr *gps_r1 = new GPSSimuUncorr(cx, dim);
        v1[0] = TWOSIGMA_TO_VAR(0.15);      gps_r1->SetSensorSimVariance(v1);
        v1[0] = TWOSIGMA_TO_VAR(0.17);      gps_r1->SetSensorModelVar(v1);
        gps_r1->SetPeriod(0.5);
        gps_r1->SetProcTime(time::from_ms(300));
        gps_r1->SetTimeOffset(time::from_ms(5));
        gps_r1->PrintObservationModel();

        // Create and initialize the robot
        RobotSimu   robsim1(cx, "R1D1", true);
        robsim1.FillProfile(PROF_SIZE_R1, accProfR1);
              
        v3_1.clear();  v3_2.clear();
        v3_1[0] = 0.4;
        robsim1.SetInitialConditions(v3_1, v3_2);
        robsim1.AddSensor(vel_r1);
        robsim1.AddSensor(gps_r1);
              
        robsim1.SimulatePropPerception();
        robsim1.FillUpMeasurementSet(RobotSimu::SORT_STAMP_SENSED);    // create one buffer with all measurements
        robsim1.PrintRobot();
        robsim1.DumpAllMeasures("delay_meas_all_sensed.log");
        robsim1.DumpTruth("delay_truth.log");
        vel_r1->DumpMeasurements("delay_meas_vel.log");
        gps_r1->DumpMeasurements("delay_meas_pos.log");
        robsim1.ResetCounter();
  
        // Initialize the information filter for sensor fusion (cst speed model)
        Upd_const_speed   myPredict(dim,2*dim,dim);
        FSStorageBase<ParametersGeneric>    store(Const_speed_predict_model::SV_SIZE, FS_UPDATE);
  
        myPredict.StoreStates(true);
        v1[0] = 0.002;
        myPredict.Updateq(v1);
        v2[0] = 0.0;   v2[1] = v3_1[0]+0.2;
        Y0.clear();    Y0(0,0) = 0.4;    Y0(1,1) = 0.3333;
        myPredict.InitInformation(v2, Y0);
        myPredict.SetNeedUpdate(NEED_UPD_Q, false);
  
        // Do synchronous sensor fusion
        now = robsim1.GetMinTime();
              
        while(!robsim1.EndBuf())// && robsim1.GetCurrentRelTime() < time(0,300000))
        {
          MeasureItem  const& mItem = robsim1.GetCurrentMeasure();
  
            // Prediction phase
          horizon = mItem.GetSensedTime();
          myPredict.SetModelParams(now, horizon);
          myPredict.Predict(horizon);
          FillStorage(myPredict, FS_PREDICT, m_storage);
  
          now = horizon;
  
            // Update phase
          do
          {
            robsim1.GetCurrentInfo(iCont);
            myPredict.UpdatePredictionWith(iCont);
            FillStorage(myPredict, FS_UPDATE, m_storage);
            robsim1.IncCounter();
          }
          while(!robsim1.EndBuf() && robsim1.GetCurrentRelTime() == now);
        }
  
        //myPredict.PrintHistory();
        DumpStore(std::string("delay_histo_sensed.log"), m_storage);
        myPredict.PrintHistory();
        DumpHistory(std::string("delay_histo_filter_sens.log"), myPredict, m_storage);
  
        // *** Now do it with delayed data involved
  
        myPredict.InitInformation(v2, Y0);
              
        robsim1.ClearUpMeasurementSet();
        robsim1.FillUpMeasurementSet(RobotSimu::SORT_STAMP_AVAILABLE);
        robsim1.DumpAllMeasures("delay_meas_all_available.log");
  
        now = robsim1.GetMinTime();
        robsim1.ResetCounter();
              
        while(!robsim1.EndBuf())// && robsim1.GetCurrentRelTime() < time(0,300000))
        {
          MeasureItem  const& mItem = robsim1.GetCurrentMeasure();
  
          if(mItem.GetSensedTime() < mItem.GetAvailableTime()) {
            robsim1.GetCurrentInfo(iCont);
            JFR_VDEBUG(iCont << "-- InsertDelayedMeasure\n");
            myPredict.InsertDelayedMeasure(iCont, mItem.GetAvailableTime());
            FillStorage(myPredict, FS_UPDATE, m_storage);
            robsim1.IncCounter();
          }
          else {
                
              // Prediction phase
            horizon = mItem.GetSensedTime();
            myPredict.SetModelParams(now, horizon);
            myPredict.Predict(horizon);
            FillStorage(myPredict, FS_PREDICT, m_storage);
  
            now = horizon;
  
              // Update phase
            do
            {
              robsim1.GetCurrentInfo(iCont);
              myPredict.UpdatePredictionWith(iCont);
              FillStorage(myPredict, FS_UPDATE, m_storage);
              robsim1.IncCounter();
            }
            while(!robsim1.EndBuf() && robsim1.GetCurrentRelTime() == now);

          }
        }
  
        DumpStore(std::string("delay_histo_available.log"),m_storage);
        myPredict.PrintHistory();
        DumpHistory(std::string("delay_histo_filter_avail.log"), myPredict, m_storage);
    }
    } // namespace demo
  } // namespace ddfsimu
} //namespace jafar
#endif