DAEKF.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------



/* @{ */

#ifndef __DAEKF_H
#define __DAEKF_H

#include "MutLoc.h"

#define CLMEMORY       100
#define KILLPERCENT    0.85
#define INITVALUE      0.8
#define MAXCLSTNUMBER  500

namespace MipAlgorithms{
 class Cluster{
  public:
   Pose     mean;
   MIPMatrix  *cov;
   Decimal              value;
   bool                miss[CLMEMORY];
   int                 firstIn;
   int                 life;
   int                 id;
   
   Cluster();
   
   Cluster(Pose tMean, MIPMatrix &tCov, Decimal tValue = INITVALUE*((Decimal)CLMEMORY), int ID = 0);
   
   Cluster(const Cluster &C);
   
   ~Cluster();
   
   void operator=(Cluster C);
   
   string print();
 };
};// end MipAlgorithms


namespace MipAlgorithms{
 class DAEKFilParams {
  public:
   Decimal minProbTh;
   Decimal maxDistTh;
   Decimal minSurvTh;
   FilterType filterType;
   int secsForPhase2;
   
   DAEKFilParams();
   
   DAEKFilParams(Decimal p, Decimal d, Decimal s, FilterType t, int secs);
   
   DAEKFilParams(const DAEKFilParams &A);
   
   void operator= (const DAEKFilParams &A);
 };
};// end namespace MipAlgorithms


namespace MipAlgorithms{
 class DAEKFilVariables{
  public:
   int step;
   int phase;
   int clustersNumber;
   vector<Cluster> clusters;
   MIPMatrix *Vjacobian;
   MIPMatrix *VjacobianTrans;
   vector<MIPMatrix>  VRVT;
   Decimal       wrongClustProb;
   Decimal       bestValue;
   int createdClusterNumber;
   timeval startTime;
  
   DAEKFilVariables();
   
   DAEKFilVariables(const DAEKFilVariables &A);
   
   void operator=(DAEKFilVariables &A);
   
   ~DAEKFilVariables();
 };
};//end namespace MipAlgorithms

namespace MipAlgorithms{
 class DAEKFilter : public MutLocFilter{
  public:
   
   DAEKFilter();
   
   DAEKFilter(DAEKFilParams in);
   
   void step(MutLocFilInput &input);
   
   void reset();
   
   int getEstimate(Pose *best);
   
   int getAllEstimates(vector<Pose> &all, vector<Decimal> &allMarks);
   
   string print();
   
   void reset(DAEKFilParams inPar);
   
   int getStep();
   
   int getPhase();
   
   int getClustersNumber();
   
   Pose getClustersMean(int num);
   
   Decimal getClustersValue(int num);
   
  protected:
   DAEKFilParams    par;
   DAEKFilVariables *var;
   
  private:
   inline Decimal lfilComputeError(Pose delta);
   inline void   lfilEvaluate(Decimal error, int partNum);
   inline void   lfilNormalize();
   inline Decimal lfilComputeDistance(Pose measure, Pose clusterMean, int numMeas);
   void lfilKalman(int clNum, Pose meas,int numMeas);
   void lfilAddCluster(Pose t,int numMeas);
   
   void lfilVjacobiansUpdate(MutLocFilInput &input);
   void lfilVRVTUpdate(MutLocFilInput &input);
   
   void lfilKInitStep(MutLocFilInput &input); //filtraggio di tipo Kalman
   void lfilKAlgoStep(MutLocFilInput &input); //filtraggio di tipo Kalman
 };
};//end namespace MipAlgorithms




#endif





/* @} */