constraintModel.hpp

/* $Id$ */

#ifndef FILTER_CONSTRAINT_MODEL
#define FILTER_CONSTRAINT_MODEL

#include "kernel/dataLog.hpp"

#include "jmath/jblas.hpp"

namespace jafar {
  namespace filter {
    
    class JacobianStrongConstraintModel : public jafar::kernel::DataLoggable {

    protected:

      std::size_t p_sizeState;

      double p_value;

      double p_alpha;

      double p_R0;

      bool p_doApplyConstraint;

      double p_R;

      double p_relativeStrength;

      mutable double p_estimatedValue;

      int p_nbTimesApplied;

      int p_maxNbTimesApplied;

      static const double applyThreshold;

      virtual void writeLogHeader(jafar::kernel::DataLogger& log) const;
      virtual void writeLogData(jafar::kernel::DataLogger& log) const;

    public:

      jblas::mat J;

      JacobianStrongConstraintModel(std::size_t sizeState,
            double value,
            double alpha,
            int maxNbTimesApplied = -1) :
  p_sizeState(sizeState), p_value(value), p_alpha(alpha),
  p_R0(0), p_doApplyConstraint(false), p_R(0), p_relativeStrength(0),
  p_estimatedValue(0),
  p_nbTimesApplied(0),
  p_maxNbTimesApplied(maxNbTimesApplied),
  J(1,sizeState)
      {}

      virtual ~JacobianStrongConstraintModel() {}

      std::size_t sizeState() const {return p_sizeState;}
      double value() const {return p_value;}

      bool doApplyConstraint(jblas::vec_range const& x,
           jblas::sym_mat_range const& xCov);

      virtual double estimateValue(jblas::vec_range const& x0) const = 0;
      virtual void estimateValueJac(jblas::vec_range const& x0) = 0;

      virtual double weakeningVariance();

    };

  } // namespace filter
} // namespace jafar

#endif // FILTER_CONSTRAINT_MODEL