gndoep.h

Go to the documentation of this file.

#ifndef GCOP_GNDOEP_H
#define GCOP_GNDOEP_H

#include "doep.h"
#include "lqsensorcost.h"

#include <unsupported/Eigen/NonLinearOptimization>

namespace gcop {

 
  using namespace std;
  using namespace Eigen;

 
  template <typename T, 
    int _nx, 
    int _nu,
    int _np,
    int _ng,
    typename Tz,
    int _nz,
    typename T1,
    int _nx1> class GnCost;
  
  template <typename T = VectorXd, 
    int _nx = Dynamic, 
    int _nu = Dynamic,
    int _np = Dynamic,
    int _ng = Dynamic,
    typename Tz = VectorXd,
    int _nz = Dynamic,
    typename T1 = T,
    int _nx1 = _nx> 
    class GnDoep : public Doep<T, _nx, _nu, _np, Tz, _nz, T1, _nx1> {
    
    typedef Matrix<double, _ng, 1> Vectorgd;
    typedef Matrix<double, _ng, _nx> Matrixgxd;
    typedef Matrix<double, _ng, _nu> Matrixgud;
    typedef Matrix<double, _ng, _np> Matrixgpd;
    
    typedef Matrix<double, _nx, 1> Vectornd;
    typedef Matrix<double, _nu, 1> Vectorcd;

    typedef Matrix<double, _nx, _nx> Matrixnd;
    typedef Matrix<double, _nx, _nu> Matrixncd;
    typedef Matrix<double, _nu, _nx> Matrixcnd;
    typedef Matrix<double, _nu, _nu> Matrixcd;
    
    typedef Matrix<double, _np, 1> Vectormd;
    
    typedef Matrix<double, _np, _np> Matrixmd;
    typedef Matrix<double, _nx, _np> Matrixnmd;
    typedef Matrix<double, _np, _nx> Matrixmnd;    

    typedef Matrix<double, _nz, 1> Vectorrd;
    typedef Matrix<double, _nz, _nz> Matrixrd;
    typedef Matrix<double, _nz, _nx> Matrixrnd;
    typedef Matrix<double, _nz, _nu> Matrixrcd;
    typedef Matrix<double, _nz, _np> Matrixrmd;

    typedef void(*Func_type)(const T &, T1 &);

  public:
    GnDoep(System<T, _nx, _nu, _np> &sys, Sensor<T1, _nx1, _nu, _np, Tz, _nz> &sensor,
           LqSensorCost<T, _nx, _nu, _np, _ng, Tz, _nz> &cost, 
           vector<double> &ts, vector<T> &xs, vector<Vectorcd> &us, Vectormd &p,
           vector<double> &ts1, Func_type _project=NULL, bool update = true);
    
    virtual ~GnDoep();

    void Iterate();

    int info;
    double fnorm, covfac;

    int inputs;
    int values;
    
    VectorXd s;  
    
    GnCost<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1> *functor;
    NumericalDiff<GnCost<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1> > *numDiff;
    LevenbergMarquardt<NumericalDiff<GnCost<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1> > > *lm;
  };

  
// Generic functor
template<typename _Scalar, int NX=Dynamic, int NY=Dynamic>
struct Functor
{
  typedef _Scalar Scalar;
  enum {
    InputsAtCompileTime = NX,
    ValuesAtCompileTime = NY
  };
  typedef Matrix<Scalar,InputsAtCompileTime,1> InputType;
  typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
  typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

  const int m_inputs, m_values;

  Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
  Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

  int inputs() const { return m_inputs; }
  int values() const { return m_values; }

  // you should define that in the subclass :
//  void operator() (const InputType& x, ValueType* v, JacobianType* _j=0) const;
};


 template <typename T, 
   int _nx = Dynamic,
   int _nu = Dynamic,
   int _np = Dynamic,
   int _ng = Dynamic,
   typename Tz=VectorXd,
   int _nz = Dynamic,
   typename T1 = T,
   int _nx1 = _nx>
   struct GnCost : Functor<double> {  
   GnCost<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>(int inputs, int values) : Functor<double>(inputs, values) {};
   
   GnDoep<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1> *doep;
   
   typedef Matrix<double, _nx, 1> Vectornd;
   typedef Matrix<double, _np, 1> Vectormd;
   typedef Matrix<double, _nu, 1> Vectorcd;
   typedef Matrix<double, _ng, 1> Vectorgd;
   typedef Matrix<double, _nz, 1> Vectorrd;
   
