unscentedbase.h

Go to the documentation of this file.

#ifndef GCOP_UNSCENTEDBASE_H
#define GCOP_UNSCENTEDBASE_H

#include "manifold.h"

namespace gcop {

  using namespace std;
  
  template <typename T = VectorXd, 
    int _nx = Dynamic> class UnscentedBase {
  public:
  
  typedef Matrix<double, _nx, 1> Vectornd;
  typedef Matrix<double, _nx, _nx> Matrixnd;

  UnscentedBase(Manifold<T, _nx> &X);
  
  virtual ~UnscentedBase();

  void Points(vector<T> &xs,
              const T& x);
  
  Manifold<T, _nx> &X; 

  double a;      
  double k;      
  double b;      
  
  protected:
  int L;         
  double l;      
  
  vector<T> Xs;   
  
  vector<double> Ws;  
  vector<double> Wc;  

  Matrixnd A;   
  };
  

  
  template <typename T, int _nx> 
    UnscentedBase<T, _nx>::UnscentedBase(Manifold<T, _nx> &X) :
    X(X),
    Xs(2*X.n + 1),
    Ws(2*X.n + 1),
    Wc(2*X.n + 1) {
    
    this->L = X.n;
    this->a = .01;
    // this->a = 1;
    this->k = 0;
    this->b = 2;     
    //    this->b = 0;
    this->l = a*a*(L+k)-L;
    
    for (int i = 0; i < 2*L + 1; ++i) {      
      if (i == 0) {
        Ws[0] = l/(L+l);
        Wc[0] = l/(L+l) + (1-a*a+b);
      } else {
        Ws[i] = 1/(2*(L+l));
        Wc[i] = 1/(2*(L+l));
      }
    }
          
    if (_nx == Dynamic) {
      A.resize(X.n, X.n);
    }
  }
  
  template <typename T, int _nx> 
    UnscentedBase<T, _nx>::~UnscentedBase() {
  }  
  template <typename T, int _nx> 
    void UnscentedBase<T, _nx>::Points(vector<T> &Xs,
                                       const T &x) {
    Xs[0] = x;
    
    A = x.P.llt().matrixL().transpose();
    bool pd = true;
    
    if (!pd) {
      cout << "[W] UKF::Predict: Cholesky failed!" << endl;
    }
    
    //    cout << "A=" << A << endl;
    
    for (int i = 0; i < L; ++i) {
      Vectornd dx = sqrt(L+l)*A.col(i);
      Vectornd dxm = -sqrt(L+l)*A.col(i);
      
      this->X.Retract(Xs[i + 1], x, dx);
      /*
      cout << "#i=" << i << endl;
      cout << "x.R=" << x.R << endl;
      cout << "dx=" << dx.transpose() << endl;
      cout << "R=" << Xs[i+1].R << endl;
      */
      this->X.Retract(Xs[i + 1 + L], x, dxm);
    }
  }
}


#endif