GMPHDLinear2DTargetNoIdFilter.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 __GMPHD_FILTER_H
#define __GMPHD_FILTER_H

#include <Spaces.h>
#include <GaussianMixture.h>
#include "PHDFilter.h"

using namespace MipBaselib;
using namespace arma;

namespace MipAlgorithms{
 
 /* @{ */
 
 class GMPHDLinear2DTargetNoIdFilterPars:public PHDFilterPars{
  public:
   
   // system model
   DMat A;
   DMat N_n_x;
   
   // measurement model
   DMat H;
   DMat N_n_z;
   
   // pruning thresholds
   Decimal truncThresh;
   int maxNumComponents;
   Decimal mergThresh;
   
   // constructors
   GMPHDLinear2DTargetNoIdFilterPars();
   GMPHDLinear2DTargetNoIdFilterPars(Decimal pS, Decimal pD, Time t, DMat& a, DMat& nx, DMat& h, DMat& nz, Decimal tt, int mc, Decimal mt);
 };
 
 
 class GMPHDLinear2DTargetNoIdFilterVars:public PHDFilterVars{
   
  public:
   GaussianMixture prediction;
   
   vector<DCol> predictedMeasures;
   vector<DMat> predictedCovariances;
   vector<DMat> kalmanGains;
   vector<DMat> stepKCovariances;
   
   GaussianMixture belief;
   
   GMPHDLinear2DTargetNoIdFilterVars();
 };
 
 
 class GMPHDLinear2DTargetNoIdFilter:public PHDFilter{
  private:
   GMPHDLinear2DTargetNoIdFilterVars* _gmvars;
   GMPHDLinear2DTargetNoIdFilterPars* _gmpars;

  public:
   GMPHDLinear2DTargetNoIdFilter();
   
   GMPHDLinear2DTargetNoIdFilter(GMPHDLinear2DTargetNoIdFilterPars inParams);
   
   void step(PHDFilterIn &in);
   
   string print();

   void getBelief(GaussianMixture &gm);
   
   string getObjectName() const {
    return "GMPHDLinear2DTargetNoIdFilter";
   }
   
   PHDFilterOutputType getOutputType(){
    return PHD_OUT_GMID;
   }
   
  protected:
   void _predictBirthTarget();
   void _predictExistingTarget();
   void _buildPHDUpdateComponents();
   void _updateWithMeasurements(PHDFilterIn &in);
   void _prune();
   void _prune2();

   void _initialize(PHDFilterIn &in);
   void _normalStep(PHDFilterIn &in);
   
   Decimal _mahalanobisDist(GaussianComponent& a, GaussianComponent& b);
   
 };
 
 /* @} */
 
}


#endif