   int operator()(const VectorXd &s, VectorXd &fvec) const
   {
     assert(doep);

     const int &np = doep->sys.P.n;
     const int &nz = doep->sensor.Z.n;
     const int &nw = doep->sys.X.n;
     const int N = doep->us.size();

     //Update inputs: process noise ws[i], p:
     doep->p = s.tail(np);
     for(int i = 0; i < N; i++)
     {
       doep->ws[i] = s.segment(i*nw,nw);
     }
     //Update sensor measurements using the input parameters and noise ws:
     doep->Update(false);

     Vectorgd &g = ((LsSensorCost<T, _nx, _nu, _np, _ng, Tz, _nz>&)doep->cost).g;
     Vectormd &gp = ((LsSensorCost<T, _nx, _nu, _np, _ng, Tz, _nz>&)doep->cost).gp;
     fvec.tail(np).setZero();

     //Compute and set residuals
     int i = 0;
     int sensor_index = 0;
     for(int k = 0; k< N; ++k)
     {
       double h = doep->ts[k+1] - doep->ts[k];
       //cout<<"Res: "<<g.transpose()<<endl;
       //cout<<"i: "<<i<<endl;
       //cout<<"More Time debug Info: "<<doep->ts1[sensor_index]<<"\t"<<doep->ts[k]<<"\t"<<doep->ts[k+1]<<"\t"<<k<<"\t"<<sensor_index<<endl;

       if((doep->ts1[sensor_index] - doep->ts[k])>= 0 && (doep->ts1[sensor_index] - doep->ts[k+1]) < 0)
       {
         while((doep->ts1[sensor_index] - doep->ts[k])>= 0 && (doep->ts1[sensor_index] - doep->ts[k+1]) < 0)//Nearest state to find the sensor measurement
         {
           ((LqSensorCost<T, _nx, _nu, _np, _ng, Tz, _nz>&)doep->cost).Res(g, doep->ts[k], doep->zs[sensor_index], doep->ws[k], doep->p, h, sensor_index);
           fvec.segment(i,nz) = g.segment(nw,nz);
           i += nz;
           sensor_index = sensor_index < (doep->ts1.size()-1)?sensor_index+1:sensor_index;
           if(sensor_index == (doep->ts1.size()-1))
             break;
           fvec.tail(np) += g.tail(np);//The tail is a constant residual for parameters
         }
         fvec.segment(i,nw) = g.head(nw);
         i += nw;
       }
       else
       {
         ((LqSensorCost<T, _nx, _nu, _np, _ng, Tz, _nz>&)doep->cost).Res(g, doep->ts[k], doep->zs[sensor_index], doep->ws[k], doep->p, h, sensor_index);
         memcpy(fvec.data() + i, g.data(), nw*sizeof(double));
         i += nw;
       }
     }
     assert(sensor_index == (doep->ts1.size()-1));//Assert that we collected all the sensor data
     ((LqSensorCost<T, _nx, _nu, _np, _ng, Tz, _nz>&)doep->cost).Resp(gp, doep->p);
     fvec.tail(np) += gp;
     //cout<<"Gp: "<<gp<<endl;//#DEBUG

     if(doep->debug)
     {
       std::cout<<"Input: "<<s.transpose()<<endl;
       std::cout<<"Fvec: "<<fvec.transpose()<<endl;
       //std::cout<<"Resp: "<<fvec.tail(np).transpose()<<"\t"<<doep->p<<endl;
     }
       std::cout<<"Cost: "<<0.5*(fvec.transpose()*fvec)<<endl;
     //getchar();
     return 0;
   }
  };

  using namespace std;
  using namespace Eigen;
  
  template <typename T, int _nx, int _nu, int _np, int _ng, typename Tz, int _nz, typename T1, int _nx1> 
    GnDoep<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>::GnDoep(System<T, _nx, _nu, _np> &sys, Sensor<T1, _nx1, _nu, _np, Tz, _nz> &sensor,
                                                LqSensorCost<T, _nx, _nu, _np, _ng, Tz, _nz> &cost,
                                                vector<double> &ts, 
                                                vector<T> &xs, 
                                                vector<Vectorcd > &us,
                                                Vectormd &p,
                                                vector<double> &ts1, 
                                                Func_type _project,
                                                bool update) : 
    Doep<T, _nx, _nu, _np, Tz, _nz, T1, _nx1>(sys, sensor, cost, ts, xs, us, p, ts1, _project, update),
    inputs(us.size()*sys.X.n + sys.P.n),
    values((sys.X.n)*us.size()+(sensor.Z.n)*ts1.size()+sys.P.n), s(inputs), 
    functor(0), numDiff(0), lm(0)
    {
      cout <<"inputs=" <<inputs<<" values= "<<values<< endl;
    }
  
  template <typename T, int _nx, int _nu, int _np, int _ng, typename Tz, int _nz, typename T1, int _nx1> 
    GnDoep<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>::~GnDoep()
    {
      delete lm;
      delete numDiff;
      delete functor;
    }  
  
  template <typename T, int _nx, int _nu, int _np, int _ng, typename Tz, int _nz, typename T1, int _nx1> 
    void GnDoep<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>::Iterate() {

    if (!lm) {
      functor = new GnCost<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>(inputs, values);
      functor->doep = this;
      numDiff = new NumericalDiff<GnCost<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1> >(*functor);
      lm = new LevenbergMarquardt<NumericalDiff<GnCost<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1> > >(*numDiff);

      s.setZero();//Set the initial noise ws to zero
      const int &np = this->sys.P.n;
      s.tail(np) = this->p;//Set the system parameters to initial guess
    }

    /*
    for (int i = 0, k = 0; k < this->us.size(); ++k) {
      memcpy(s.data() + i, this->us[k].data(), this->sys.U.n*sizeof(double));
      i += this->sys.U.n;
    }
    */

    //    lm.parameters.maxfev=10000;
    info = lm->minimize(s);
    
    cout <<"info="<<info <<endl;
    // check return values
    // VERIFY_IS_EQUAL(info, 1);
    //   VERIFY_IS_EQUAL(lm.nfev(), 26);
  }
}

#endif