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
35using namespace std;
36
37namespace itpp
38{
39
40struct Point {
41 double x;
42 double y;
43 double z;
44};
45
46//class for managing a set of integers (indices)
47class IndexSet
48{
49public:
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 }
99private:
100 unsigned int size_;
101 unsigned int max_size_;
102 unsigned int *data_;
103};
104
105//helper class
106class PosHelper
107{
108public:
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
297class Algorithm
298{
299public:
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
315class Spherical : public Algorithm
316{
317public:
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 }
336private:
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
376Spherical::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
383Spherical::~Spherical()
384{
385 delete[] meas_mat_;
386 delete[] mult_mat_;
387}
388
389/*validate a given set of BSs and the associated measurements*/
390Algorithm::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
414bool 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
526bool 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
584void 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
592bool 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
608void 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
615bool 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
638class Hyperbolic : public Algorithm
639{
640public:
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 }
659private:
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
721Hyperbolic::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
728Hyperbolic::~Hyperbolic()
729{
730 delete[] meas_mat_;
731 delete[] mult_mat_;
732}
733
734bool 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
792Algorithm::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
833bool 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
893void 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
900bool 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
923void 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*/
934bool 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
967bool 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
1004set_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
1040bool 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
1072Multilateration::~Multilateration()
1073{
1074 delete algo_;
1075 delete bs_pos_;
1076}
1077
1078bool 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;
1137exit:
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
1153bool 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
1238bool 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
1395bool 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
1410bool 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
1484exit:
1485 return res;
1486}
1487
1488bool 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
1514double 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}
Binary arithmetic (boolean) class.
Definition binary.h:57
double det(const mat &X)
Determinant of real square matrix.
Definition det.cpp:43
Mat< T > diag(const Vec< T > &v, const int K=0)
Create a diagonal matrix using vector v as its diagonal.
Definition matfunc.h:557
#define it_error(s)
Abort unconditionally.
Definition itassert.h:126
#define it_warning(s)
Display a warning message.
Definition itassert.h:173
bool inv(const mat &X, mat &Y)
Inverse of real square matrix.
Definition inv.cpp:87
int size(const Vec< T > &v)
Length of vector.
Definition matfunc.h:55
T sum(const Vec< T > &v)
Sum of all elements in the vector.
Definition matfunc.h:59
T sum_sqr(const Vec< T > &v)
Sum of square of the elements in a vector.
Definition matfunc.h:116
int length(const Vec< T > &v)
Length of vector.
Definition matfunc.h:51
T prod(const Vec< T > &v)
The product of all elements in the vector.
Definition matfunc.h:195
Definition of multilateration class for indoor localization.
itpp namespace
Definition itmex.h:37
const double eps
Constant eps.
Definition misc.h:109
STL namespace.
SourceForge Logo

Generated on Tue Dec 10 2024 04:49:37 for IT++ by Doxygen 1.12.0