multilateration.h

Go to the documentation of this file.

#ifndef MULTILATERATION_H
#define MULTILATERATION_H

#include <itpp/itbase.h>
#include <itpp/itexports.h>

namespace itpp
{

class Algorithm;
struct Point;

class ITPP_EXPORT Multilateration
{
public:
  enum Type {MULTI_FAILURE = -1, 
             MULTI_SPHERICAL, 
             MULTI_HYPERBOLIC, 
             MULTI_HYBRID 
            };
  Multilateration() :
    algo_(NULL), nb_fails_part(0), nb_fails_pos(0), type_(MULTI_FAILURE), method_(itpp::bvec()), bs_pos_(NULL), nb_bs_(0)
    {}

  Multilateration(const itpp::bvec &method, 
                  const itpp::mat &bs_pos 
                 ) :
    algo_(NULL), nb_fails_part(0), nb_fails_pos(0), type_(MULTI_FAILURE), method_(itpp::bvec()), bs_pos_(NULL), nb_bs_(0) {
    setup(method, bs_pos);
  }
  virtual ~Multilateration();

  void setup(const itpp::bvec &method, 
             const itpp::mat &bs_pos 
            ) {
    if((false == set_bs_pos(bs_pos)) || (false == set_method(method))) {
      it_error("cannot init multilateration");
    }
  }

  bool get_pos(itpp::vec &ms_pos, 
               const itpp::vec &measures 
              ) {
    return get_pos(ms_pos, measures._data());
  }

  bool get_pos(itpp::vec &ms_pos, 
               const itpp::mat &measures 
              ) {
    return get_pos(ms_pos, measures._data());
  }
  unsigned int get_nb_fails_part() const {
    return nb_fails_part;
  }
  unsigned int get_nb_fails_pos() const {
    return nb_fails_pos;
  }
  void reset_err_counters() {
    nb_fails_part = 0;
    nb_fails_pos = 0;
  }
  Type get_type() const {
    return type_;
  }
  double get_crlb(const vec &ms_pos, 
                  double sigma2 
                 );
private:
  bool get_pos(itpp::vec &ms_pos, const double *measures);
  bool set_method(const itpp::bvec &method);
  bool set_bs_pos(const itpp::mat &bs_pos);
  bool hybrid2spherical(Point *bs_pos, double *meas);
  bool partition(unsigned int **subsets_idx, unsigned int *subsets_nb, const Point *bs_pos, unsigned int nb_bs, unsigned int subset_len);
  bool get_ml_pos(Point *ms_pos, const Point *bs_pos, unsigned int nb_bs, const unsigned int *subsets_idx, unsigned int subsets_nb, unsigned int subset_len);
  bool get_bs_pos_subset(Point *bs_pos_subset, const Point *bs_pos, unsigned int nb_bs, const unsigned int *subset_idx, unsigned int subset_len);
  bool prod(double *out, const double *AT, const unsigned int *d, unsigned int cols, unsigned int rows);
  Algorithm *algo_;
  unsigned int nb_fails_part;
  unsigned int nb_fails_pos;
  Type type_;
  itpp::bvec method_;
  Point *bs_pos_;
  unsigned int nb_bs_;
};

}

#endif