IT++ Logo
multilateration.cpp
Go to the documentation of this file.
1 
30 #ifdef _MSC_VER
31 #include <float.h>
32 #define isfinite _finite
33 #endif
34 
35 using namespace std;
36 
37 namespace itpp
38 {
39 
40 struct Point {
41  double x;
42  double y;
43  double z;
44 };
45 
46 //class for managing a set of integers (indices)
47 class IndexSet
48 {
49 public:
50  IndexSet(unsigned int max_size) :
51  size_(0), max_size_(max_size), data_(new unsigned int[max_size]) {}
52  unsigned int get_size() const {
53  return size_;
54  }
55  ~IndexSet() {
56  delete[] data_;
57  }
58  unsigned int& operator[](unsigned int n) {
59  return data_[n];
60  }
61  unsigned int operator[](unsigned int n) const {
62  return data_[n];
63  }
64  void set_size(unsigned int size) {
65  size_ = size;
66  }
67  bool exist(unsigned int elem) {
68  bool res = false;
69  for(unsigned int i = 0; i < size_; ++i) {
70  if(data_[i] == elem) {
71  res = true;
72  break;
73  }
74  }
75  return res;
76  }
77  bool insert(unsigned int elem) {
78  bool res = false;
79  if((size_ < max_size_) && (false == exist(elem))) {
80  data_[size_++] = elem;
81  res = true;
82  }
83  return res;
84  }
85  bool remove(unsigned int elem) {
86  bool res = false;
87  for(unsigned int i = 0; i < size_; ++i) {
88  if(data_[i] == elem) {
89  if((i + 1) < size_) {
90  memcpy(data_ + i, data_ + i + 1, (size_ - i - 1)*sizeof(*data_));
91  }
92  --size_;
93  res = true;
94  break;
95  }
96  }
97  return res;
98  }
99 private:
100  unsigned int size_;
101  unsigned int max_size_;
102  unsigned int *data_;
103 };
104 
105 //helper class
106 class PosHelper
107 {
108 public:
109  static double get_dist(const Point *p0, const Point *p1) {
110  double out = 0.0;
111  double *coord_p0 = (double*)p0;
112  double *coord_p1 = (double*)p1;
113  for(int n = 0; n < 3; ++n) {
114  out += (coord_p1[n] - coord_p0[n]) * (coord_p1[n] - coord_p0[n]);
115  }
116  return std::sqrt(out);
117  }
118  static bool test_uniqueness(const Point *bs_pos, int bs_pos_len, double eps) {
119  int i, j;
120 
121  for(i = bs_pos_len - 1; i >= 0; --i) {
122  for(j = i - 1; j >= 0; --j) {
123  if((fabs(bs_pos[i].x - bs_pos[j].x) < eps) && (fabs(bs_pos[i].y - bs_pos[j].y) < eps) &&
124  (fabs(bs_pos[i].z - bs_pos[j].z) < eps)) {
125  it_warning("found 2 identical BSs");
126  return false;
127  }
128  }
129  }
130  return true;
131  }
132  static double det_3by3(const double mat[9]) {
133  return mat[0] * (mat[4] * mat[8] - mat[5] * mat[7]) + mat[3] * (mat[2] * mat[7] - mat[1] * mat[8]) + mat[6] * (mat[1] * mat[5] - mat[2] * mat[4]);
134  }
135  static double det_4by4(const double mat[16]) {
136  return mat[0] * mat[5] * mat[10] * mat[15] + mat[0] * mat[9] * mat[14] * mat[7] + mat[0] * mat[13] * mat[6] * mat[11] +
137  mat[4] * mat[1] * mat[14] * mat[11] + mat[4] * mat[9] * mat[2] * mat[15] + mat[4] * mat[13] * mat[10] * mat[3] +
138  mat[8] * mat[1] * mat[6] * mat[15] + mat[8] * mat[5] * mat[14] * mat[3] + mat[8] * mat[13] * mat[2] * mat[7] +
139  mat[12] * mat[1] * mat[10] * mat[7] + mat[12] * mat[5] * mat[2] * mat[11] + mat[12] * mat[9] * mat[6] * mat[3] -
140  mat[0] * mat[5] * mat[14] * mat[11] - mat[0] * mat[9] * mat[6] * mat[15] - mat[0] * mat[13] * mat[10] * mat[7] -
141  mat[4] * mat[1] * mat[10] * mat[15] - mat[4] * mat[9] * mat[14] * mat[3] - mat[4] * mat[13] * mat[2] * mat[11] -
142  mat[8] * mat[1] * mat[14] * mat[7] - mat[8] * mat[5] * mat[2] * mat[15] - mat[8] * mat[13] * mat[6] * mat[3] -
143  mat[12] * mat[1] * mat[6] * mat[11] - mat[12] * mat[5] * mat[10] * mat[3] - mat[12] * mat[9] * mat[2] * mat[7];
144  }
145  static bool inv_3by3(double *inv, const double *mat) {
146  double det = det_3by3(mat);
147  if((0 == det) || (0 == isfinite(det))) {
148  memset(inv, 0, 9 * sizeof(*inv));
149  return false;
150  }
151  inv[0] = (mat[4] * mat[8] - mat[5] * mat[7]) / det;
152  inv[1] = (mat[2] * mat[7] - mat[1] * mat[8]) / det;
153  inv[2] = (mat[1] * mat[5] - mat[2] * mat[4]) / det;
154  inv[3] = (mat[5] * mat[6] - mat[3] * mat[8]) / det;
155  inv[4] = (mat[0] * mat[8] - mat[2] * mat[6]) / det;
156  inv[5] = (mat[2] * mat[3] - mat[0] * mat[5]) / det;
157  inv[6] = (mat[3] * mat[7] - mat[4] * mat[6]) / det;
158  inv[7] = (mat[1] * mat[6] - mat[0] * mat[7]) / det;
159  inv[8] = (mat[0] * mat[4] - mat[1] * mat[3]) / det;
160 
161  return true;
162  }
163  static bool test_noncoplanar(const Point *bs_pos, int bs_pos_len) {
164  double mat[3 * 3];
165  double *coord[] = {NULL, NULL};
166  int i;
167  int j;
168 
169  if((NULL == bs_pos) || (4 != bs_pos_len)) {
170  it_warning("4 points are needed");
171  return false;
172  }
173 
174  coord[0] = (double*)bs_pos;
175  for(i = 0; i < 3; ++i) {
176  coord[1] = (double*)(bs_pos + i + 1);
177  for(j = 0; j < 3; ++j) {
178  mat[i + 3 * j] = (*(coord[1] + j)) - (*(coord[0] + j));
179  }
180  }
181  return (0 != det_3by3(mat)) ? true : false;
182  }
183  static bool combination(unsigned int *comb_mat, unsigned int *nb_cols, unsigned int nb_rows, unsigned int len) {
184  unsigned int idx = 0;
185  unsigned int j, k, l, m;
186 
187  if((NULL == comb_mat) || (NULL == nb_cols)) {
188  it_warning("invalid input");
189  return false;
190  }
191  if(nb_rows > len) {
192  //it_warning("number of rows superior to total length");
193  return false;
194  }
195 
196  switch(nb_rows) {
197  case 5: {
198  for(j = 1; j < len; ++j) {
199  for(k = j + 1; k < len; ++k) {
200  for(l = k + 1; l < len; ++l) {
201  for(m = l + 1; m < len; ++m) {
202  comb_mat[idx++] = 0;
203  comb_mat[idx++] = j;
204  comb_mat[idx++] = k;
205  comb_mat[idx++] = l;
206  comb_mat[idx++] = m;
207  }
208  }
209  }
210  }
211  }
212  break;
213  case 4: {
214  for(j = 1; j < len; ++j) {
215  for(k = j + 1; k < len; ++k) {
216  for(l = k + 1; l < len; ++l) {
217  comb_mat[idx++] = 0;
218  comb_mat[idx++] = j;
219  comb_mat[idx++] = k;
220  comb_mat[idx++] = l;
221  }
222  }
223  }
224  }
225  break;
226  case 3: {
227  for(j = 1; j < len; ++j) {
228  for(k = j + 1; k < len; ++k) {
229  comb_mat[idx++] = 0;
230  comb_mat[idx++] = j;
231  comb_mat[idx++] = k;
232  }
233  }
234  }
235  break;
236  case 2: {
237  for(j = 1; j < len; ++j) {
238  comb_mat[idx++] = 0;
239  comb_mat[idx++] = j;
240  }
241  }
242  break;
243  default:
244  it_warning("invalid nb_rows");
245  return false;
246  }
247 
248  *nb_cols = idx / nb_rows;
249  return true;
250  }
251  static unsigned int comb_nb(unsigned int n, unsigned int k) {
252  unsigned int i;
253  unsigned int nom = 1;
254  unsigned int denom = 1;
255 
256  for(i = k + 1; i <= n; ++i) {
257  nom *= i;
258  }
259 
260  for(i = 2; i <= (n - k); ++i) {
261  denom *= i;
262  }
263  return nom / denom;
264  }
265  static bool update_subsets(IndexSet &used_subset_idx, IndexSet &unused_subset_idx,
266  const unsigned int *subset_idx, unsigned int subset_idx_len) {
267  unsigned int i;
268  bool res = true;
269  for(i = 0; i < subset_idx_len; ++i) {
270  if((true == used_subset_idx.insert(subset_idx[i])) &&
271  (false == unused_subset_idx.remove(subset_idx[i]))) {
272  it_warning("cannot update subsets");
273  res = false;
274  break;
275  }
276  }
277  return res;
278  }
279  static bool store_subset(unsigned int **subsets, unsigned int *in_subset, unsigned int in_subset_len, unsigned int in_subset_idx) {
280  bool rc = false;
281 
282  /* realloc memory */
283  *subsets = (unsigned int*)realloc(*subsets, (in_subset_idx + 1) * in_subset_len * sizeof(**subsets));
284  if(NULL != *subsets) {
285  /* copy subset */
286  memcpy((*subsets) + in_subset_idx * in_subset_len, in_subset, in_subset_len * sizeof(*in_subset));
287  rc = true;
288  }
289  else {
290  it_warning("cannot allocate memory");
291  }
292  return rc;
293  }
294 };
295 
296 // interface for multilateration algorithms: spherical or hyperbolic
297 class Algorithm
298 {
299 public:
300  enum GeoPos {GEO_POS_NO = 0, GEO_POS_YES, GEO_POS_EXACT};
301  virtual GeoPos validate(const Point *bs_pos, unsigned int bs_pos_len, const double *r) = 0;
302  virtual bool setup(const Point *bs_pos, unsigned int bs_pos_len) = 0;
303  /* compute an unique MS position */
304  virtual bool get_pos(Point *ms_pos, const double *range, unsigned int range_len) = 0;
305  virtual void set_meas_mat(const double *meas) = 0;
306  virtual void get_meas(double *meas, const unsigned int *bs_subset_idx, unsigned int len) = 0;
307  virtual bool get_meas_mult_mat(const unsigned int *bs_pos_idx, unsigned int rows, unsigned int cols) = 0;
308  virtual void get_meas_mult(unsigned int *meas_mult, const unsigned int *subset_idx, unsigned int meas_mult_len) = 0;
309  virtual bool get_grad(double *grad, const Point *bs_pos, unsigned int bs_nb, const Point *ms_pos) = 0;
310  virtual double* get_meas_mat() const = 0;
311  virtual unsigned int* get_mult_mat() const = 0;
312  virtual ~Algorithm() {}
313 };
314 
315 class Spherical : public Algorithm
316 {
317 public:
318  explicit Spherical(unsigned int nb_bs);
319  virtual ~Spherical();
320  virtual Algorithm::GeoPos validate(const Point *bs_pos, unsigned int bs_pos_len, const double *r);
321  virtual bool setup(const Point *bs_pos, unsigned int bs_pos_len);
322  virtual bool get_pos(Point *ms_pos, const double *range, unsigned int range_len);
323  virtual void set_meas_mat(const double *meas) {
324  memcpy(meas_mat_, meas, nb_bs_ * sizeof(*meas));
325  }
326  virtual void get_meas(double *meas, const unsigned int *bs_subset_idx, unsigned int len);
327  virtual bool get_meas_mult_mat(const unsigned int *bs_pos_idx, unsigned int rows, unsigned int cols);
328  virtual void get_meas_mult(unsigned int *meas_mult, const unsigned int *subset_idx, unsigned int meas_mult_len);
329  virtual bool get_grad(double *grad, const Point *bs_pos, unsigned int bs_nb, const Point *ms_pos);
330  virtual double* get_meas_mat() const {
331  return meas_mat_;
332  }
333  virtual unsigned int* get_mult_mat() const {
334  return mult_mat_;
335  }
336 private:
337  double mult_vTMw(double v[2], double M[4], double w[2]) {
338  return w[0] * (M[0] * v[0] + M[1] * v[1]) + w[1] * (M[2] * v[0] + M[3] * v[1]);
339  }
340  double comp_quadratic(double *u, const double *range) {
341  double v[10];
342  double scalar;
343  int i;
344 
345  v[0] = u[0] = 1;
346  v[1] = u[1] = range[0] * range[0];
347  v[2] = u[2] = range[1] * range[1];
348  v[3] = u[3] = range[2] * range[2];
349 
350  v[4] = u[1] * u[1];
351  v[5] = u[2] * u[2];
352  v[6] = u[3] * u[3];
353  v[7] = u[1] * u[2];
354  v[8] = u[1] * u[3];
355  v[9] = u[2] * u[3];
356 
357  /*could generate numerical overflows*/
358  scalar = xi[0] * v[0];
359  for(i = 1; i < 10; ++i) {
360  scalar += xi[i] * v[i];
361  }
362  return scalar;
363  }
364  //data members
365  Point bs_pos_;
366  unsigned int bs_pos_len_;
367  double xi[10];
368  double Lambda[12];
369  double mu[6];
370 
371  unsigned int nb_bs_;
372  double *meas_mat_;
373  unsigned int *mult_mat_;
374 };
375 
376 Spherical::Spherical(unsigned int nb_bs) :
377  nb_bs_(nb_bs),
378  meas_mat_(new double[nb_bs]),
379  mult_mat_(new unsigned int [nb_bs])
380 {
381 }
382 
383 Spherical::~Spherical()
384 {
385  delete[] meas_mat_;
386  delete[] mult_mat_;
387 }
388 
389 /*validate a given set of BSs and the associated measurements*/
390 Algorithm::GeoPos Spherical::validate(const Point *bs_pos, unsigned int bs_pos_len, const double *r)
391 {
392  GeoPos out = GEO_POS_NO;
393 
394  if(3 > bs_pos_len) {
395  it_warning("invalid input");
396  return GEO_POS_NO;
397  }
398  if(true == Spherical::setup(bs_pos, bs_pos_len)) {
399  double u[4];/*not used*/
400  double scalar = comp_quadratic(u, r);
401  if(0 == scalar) {
402  out = GEO_POS_EXACT;
403  }
404  else if((0 < scalar) && (1 == bs_pos_len_)) {
405  out = GEO_POS_YES;
406  }
407  else {
408  out = GEO_POS_NO;
409  }
410  }
411  return out;
412 }
413 
414 bool Spherical::setup(const Point *bs_pos, unsigned int bs_pos_len)
415 {
416  double S[3];
417  double W[4];
418  double detW;
419  double invW[4];
420  double G[4];/*W^{-T}*W^{-1}*/
421  double d[2];
422  double rh1[2];
423  double a;
424  double delta[2];
425  double e[2];
426  double lambda[4];
427  double w1Td;
428  double w2Td;
429  int i;
430 
431  if(3 > bs_pos_len) {
432  it_warning("at least 3 BSs are needed");
433  return false;
434  }
435 
436  memset(xi, 0, sizeof(xi));
437  memset(Lambda, 0, sizeof(Lambda));
438  memset(mu, 0, sizeof(mu));
439  bs_pos_len_ = 0;
440 
441  for(i = 0; i < 3; ++i) {
442  S[i] = bs_pos[i].x * bs_pos[i].x + bs_pos[i].y * bs_pos[i].y + bs_pos[i].z * bs_pos[i].z;
443  }
444  W[0] = bs_pos[1].x - bs_pos[0].x;
445  W[1] = bs_pos[2].x - bs_pos[0].x;
446  W[2] = bs_pos[1].y - bs_pos[0].y;
447  W[3] = bs_pos[2].y - bs_pos[0].y;
448  detW = W[0] * W[3] - W[1] * W[2];
449  if(0 == detW) {
450  //it_warning("please make sure that the 3 known points are not colinear");
451  return false;
452  }
453  invW[0] = W[3] / detW;
454  invW[1] = -W[1] / detW;
455  invW[2] = -W[2] / detW;
456  invW[3] = W[0] / detW;
457  G[0] = invW[0] * invW[0] + invW[1] * invW[1];
458  G[1] = invW[0] * invW[2] + invW[1] * invW[3];
459  G[2] = G[1];
460  G[3] = invW[2] * invW[2] + invW[3] * invW[3];
461  rh1[0] = bs_pos[0].x;
462  rh1[1] = bs_pos[0].y;
463  for(i = 0; i < 2; ++i) {
464  d[i] = bs_pos[i + 1].z - bs_pos[0].z;
465  delta[i] = S[0] - S[i + 1];
466  }
467  for(i = 0; i < 2; ++i) {
468  e[i] = 0.5 * (delta[0] * G[2 * i] + delta[1] * G[2 * i + 1]) + rh1[0] * invW[2 * i] + rh1[1] * invW[2 * i + 1];
469  }
470  a = 1.0 + mult_vTMw(d, G, d);
471 
472  lambda[0] = -(2 * mult_vTMw(rh1, invW, d) - 2 * bs_pos[0].z + mult_vTMw(d, G, delta)) / (2 * a);
473  lambda[1] = ((bs_pos[1].z - bs_pos[0].z) * (G[0] + G[1]) + (bs_pos[2].z - bs_pos[0].z) * (G[1] + G[3])) / (2 * a);
474  lambda[2] = -((bs_pos[1].z - bs_pos[0].z) * G[0] + (bs_pos[2].z - bs_pos[0].z) * G[1]) / (2 * a);
475  lambda[3] = -((bs_pos[2].z - bs_pos[0].z) * G[3] + (bs_pos[1].z - bs_pos[0].z) * G[1]) / (2 * a);
476 
477  xi[0] = lambda[0] * lambda[0] - (0.25 * mult_vTMw(delta, G, delta) + mult_vTMw(rh1, invW, delta) + S[0]) / a;
478  xi[1] = 2 * lambda[0] * lambda[1] + (1 + e[0] + e[1]) / a;
479  xi[2] = 2 * lambda[0] * lambda[2] - e[0] / a;
480  xi[3] = 2 * lambda[0] * lambda[3] - e[1] / a;
481  xi[4] = lambda[1] * lambda[1] - (G[0] + 2 * G[1] + G[3]) / (4 * a);
482  xi[5] = lambda[2] * lambda[2] - G[0] / (4 * a);
483  xi[6] = lambda[3] * lambda[3] - G[3] / (4 * a);
484  xi[7] = 2 * lambda[1] * lambda[2] + (G[0] + G[1]) / (2 * a);
485  xi[8] = 2 * lambda[1] * lambda[3] + (G[1] + G[3]) / (2 * a);
486  xi[9] = 2 * lambda[2] * lambda[3] - G[1] / (2 * a);
487 
488  w1Td = invW[0] * d[0] + invW[2] * d[1];
489  w2Td = invW[1] * d[0] + invW[3] * d[1];
490  /*row 0*/
491  Lambda[0] = -w1Td * lambda[0] - (invW[0] * delta[0] + invW[2] * delta[1]) / 2;
492  Lambda[3] = -w1Td * lambda[1] + (bs_pos[2].y - bs_pos[1].y) / (2 * detW);
493  Lambda[6] = -w1Td * lambda[2] - (bs_pos[2].y - bs_pos[0].y) / (2 * detW);
494  Lambda[9] = -w1Td * lambda[3] + (bs_pos[1].y - bs_pos[0].y) / (2 * detW);
495  /*row 1*/
496  Lambda[1] = -w2Td * lambda[0] - (invW[1] * delta[0] + invW[3] * delta[1]) / 2;
497  Lambda[4] = -w2Td * lambda[1] + (bs_pos[1].x - bs_pos[2].x) / (2 * detW);
498  Lambda[7] = -w2Td * lambda[2] + (bs_pos[2].x - bs_pos[0].x) / (2 * detW);
499  Lambda[10] = -w2Td * lambda[3] - (bs_pos[1].x - bs_pos[0].x) / (2 * detW);
500  /*row 2*/
501  Lambda[2] = lambda[0];
502  Lambda[5] = lambda[1];
503  Lambda[8] = lambda[2];
504  Lambda[11] = lambda[3];
505 
506  mu[0] = -w1Td;
507  mu[1] = -w2Td;
508  mu[2] = 1;
509 
510  mu[3] = w1Td;
511  mu[4] = w2Td;
512  mu[5] = -1;
513 
514  if(4 == bs_pos_len) {
515  if(false == PosHelper::test_noncoplanar(bs_pos, bs_pos_len)) {
516  //it_warning("4 noncoplanar BSs are needed");
517  return false;
518  }
519  bs_pos_ = bs_pos[3];
520  bs_pos_len_ = 1;
521  }
522 
523  return true;
524 }
525 
526 bool Spherical::get_pos(Point *ms_pos, const double *range, unsigned int range_len)
527 {
528  double u[4];
529  double scalar;
530  int i;
531  int j;
532  double *coord = NULL;
533 
534  if(3 > range_len) {
535  it_warning("at least 3 measurements are needed");
536  return false;
537  }
538  if(0 == bs_pos_len_) {
539  it_warning("geo_spheric_setup needs to be called first");
540  return false;
541  }
542 
543  scalar = comp_quadratic(u, range);
544  if(scalar < 0) {
545  it_warning("square root from negative number");
546  scalar = std::sqrt(-scalar);
547  }
548  else {
549  scalar = std::sqrt(scalar);
550  }
551 
552  /* first solution */
553  coord = (double*)ms_pos;
554  for(j = 0; j < 3; ++j) {
555  coord[j] = mu[j] * scalar;
556  for(i = 0; i < 4; ++i) {
557  coord[j] += Lambda[3 * i + j] * u[i];
558  }
559  }
560 
561  if(0 != scalar) {
562  double r1[2];
563  Point tmp_pos;
564 
565  /* second solution */
566  coord = (double*)(&tmp_pos);
567  for(j = 0; j < 3; ++j) {
568  coord[j] = mu[3 + j] * scalar;
569  for(i = 0; i < 4; ++i) {
570  coord[j] += Lambda[3 * i + j] * u[i];
571  }
572  }
573  /*smallest distance wrt the fourth measure gives the position*/
574  r1[0] = fabs(PosHelper::get_dist(ms_pos, &bs_pos_) - range[3]);
575  r1[1] = fabs(PosHelper::get_dist(&tmp_pos, &bs_pos_) - range[3]);
576  if(r1[0] > r1[1]) {
577  *ms_pos = tmp_pos;
578  }
579  }
580 
581  return true;
582 }
583 
584 void Spherical::get_meas(double *meas, const unsigned int *bs_subset_idx, unsigned int len)
585 {
586  unsigned int i;
587  for(i = 0; i < len; ++i) {
588  meas[i] = meas_mat_[bs_subset_idx[i]];
589  }
590 }
591 
592 bool Spherical::get_meas_mult_mat(const unsigned int *bs_pos_idx, unsigned int rows, unsigned int cols)
593 {
594  memset(mult_mat_, 0, nb_bs_ * sizeof(*mult_mat_));
595  if(1 == rows) {
596  it_warning("nothing to do");
597  return true;
598  }
599  for(unsigned int i = 0; i < (rows * cols); ++i) {
600  if(nb_bs_ <= bs_pos_idx[i]) {
601  return false;
602  }
603  ++mult_mat_[bs_pos_idx[i]];
604  }
605  return true;
606 }
607 
608 void Spherical::get_meas_mult(unsigned int *meas_mult, const unsigned int *subset_idx, unsigned int meas_mult_len)
609 {
610  for(unsigned int i = 0; i < meas_mult_len; ++i) {
611  meas_mult[i] = mult_mat_[subset_idx[i]];
612  }
613 }
614 
615 bool Spherical::get_grad(double *grad, const Point *bs_pos, unsigned int bs_nb, const Point *ms_pos)
616 {
617  double *coord_ms = NULL;
618  double *coord_bs = NULL;
619  double denom = 0;
620  unsigned int n;
621  int i;
622 
623  coord_ms = (double*)ms_pos;
624  for(n = 0; n < bs_nb; ++n) {
625  denom = PosHelper::get_dist(ms_pos, bs_pos + n);
626  if(0 == denom) {
627  it_warning("division by zero");
628  return false;
629  }
630  coord_bs = (double*)(bs_pos + n); /*BSn*/
631  for(i = 0; i < 3; ++i) {
632  grad[i + 3 * n] = (coord_ms[i] - coord_bs[i]) / denom;
633  }
634  }
635  return true;
636 }
637 
638 class Hyperbolic : public Algorithm
639 {
640 public:
641  explicit Hyperbolic(unsigned int nb_bs);
642  virtual ~Hyperbolic();
643  virtual Algorithm::GeoPos validate(const Point *bs_pos, unsigned int bs_pos_len, const double *r);
644  virtual bool setup(const Point *bs_pos, unsigned int bs_pos_len);
645  virtual bool get_pos(Point *ms_pos, const double *range, unsigned int range_len);
646  virtual void set_meas_mat(const double *meas) {
647  memcpy(meas_mat_, meas, nb_bs_ * nb_bs_ * sizeof(*meas));
648  }
649  virtual void get_meas(double *meas, const unsigned int *bs_subset_idx, unsigned int len);
650  virtual bool get_meas_mult_mat(const unsigned int *bs_pos_idx, unsigned int rows, unsigned int cols);
651  virtual void get_meas_mult(unsigned int *meas_mult, const unsigned int *subset_idx, unsigned int meas_mult_len);
652  virtual bool get_grad(double *grad, const Point *bs_pos, unsigned int bs_nb, const Point *ms_pos);
653  virtual double* get_meas_mat() const {
654  return meas_mat_;
655  }
656  virtual unsigned int* get_mult_mat() const {
657  return mult_mat_;
658  }
659 private:
660  double dot_prod(const double *v, const double *w, int len) {
661  double out = v[0] * w[0];
662  int i;
663  for(i = 1; i < len; ++i) {
664  out += v[i] * w[i];
665  }
666  return out;
667  }
668  char comp_quadratic(double *c, double *r1, double *beta, const double *r) {
669  double diff_range2[3];
670  double alpha[3];
671  double d, e, f;
672  char out = 0;
673  int i;
674  double delta;
675  double *coord;
676 
677  for(i = 0; i < 3; ++i) {
678  diff_range2[i] = r[i] * r[i];
679  }
680  coord = (double*)(bs_pos_);
681  for(i = 0; i < 3; ++i) {
682  c[i] = b[i] - 0.5 * dot_prod(A + 3 * i, diff_range2, 3);
683  alpha[i] = coord[i] - c[i];
684  beta[i] = dot_prod(A + 3 * i, r, 3);
685  }
686 
687  /* quadratic equation */
688  d = -1.0;
689  e = f = 0;
690  for(i = 0; i < 3; ++i) {
691  d += beta[i] * beta[i];
692  e += 2 * alpha[i] * beta[i];
693  f += alpha[i] * alpha[i];
694  }
695  delta = e * e - 4 * d * f;
696  if(0 > delta) {
697  //it_warning("square root of negative number, inverting sign");
698  delta = -delta;
699  out = -1;
700  }
701  else if(0 < delta) {
702  out = 1;
703  }
704  /* compute both solutions */
705  r1[0] = (-e + std::sqrt(delta)) / (2 * d);
706  r1[1] = (-e - std::sqrt(delta)) / (2 * d);
707 
708  return out;
709  }
710  //internal data
711  double b[3];
712  double A[9];
713  Point bs_pos_[2];/*BS 1 and 5*/
714  unsigned int bs_pos_len_;
715 
716  double *meas_mat_;
717  unsigned int *mult_mat_;
718  unsigned int nb_bs_;
719 };
720 
721 Hyperbolic::Hyperbolic(unsigned int nb_bs) :
722  nb_bs_(nb_bs),
723  meas_mat_(new double[nb_bs*nb_bs]),
724  mult_mat_(new unsigned int[nb_bs*nb_bs])
725 {
726 }
727 
728 Hyperbolic::~Hyperbolic()
729 {
730  delete[] meas_mat_;
731  delete[] mult_mat_;
732 }
733 
734 bool Hyperbolic::setup(const Point *bs_pos, unsigned int bs_pos_len)
735 {
736  double K[3];
737  double K0;
738  double tmp[3 * 3];
739  int i;
740 
741  if((4 > bs_pos_len) || (NULL == bs_pos)) {
742  it_warning("at least 4 BSs are needed");
743  return false;
744  }
745 
746  memset(b, 0, sizeof(b));
747  memset(A, 0, sizeof(A));
748  memset(bs_pos_, 0, sizeof(bs_pos_));
749  bs_pos_len_ = 0;
750 
751  for(i = 0; i < 3; ++i) {
752  tmp[i] = bs_pos[i + 1].x - bs_pos[0].x;
753  tmp[i + 3] = bs_pos[i + 1].y - bs_pos[0].y;
754  tmp[i + 2 * 3] = bs_pos[i + 1].z - bs_pos[0].z;
755  }
756  if(false == PosHelper::inv_3by3(A, tmp)) {
757  //it_warning("base stations cannot be all in the same plane");
758  return false;
759  }
760  /*transpose as A needs to be stored row-wise*/
761  for(i = 0; i < 3; ++i) {
762  tmp[3 * i] = A[i];
763  tmp[3 * i + 1] = A[3 + i];
764  tmp[3 * i + 2] = A[6 + i];
765  }
766  memcpy(A, tmp, sizeof(A));
767 
768  K0 = bs_pos[0].x * bs_pos[0].x + bs_pos[0].y * bs_pos[0].y + bs_pos[0].z * bs_pos[0].z;
769  for(i = 0; i < 3; ++i) {
770  K[i] = bs_pos[i + 1].x * bs_pos[i + 1].x + bs_pos[i + 1].y * bs_pos[i + 1].y + bs_pos[i + 1].z * bs_pos[i + 1].z - K0;
771  }
772  for(i = 0; i < 3; ++i) {
773  b[i] = 0.5 * dot_prod(A + 3 * i, K, 3);
774  }
775 
776  memcpy(bs_pos_, bs_pos, sizeof(bs_pos_[0]));
777  bs_pos_len_ = 1;
778  if(5 == bs_pos_len) {
779  /*test that all BSs are unique*/
780  /*TODO: condition on 5th BS position*/
781  if(false == PosHelper::test_uniqueness(bs_pos, bs_pos_len, 1e-3)) {
782  return false;
783  }
784  /*copy the 5th BS position*/
785  memcpy(bs_pos_ + 1, bs_pos + 4, sizeof(bs_pos_[1]));
786  ++bs_pos_len_;
787  }
788 
789  return true;
790 }
791 
792 Algorithm::GeoPos Hyperbolic::validate(const Point *bs_pos, unsigned int bs_pos_len, const double *r)
793 {
794  double c[3];
795  double r1[2];
796  double beta[3];
797  Algorithm::GeoPos out = GEO_POS_NO;
798  char delta_sign;
799 
800  if((NULL == bs_pos) || (0 == bs_pos_len) || (NULL == r)) {
801  it_warning("invalid input");
802  return out;
803  }
804 
805  if(true == setup(bs_pos, bs_pos_len)) {
806 
807  /*quadratic equation*/
808  delta_sign = comp_quadratic(c, r1, beta, r);
809 
810  /*decide if a precise location is possible*/
811  if(0 == delta_sign) {
812  /* very uncommon situation */
813  out = GEO_POS_EXACT;
814  }
815  else if((0 < delta_sign) && (2 == bs_pos_len_)) {
816  /*compute the determinant of the 4x4 matrix*/
817  double tmp[4 * 4];
818  int i;
819  double det;
820  for(i = 0; i < 4; ++i) {
821  tmp[i] = bs_pos[i + 1].x - bs_pos[0].x;
822  tmp[4 + i] = bs_pos[i + 1].y - bs_pos[0].y;
823  tmp[2 * 4 + i] = bs_pos[i + 1].z - bs_pos[0].z;
824  tmp[3 * 4 + i] = r[i];
825  }
826  det = PosHelper::det_4by4(tmp);
827  out = (0 != det) ? GEO_POS_YES : GEO_POS_NO;
828  }
829  }
830  return out;
831 }
832 
833 bool Hyperbolic::get_pos(Point *ms_pos, const double *r, unsigned int r_len)
834 {
835  int i, j;
836  double c[3];
837  double r1[2];
838  double beta[3];
839  char delta_sign;
840  double *coord;
841 
842  if((3 > r_len) || (NULL == r)) {
843  it_warning("at least 3 measurements are needed");
844  return false;
845  }
846  if(NULL == ms_pos) {
847  it_warning("output is NULL");
848  return false;
849  }
850  if(0 == bs_pos_len_) {
851  it_warning("geo_hyper_setup needs to be called first");
852  return false;
853  }
854 
855  /* quadratic equation */
856  delta_sign = comp_quadratic(c, r1, beta, r);
857 
858  /* bs_position */
859  if(0 == delta_sign) {
860  /* one solution */
861  coord = (double*)(ms_pos);
862  for(j = 0; j < 3; ++j) {
863  coord[j] = c[j] - beta[j] * r1[0];
864  }
865  }
866  else {
867  Point out[2];/* two possible solutions */
868 
869  if((4 != r_len) || (2 != bs_pos_len_)) {
870  it_warning("4 measurements from 5 BSs are needed");
871  return false;
872  }
873  /* compute both positions */
874  for(i = 0; i < 2; ++i) {
875  coord = (double*)(out + i);
876  for(j = 0; j < 3; ++j) {
877  coord[j] = c[j] - beta[j] * r1[i];
878  }
879  }
880  /* compute TDOA distance for each solution */
881  for(i = 0; i < 2; ++i) {
882  r1[i] = PosHelper::get_dist(bs_pos_ + 1, out + i) - PosHelper::get_dist(bs_pos_, out + i);
883  r1[i] = fabs(r1[i] - r[3]);
884  }
885  /*smallest distance wrt the fourth measure gives the position*/
886  /*TODO: should put a condition on the 5th BS*/
887  j = (r1[0] > r1[1]);
888  memcpy(ms_pos, out + j, sizeof(*ms_pos));
889  }
890  return true;
891 }
892 
893 void Hyperbolic::get_meas(double *meas, const unsigned int *bs_subset_idx, unsigned int len)
894 {
895  for(unsigned int i = 0; i < (len - 1); ++i) {
896  meas[i] = meas_mat_[bs_subset_idx[i + 1] + nb_bs_ * bs_subset_idx[0]];
897  }
898 }
899 
900 bool Hyperbolic::get_meas_mult_mat(const unsigned int *bs_pos_idx, unsigned int rows, unsigned int cols)
901 {
902  unsigned int i, j;
903 
904  memset(mult_mat_, 0, nb_bs_ * nb_bs_ * sizeof(*mult_mat_));
905  if(1 == rows) {
906  it_warning("nothing to do");
907  return true;
908  }
909  for(j = 0; j < cols; ++j) {
910  if(nb_bs_ <= bs_pos_idx[rows * j]) {
911  return false;
912  }
913  for(i = 0; i < (rows - 1); ++i) {
914  if(nb_bs_ <= bs_pos_idx[i + 1 + rows * j]) {
915  return false;
916  }
917  ++mult_mat_[bs_pos_idx[i + 1 + rows * j] + nb_bs_ * bs_pos_idx[rows * j]];
918  }
919  }
920  return true;
921 }
922 
923 void Hyperbolic::get_meas_mult(unsigned int *meas_mult, const unsigned int *subset_idx, unsigned int meas_mult_len)
924 {
925  for(unsigned int i = 0; i < meas_mult_len; ++i) {
926  meas_mult[i] = mult_mat_[subset_idx[i + 1] + nb_bs_ * subset_idx[0]];
927  }
928 }
929 
930 /* get gradient for the measurement function generated by the current set of BSs and the MS
931  * By convention, the first BS is the reference BS
932  * Output
933  * grad: matrix 3x(bs_nb-1) stored column-wise*/
934 bool Hyperbolic::get_grad(double *grad, const Point *bs_pos, unsigned int bs_nb, const Point *ms_pos)
935 {
936  double dr[3];
937  double *coord_ms = NULL;
938  double *coord_bs = NULL;
939  double denom = 0;
940  int i;
941  unsigned int n;
942 
943  denom = PosHelper::get_dist(ms_pos, bs_pos);
944  if(0 == denom) {
945  it_warning("division by zero");
946  return false;
947  }
948  coord_ms = (double*)ms_pos;
949  coord_bs = (double*)bs_pos;/*BS0*/
950  for(i = 0; i < 3; ++i) {
951  dr[i] = (coord_ms[i] - coord_bs[i]) / denom;
952  }
953  for(n = 1; n < bs_nb; ++n) {
954  denom = PosHelper::get_dist(ms_pos, bs_pos + n);
955  if(0 == denom) {
956  it_warning("division by zero");
957  return false;
958  }
959  coord_bs = (double*)(bs_pos + n); /*BSn*/
960  for(i = 0; i < 3; ++i) {
961  grad[i + 3 * (n - 1)] = (coord_ms[i] - coord_bs[i]) / denom - dr[i];
962  }
963  }
964  return true;
965 }
966 
967 bool Multilateration::set_method(const itpp::bvec &method)
968 {
969  int method_len = length(method);
970  int n = 0;
971 
972  type_ = MULTI_FAILURE;
973  method_.set_size(0);
974 
975  if((0 == nb_bs_) || (4 > method_len)) {
976  it_warning("BSs positions are not set or too small method length");
977  return false;
978  }
979 
980  method_ = method;
981 
982  for(n = 0; n < method_len; ++n) {
983  if(bin(1) == method[n]) {
984  break;
985  }
986  }
987  if(n == method_len) {
988  type_ = MULTI_SPHERICAL;
989  goto set_algo;
990  }
991 
992  for(n = 0; n < method_len; ++n) {
993  if(bin(0) == method[n]) {
994  break;
995  }
996  }
997  if(n == method_len) {
998  type_ = MULTI_HYPERBOLIC;
999  goto set_algo;
1000  }
1001 
1002  type_ = MULTI_HYBRID;
1003 
1004 set_algo:
1005  //set algorithm
1006  if(NULL != algo_) {
1007  delete algo_;
1008  }
1009  switch(type_) {
1010  case MULTI_HYPERBOLIC:
1011  if((method_len + 1) != nb_bs_) {
1012  it_warning("For hyperbolic multilateration the number of BSs should exceed by one the number of measures");
1013  return false;
1014  }
1015  algo_ = new Hyperbolic(nb_bs_);
1016  break;
1017  case MULTI_HYBRID:
1018  if((method_len + 1) != nb_bs_) {
1019  it_warning("For hybrid multilateration the number of BSs should exceed by one the number of measures");
1020  return false;
1021  }
1022  algo_ = new Spherical(nb_bs_ - 1); /* after conversion the number of BSs is reduced by one */
1023  break;
1024  case MULTI_SPHERICAL:
1025  if(method_len != nb_bs_) {
1026  it_warning("For spherical multilateration the number of BSs should equal the number of measures");
1027  return false;
1028  }
1029  algo_ = new Spherical(nb_bs_);
1030  break;
1031  default:
1032  it_warning("unknown multilateration method");
1033  algo_ = NULL;
1034  }
1035  nb_fails_part = nb_fails_pos = 0;
1036 
1037  return (NULL != algo_) ? true : false;
1038 }
1039 
1040 bool Multilateration::set_bs_pos(const itpp::mat &bs_pos)
1041 {
1042  int rows = bs_pos.rows();
1043  int cols = bs_pos.cols();
1044 
1045  if(((3 != cols) && (3 != rows)) || (cols == rows)) {
1046  it_warning("BS positions should be specified in 3D cartezian coordinates on either columns or rows");
1047  return false;
1048  }
1049  nb_bs_ = (3 == cols) ? rows : cols;
1050  if(NULL != bs_pos_) {
1051  delete[] bs_pos_;
1052  }
1053  bs_pos_ = new Point[nb_bs_];
1054  unsigned int n = 0;
1055  if(3 == cols) {
1056  for(n = 0; n < nb_bs_; ++n) {
1057  bs_pos_[n].x = bs_pos(n, 0);
1058  bs_pos_[n].y = bs_pos(n, 1);
1059  bs_pos_[n].z = bs_pos(n, 2);
1060  }
1061  }
1062  else if(3 == rows) {
1063  for(n = 0; n < nb_bs_; ++n) {
1064  bs_pos_[n].x = bs_pos(0, n);
1065  bs_pos_[n].y = bs_pos(1, n);
1066  bs_pos_[n].z = bs_pos(2, n);
1067  }
1068  }
1069  return true;
1070 }
1071 
1073 {
1074  delete algo_;
1075  delete bs_pos_;
1076 }
1077 
1078 bool Multilateration::get_pos(itpp::vec &ms_pos, const double *measures)
1079 {
1080  unsigned int *subsets_idx = NULL;
1081  unsigned int subsets_nb = 0;
1082  unsigned int subset_len = 0;
1083  Point *tmp_bs_pos = NULL;
1084  Point *bs_pos = bs_pos_;
1085  unsigned int nb_bs = nb_bs_;
1086  bool out = false;
1087  const int method_len = length(method_);
1088  Point tmp_ms_pos;
1089 
1090  if((0 == method_len) || (NULL == bs_pos_) || (0 == nb_bs_) || (NULL == algo_)) {
1091  return false;
1092  }
1093 
1094  algo_->set_meas_mat(measures);
1095 
1096  switch(type_) {
1097  case MULTI_HYPERBOLIC:
1098  subset_len = 5;
1099  break;
1100  case MULTI_HYBRID:
1101  tmp_bs_pos = (Point*)malloc(nb_bs_ * sizeof(*tmp_bs_pos));
1102  memcpy(tmp_bs_pos, bs_pos_, nb_bs_ * sizeof(*tmp_bs_pos));
1103  if(false == hybrid2spherical(tmp_bs_pos, algo_->get_meas_mat())) {
1104  goto exit;
1105  }
1106  bs_pos = tmp_bs_pos;/* BS position might be changed after conversion */
1107  /* from this point on we deal with spherical multilateration (the number of BSs is reduced by one) */
1108  --nb_bs;
1109  /* fall through */
1110  case MULTI_SPHERICAL:
1111  subset_len = 4;
1112  break;
1113  default:
1114  (void)0;
1115  }
1116 
1117  /*ML algorithm*/
1118  if((false == partition(&subsets_idx, &subsets_nb, bs_pos, nb_bs, subset_len)) || (0 == subsets_nb)) {
1119  ++nb_fails_part;
1120  }
1121  if(0 != subsets_nb) {
1122  if(false == algo_->get_meas_mult_mat(subsets_idx, subset_len, subsets_nb)) {
1123  goto exit;
1124  }
1125  if(false == get_ml_pos(&tmp_ms_pos, bs_pos, nb_bs, subsets_idx, subsets_nb, subset_len)) {
1126  ++nb_fails_pos;
1127  }
1128  else {
1129  ms_pos.set_size(3);
1130  ms_pos(0) = tmp_ms_pos.x;
1131  ms_pos(1) = tmp_ms_pos.y;
1132  ms_pos(2) = tmp_ms_pos.z;
1133  }
1134  }
1135 
1136  out = (0 != subsets_nb) ? true : false;
1137 exit:
1138 
1139  /* free subsets_idx */
1140  if(NULL != subsets_idx) {
1141  free(subsets_idx);
1142  subsets_idx = NULL;
1143  }
1144  /* free temporary array of BS positions */
1145  if(NULL != tmp_bs_pos) {
1146  free(tmp_bs_pos);
1147  tmp_bs_pos = NULL;
1148  }
1149 
1150  return out;
1151 }
1152 
1153 bool Multilateration::hybrid2spherical(Point *bs_pos, double *meas)
1154 {
1155  int n;
1156  int k;
1157  Point *out_bs_pos = NULL;
1158  double *out_meas = NULL;
1159  int method_len = length(method_);
1160  static const bin zero = bin(0);
1161 
1162  if((4 > method_len) || (NULL == bs_pos) || (NULL == meas)) {
1163  return false;
1164  }
1165 
1166  if(MULTI_SPHERICAL == type_) {
1167  return true;/* no conversion is needed */
1168  }
1169 
1170  /* find first TOA measure */
1171  for(n = 0; n < method_len; ++n) {
1172  if(zero == method_[n]) {
1173  break;
1174  }
1175  }
1176  if(n == method_len) {
1177  return false;/* no TOA measure (must be hyperbolic multilateration) */
1178  }
1179 
1180  out_bs_pos = (Point*)malloc(method_len * sizeof(*out_bs_pos));
1181  if(NULL == out_bs_pos) {
1182  return false;
1183  }
1184  out_meas = (double*)malloc(method_len * sizeof(*out_meas));
1185  if(NULL == out_meas) {
1186  free(out_bs_pos);
1187  return false;
1188  }
1189 
1190  if(0 == n) {
1191  /* BS_0 is used for TOA */
1192  out_meas[0] = meas[0];
1193  out_bs_pos[0] = bs_pos[0];
1194  for(k = 1; k < method_len; ++k) {
1195  if(zero == method_[k]) {
1196  out_bs_pos[k] = bs_pos[k];
1197  out_meas[k] = meas[k];
1198  }
1199  else {
1200  out_meas[k] = meas[k] + out_meas[0];
1201  out_bs_pos[k] = bs_pos[k + 1];
1202  }
1203  }
1204 
1205  }
1206  else {
1207  /* BS_n is used for TOA */
1208  int idx = 1;
1209  out_meas[0] = meas[n] - meas[n - 1];
1210  out_bs_pos[0] = bs_pos[0];
1211  for(k = 0; k < method_len; ++k) {
1212  if((n - 1) == k) {
1213  continue; /* skip this measure */
1214  }
1215  if(zero == method_[k]) {
1216  out_bs_pos[idx] = bs_pos[k];
1217  out_meas[idx] = meas[k];
1218  }
1219  else {
1220  out_bs_pos[idx] = bs_pos[k + 1];
1221  out_meas[idx] = meas[k] + out_meas[0];
1222  }
1223  ++idx;
1224  }
1225  }
1226 
1227  /* copy to output */
1228  for(k = 0; k < method_len; ++k) {
1229  bs_pos[k] = out_bs_pos[k];
1230  meas[k] = out_meas[k];
1231  }
1232 
1233  free(out_bs_pos);
1234  free(out_meas);
1235  return true;
1236 }
1237 
1238 bool Multilateration::partition(unsigned int **subsets_idx, unsigned int *subsets_nb, const Point *bs_pos, unsigned int nb_bs, unsigned int subset_len)
1239 {
1240  unsigned int nb_avail_bs = nb_bs;
1241  unsigned int *comb_mat = NULL;
1242  unsigned int nb_comb = 0;
1243  unsigned int i = 0;
1244  unsigned int n = 0;
1245  unsigned int k = 0;
1246  Point bs_pos_subset[5];/*enough for both hyperbolic and spherical trilateration*/
1247  unsigned int *bs_subset_idx = NULL;
1248  double meas[4];
1249  char subset_found = 0;
1250  int valid_res = Algorithm::GEO_POS_NO;
1251  bool res = true;
1252 
1253  if((nb_avail_bs < subset_len) || (NULL == subsets_idx) || (NULL == subsets_nb) || (NULL == bs_pos)) {
1254  it_warning("invalid input");
1255  return false;
1256  }
1257  *subsets_idx = NULL;/* make sure that the output is freed properly */
1258  *subsets_nb = 0;
1259 
1260  /* init subsets */
1261  IndexSet unused_subset_idx(nb_avail_bs);
1262  for(n = 0; n < nb_avail_bs; ++n) {
1263  unused_subset_idx[n] = n;
1264  }
1265  unused_subset_idx.set_size(nb_avail_bs);
1266  IndexSet used_subset_idx(nb_avail_bs);//size is zero
1267  nb_comb = PosHelper::comb_nb(nb_avail_bs - 1, subset_len - 1);
1268  comb_mat = (unsigned int*)malloc(subset_len * nb_comb * sizeof(*comb_mat));
1269  if(NULL == comb_mat) {
1270  it_warning("cannot allocate memory");
1271  return false;
1272  }
1273  bs_subset_idx = (unsigned int*)malloc(subset_len * sizeof(*bs_subset_idx));
1274  if(NULL == bs_subset_idx) {
1275  free(comb_mat);
1276  it_warning("cannot allocate memory");
1277  return false;
1278  }
1279 
1280  /*main loop*/
1281  subset_found = 0;
1282  while((0 != unused_subset_idx.get_size()) && (nb_avail_bs >= subset_len)) {
1283  /* check the unused set */
1284  if(false == PosHelper::combination(comb_mat, &nb_comb, subset_len, unused_subset_idx.get_size())) {
1285  //it_warning("error in PosHelper::combination");
1286  /* no error here */
1287  goto combine_both;
1288  }
1289  n = 0;
1290  while(n < nb_comb) {
1291  for(i = 0; i < subset_len; ++i) {
1292  bs_subset_idx[i] = unused_subset_idx[comb_mat[i + n * subset_len]];
1293  bs_pos_subset[i] = bs_pos[bs_subset_idx[i]];
1294  }
1295  algo_->get_meas(meas, bs_subset_idx, subset_len);
1296  valid_res = algo_->validate(bs_pos_subset, subset_len, meas);
1297  if((Algorithm::GEO_POS_EXACT == valid_res) || (Algorithm::GEO_POS_YES == valid_res)) {
1298  /*store the subset*/
1299  if(false == PosHelper::store_subset(subsets_idx, bs_subset_idx, subset_len, *subsets_nb)) {
1300  it_warning("error in geo_pos_store_subset");
1301  res = false;
1302  break;
1303  }
1304  /*update used and unused sets*/
1305  if(false == PosHelper::update_subsets(used_subset_idx, unused_subset_idx, bs_subset_idx, subset_len)) {
1306  it_warning("error in geo_pos_update_subsets");
1307  res = false;
1308  break;
1309  }
1310  /*increment index*/
1311  ++(*subsets_nb);
1312  /* next iteration */
1313  n = 0;
1314  if(false == PosHelper::combination(comb_mat, &nb_comb, subset_len, unused_subset_idx.get_size())) {
1315  /* no error code here, continue the algorithm */
1316  break;
1317  }
1318  continue;/*restart to iterate through new combinations set*/
1319  }
1320  ++n;
1321  }
1322 
1323  if((0 == unused_subset_idx.get_size()) || (false == res)) {
1324  break;
1325  }
1326 
1327  /* combine unused and used sets */
1328  combine_both:
1329  if(false == PosHelper::combination(comb_mat, &nb_comb, subset_len, nb_avail_bs)) {
1330  it_warning("error in geo_pos_combination");
1331  res = false;/* should not fail at this point */
1332  break;
1333  }
1334  for(n = 0; n < nb_comb; ++n) {
1335  if(comb_mat[subset_len - 1 + subset_len * n] < unused_subset_idx[0]) { /* TODO: check for correctness */
1336  continue;/*skip already searched sets*/
1337  }
1338  for(i = 0; i < subset_len; ++i) {
1339  k = comb_mat[i + n * subset_len];
1340  if(k < unused_subset_idx.get_size()) {
1341  bs_subset_idx[i] = unused_subset_idx[k];
1342  }
1343  else {
1344  /* ensure that recently added BSs are used first */
1345  bs_subset_idx[i] = used_subset_idx[used_subset_idx.get_size() - 1 - (k - unused_subset_idx.get_size())];
1346  }
1347  bs_pos_subset[i] = bs_pos[bs_subset_idx[i]];
1348  }
1349  algo_->get_meas(meas, bs_subset_idx, subset_len);
1350  valid_res = algo_->validate(bs_pos_subset, subset_len, meas);
1351  if((Algorithm::GEO_POS_EXACT == valid_res) || (Algorithm::GEO_POS_YES == valid_res)) {
1352  /*store the subset*/
1353  if(false == PosHelper::store_subset(subsets_idx, bs_subset_idx, subset_len, *subsets_nb)) {
1354  it_warning("error in geo_pos_store_subset");
1355  res = false;
1356  break;
1357  }
1358  /*update used and unused sets*/
1359  if(false == PosHelper::update_subsets(used_subset_idx, unused_subset_idx, bs_subset_idx, subset_len)) {
1360  it_warning("error in geo_pos_update_subsets");
1361  res = false;
1362  break;
1363  }
1364  /*increment index*/
1365  ++(*subsets_nb);
1366  /* stop after finding a subset */
1367  subset_found = 1;
1368  break;
1369  }
1370  }
1371 
1372  if((0 == unused_subset_idx.get_size()) || (false == res)) {
1373  break;
1374  }
1375 
1376  /* remove first BS if no subset found */
1377  if(0 == subset_found) {
1378  res = false;
1379  //it_warning("no subset found");
1380  if(false == unused_subset_idx.remove(unused_subset_idx[0])) {
1381  it_warning("error in utils_set_remove");
1382  break;
1383  }
1384  --nb_avail_bs;
1385  }
1386  else {
1387  subset_found = 0;
1388  }
1389  }
1390  free(comb_mat);
1391  free(bs_subset_idx);
1392  return res;
1393 }
1394 
1395 bool Multilateration::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)
1396 {
1397  unsigned int i;
1398  unsigned int k;
1399  for(i = 0; i < subset_len; ++i) {
1400  k = subset_idx[i];
1401  if(nb_bs <= k) {
1402  it_warning("index out of range");
1403  return false;
1404  }
1405  bs_pos_subset[i] = bs_pos[k];
1406  }
1407  return true;
1408 }
1409 
1410 bool Multilateration::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)
1411 {
1412  Point bs_pos_subset[5];
1413  double meas[4];
1414  unsigned int meas_mult[4];
1415  double grad[3 * 4];
1416  double nom[3];
1417  double denom[9];
1418  double coeff[9];
1419  unsigned int n;
1420  int k;
1421  int i;
1422  double *coord = NULL;
1423  bool res = false;
1424 
1425  if((NULL == bs_pos) || (0 == nb_bs) || (NULL == subsets_idx) || (0 == subsets_nb) || (0 == subset_len) || (NULL == ms_pos)) {
1426  it_warning("invalid input");
1427  return false;
1428  }
1429 
1430  if(1 != subsets_nb) {
1431  memset(nom, 0, sizeof(nom));
1432  memset(denom, 0, sizeof(denom));
1433  }
1434 
1435  for(n = 0; n < subsets_nb; ++n) {
1436  /* compute MS position from current subset */
1437  if(false == get_bs_pos_subset(bs_pos_subset, bs_pos, nb_bs, subsets_idx + n * subset_len, subset_len)) {
1438  goto exit;
1439  }
1440  if(false == algo_->setup(bs_pos_subset, subset_len)) {
1441  it_warning("error in geo_hyper_setup");
1442  goto exit;
1443  }
1444  algo_->get_meas(meas, subsets_idx + n * subset_len, subset_len);
1445  if(false == algo_->get_pos(ms_pos, meas, 4)) {
1446  it_warning("error in geo_hyper_get_pos");
1447  goto exit;
1448  }
1449  if(1 == subsets_nb) {
1450  res = true;
1451  goto exit;
1452  }
1453  /* coefficient estimation */
1454  if(false == algo_->get_grad(grad, bs_pos_subset, subset_len, ms_pos)) {
1455  it_warning("error in geo_hyper_get_grad");
1456  goto exit;
1457  }
1458  algo_->get_meas_mult(meas_mult, subsets_idx + n * subset_len, 4);
1459  if(false == prod(coeff, grad, meas_mult, 3, 4)) {
1460  it_warning("error in geo_hyper_get_grad");
1461  goto exit;
1462  }
1463  /*nominator and denominator for the ML estimator*/
1464  coord = (double*)ms_pos;
1465  for(k = 0; k < 3; ++k) {
1466  for(i = 0; i < 3; ++i) {
1467  nom[k] += coeff[k + 3 * i] * coord[i];
1468  denom[k + 3 * i] += coeff[k + 3 * i];
1469  }
1470  }
1471  }
1472  /*ML estimator from several subsets*/
1473  if(true == PosHelper::inv_3by3(coeff, denom)) {
1474  coord = (double*)ms_pos;
1475  for(n = 0; n < 3; ++n) {
1476  coord[n] = 0;
1477  for(k = 0; k < 3; ++k) {
1478  coord[n] += coeff[n + 3 * k] * nom[k];
1479  }
1480  }
1481  res = true;
1482  }
1483 
1484 exit:
1485  return res;
1486 }
1487 
1488 bool Multilateration::prod(double *out, const double *AT, const unsigned int *d, unsigned int cols, unsigned int rows)
1489 {
1490  unsigned int i;
1491  unsigned int n;
1492  unsigned int k;
1493 
1494  if((NULL == out) || (NULL == AT) || (NULL == d) || (0 == cols) || (0 == rows)) {
1495  it_warning("invalid input");
1496  return false;
1497  }
1498 
1499  for(i = 0; i < cols; ++i) { /* each line first input */
1500  for(n = 0; n < cols; ++n) { /* each column of the second input */
1501  out[i + cols * n] = 0.0;
1502  for(k = 0; k < rows; ++k) { /* each column of the first input or each row of the second input */
1503  if(0 == d[k]) {
1504  it_warning("division by zero");
1505  return false;
1506  }
1507  out[i + cols * n] += (AT[i + k * cols] / (double)d[k]) * AT[n + k * cols];
1508  }
1509  }
1510  }
1511  return true;
1512 }
1513 
1514 double Multilateration::get_crlb(const vec &ms_pos, double sigma2)
1515 {
1516  if((3 != length(ms_pos)) || (0 == nb_bs_)) {
1517  it_error("invalid input");
1518  }
1519  if(0.0 == sigma2) {
1520  return 0.0;
1521  }
1522 
1523  vec pos(3);
1524  vec pos_ref(3);
1525  pos_ref(0) = bs_pos_[0].x;
1526  pos_ref(1) = bs_pos_[0].y;
1527  pos_ref(2) = bs_pos_[0].z;
1528 
1529  static const bin zero = bin(0);
1530  const unsigned int method_len = length(method_);
1531  mat dh(3, method_len);
1532  dh.zeros();
1533  unsigned int n;
1534  unsigned int i;
1535  for(n = 0; n < method_len; ++n) {
1536  if(zero == method_(n)) {
1537  pos(0) = bs_pos_[n].x;
1538  pos(1) = bs_pos_[n].y;
1539  pos(2) = bs_pos_[n].z;
1540  for(i = 0; i < 3; ++i) {
1541  dh(i, n) = (ms_pos(i) - pos(i)) / std::sqrt(sum_sqr(ms_pos - pos));
1542  }
1543  }
1544  else {
1545  pos(0) = bs_pos_[n + 1].x;
1546  pos(1) = bs_pos_[n + 1].y;
1547  pos(2) = bs_pos_[n + 1].z;
1548  for(i = 0; i < 3; ++i) {
1549  dh(i, n) = (ms_pos(i) - pos(i)) / std::sqrt(sum_sqr(ms_pos - pos)) -
1550  (ms_pos(i) - pos_ref(i)) / std::sqrt(sum_sqr(ms_pos - pos_ref));
1551  }
1552  }
1553  }
1554 
1555  mat info_mat(3, 3);
1556  for(n = 0; n < 3; ++n) {
1557  for(i = 0; i < 3; ++i) {
1558  info_mat(n, i) = dh.get_row(n) * dh.get_row(i);
1559  }
1560  }
1561  info_mat = info_mat / sigma2;
1562  return std::sqrt(sum(diag(inv(info_mat))));
1563 }
1564 
1565 }
SourceForge Logo

Generated on Sat Jul 6 2013 10:54:23 for IT++ by Doxygen 1.8.2