1 /*
2 *
3 * Copyright (c) 2005 The Regents of the University of California. All
4 * rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 *
10 * - Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 *
13 * - Neither the name of the University nor the names of its
14 * contributors may be used to endorse or promote products derived
15 * from this software without specific prior written permission.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
18 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
19 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
20 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
21 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
22 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
23 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
24 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
25 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 *
29 */
30
31 #include <stdio.h>
32 #include <math.h>
33 #include <emrun/emrun.h>
34 #include <libmisc/misc.h>
35 #include "multilat_i.h"
36 #include <gsl/gsl_multifit_nlin.h>
37 #include <gsl/gsl_blas.h>
38
39
40
41 QUEUE_FUNCTION_INSTANTIATIONS(range_list,_,ranges,multilat_range_t,struct _multilat_stages);
42
43 float killpairs_prob = 0;
44
45 #define PHI_DIST_VARIATION 6.0
46 #define PSIG d2r(3.5/2.0)
47 #define TSIG d2r(0.96/2.0)
48 #define MAX_BIDIR_DIFF 300
49 #define MIN_BIDIR_DIFF 5
50 #undef AVERAGE_SMALL
51 #undef EMBEDDED_USECONF
52
53 int isnormal(double d)
54 {
55 if (isnan(d)) return 0;
56 if (isinf(d)) return 0;
57 return 1;
58 }
59
60 double misc_normalize_angle_rad_d(double rad_angle)
61 {
62 double result = fmod(rad_angle, 2.0 * M_PI);
63 if (result > M_PI) result -= (2.0 * M_PI);
64 else if (result < - M_PI) result += (2.0 * M_PI);
65 return result;
66 }
67
68 inline float m_pow_2(float x) { return x*x; }
69
70 void multilatbuf_to_file(ml_state_t *mls, char *filename, buf_t *buf) {
71 if (!mls->logs_off) {
72 // int thetime = time(NULL);
73 char f[4096];
74 // sprintf(f, "/tmp/%s-%d", filename, thetime);
75 sprintf(f, "%s/%04i-%s", mls->outputprefix, mls->global_run, filename);
76 elog(LOG_WARNING, "outputing to %s", f);
77 buf_to_file(f, buf);
78 }
79 else
80 elog(LOG_WARNING, "suppressing log file output..");
81 }
82
83
84 void print_matrix(buf_t *buf, gsl_matrix *m) {
85 int i = 0;
86 int j = 0;
87 for (i = 0; i < m->size1; ++i) {
88 for (j = 0; j < m->size2; ++j) {
89 bufprintf(buf, "%f ", gsl_matrix_get(m, i, j));
90 }
91 bufprintf(buf, "\n");
92 }
93 }
94 void print_vector(buf_t *buf, gsl_vector *v) {
95 int i = 0;
96 for (i = 0; i < v->size; ++i) {
97 bufprintf(buf, "%f ", gsl_vector_get(v, i));
98 }
99 bufprintf(buf, "\n");
100 }
101
102 void print_multitlat_state(ml_state_t *mls,
103 gsl_matrix *A,
104 gsl_matrix *new_A,
105 gsl_matrix *V,
106 gsl_vector *s,
107 gsl_vector *b,
108 gsl_vector *x,
109 int state)
110 {
111 buf_t *buf = buf_new();
112 char file[4096];
113
114 bufprintf(buf, "Current State:\n");
115 bufprintf(buf, "A was:\n");
116 print_matrix(buf, A);
117 bufprintf(buf, "\n\n");
118
119 bufprintf(buf, "A is:\n");
120 print_matrix(buf, new_A);
121
122 bufprintf(buf, "\nV is:\n");
123 print_matrix(buf, V);
124
125 bufprintf(buf,"\n\n s is:\n");
126 print_vector(buf, s);
127
128 bufprintf(buf,"\n\n b is:\n");
129 print_vector(buf, b);
130
131 bufprintf(buf,"\n\n x is:\n");
132 print_vector(buf, x);
133 bufprintf(buf, "\n\n\n");
134
135 sprintf(file, "state%i", state);
136 multilatbuf_to_file(mls, file, buf);
137 buf_free(buf);
138 }
139
140
141 struct aw {
142 mreal angle;
143 mreal weight;
144 mreal total;
145 };
146 struct aw angles[100];
147 int angle_count;
148
149 void init_cm()
150 {
151 angle_count = 0;
152 }
153
154 void add_cm(mreal theta, mreal weight)
155 {
156 angles[angle_count].angle = misc_normalize_angle_rad_d(theta);
157 angles[angle_count].weight = weight;
158 angles[angle_count].total = 0;
159 angle_count++;
160 }
161
162 int aw_cmp(const void *a,const void *b)
163 {
164 return ((struct aw *)a)->angle - ((struct aw *)b)->angle;
165 }
166
167 int realcmp(const void *a,const void *b)
168 {
169 mreal cmp = *((mreal *)a) - *((mreal *)b);
170 if (cmp < 0) return -1;
171 if (cmp > 0) return 1;
172 return 0;
173 }
174
175 mreal finish_cm()
176 {
177 qsort(angles, angle_count, sizeof(struct aw), aw_cmp);
178
179 int i;
180 float max_weight = 0;
181 int max_w_index = -1;
182 for (i=0; i<angle_count; i++) {
183 int j;
184 for (j=0; j<angle_count; j++)
185 if (fabs(misc_normalize_angle_rad_d(angles[j].angle - angles[i].angle)) < d2r(10))
186 angles[i].total += angles[j].weight;
187 if (angles[i].total > max_weight || max_w_index < 0) {
188 max_weight = angles[i].total;
189 max_w_index = i;
190 }
191 }
192
193 if (max_w_index >= 0) {
194 mreal t[2] = {0,0};
195
196 int j;
197 for (j=0; j<angle_count; j++) {
198 if (fabs(misc_normalize_angle_rad_d(angles[j].angle - angles[max_w_index].angle)) < d2r(10)) {
199 t[0] += angles[j].weight*cos(angles[j].angle);
200 t[1] += angles[j].weight*sin(angles[j].angle);
201 }
202 }
203
204 return atan2(t[1], t[0]);
205 }
206
207 elog(LOG_WARNING, "couldnt compute sloppy mode");
208 return 0;
209 }
210
211 struct error_stats {
212 mreal avg_range_diff;
213 mreal rms_range_diff;
214 mreal avg_theta_diff;
215 mreal rms_theta_diff;
216 mreal avg_phi_diff;
217 mreal rms_phi_diff;
218 int range_count;
219 int theta_count;
220 int phi_count;
221 };
222
223
224 buf_t *mlat_errors_to_buf(ml_state_t *mls, struct error_stats *stats)
225 {
226 mreal avg_range_diff = 0;
227 mreal rms_range_diff = 0;
228 mreal avg_theta_diff = 0;
229 mreal rms_theta_diff = 0;
230 mreal avg_phi_diff = 0;
231 mreal rms_phi_diff = 0;
232 int range_count = 0;
233 int theta_count = 0;
234 int phi_count = 0;
235 multilat_range_t *mtr;
236
237 buf_t *err_summ = buf_new();
238 bufprintf(err_summ, "#h chirper receiver r r_diff user t terr uset p perr usep yaw pitch roll\n");
239
240 for (mtr = range_list_top(mls->multilat_lists) ;
241 mtr != NULL; mtr = range_list_next(mtr))
242 if (mtr->distance > 0) {
243
244 multilat_result_t * mrr1 = result_list_get(mls, mtr->chirp_from);
245 multilat_result_t * mrr2 = result_list_get(mls, mtr->data_from);
246
247 mreal dist = sqrt(sqrf(mrr1->x - mrr2->x) +
248 sqrf(mrr1->y - mrr2->y) +
249 sqrf(mrr1->z - mrr2->z));
250
251 mreal phi = atan2(mrr1->z - mrr2->z,
252 sqrt(sqrf(mrr1->x - mrr2->x) +
253 sqrf(mrr1->y - mrr2->y)));
254
255 mreal theta = atan2(mrr1->y - mrr2->y, mrr1->x - mrr2->x);
256
257 mreal rerr = (mtr->userange != 0) ? mtr->userange - dist :
258 mtr->distance - dist;
259 mreal perr = misc_normalize_angle_rad_d(mtr->phi) -
260 misc_normalize_angle_rad_d(phi);
261 mreal terr =
262 misc_normalize_angle_rad_d
263 (misc_normalize_angle_rad_d(mtr->theta) -
264 misc_normalize_angle_rad_d(mrr2->yaw) -
265 misc_normalize_angle_rad_d(theta));
266
267 bufprintf(err_summ, "%d\t%d\t%f\t%f\t%c\t",
268 mtr->chirp_from, mtr->data_from, mtr->distance, rerr,
269 (mtr->state == RANGE_DROPPED || mtr->drop_r) ? 'N' :
270 #ifdef RTHETA
271 'Y'
272 #else
273 ((mtr->use_r) ? 'Y' : 'M')
274 #endif
275 );
276
277 #ifdef RTHETA
278 if (!(mtr->state == RANGE_DROPPED || mtr->drop_r))
279 #else
280 if (mtr->use_r)
281 #endif
282 {
283 range_count++;
284 avg_range_diff += fabs(rerr);
285 rms_range_diff += sqrf(rerr);
286 }
287
288 #ifdef RTHETA
289 if (mtr->state != RANGE_DROPPED)
290 #else
291 if (mtr->use_t)
292 #endif
293 {
294 avg_theta_diff += fabs(terr);
295 rms_theta_diff += sqrf(terr);
296 theta_count++;
297 bufprintf(err_summ, "%f\t%f\tY\t", r2d(mtr->theta), r2d(terr));
298 }
299 else bufprintf(err_summ, "%f\t%f\t%c\t", r2d(mtr->theta), r2d(terr),
300 (mtr->state == RANGE_DROPPED || mtr->drop_t) ? 'N' : 'M');
301 #ifdef RTHETA
302 if (mtr->state != RANGE_DROPPED)
303 #else
304 if (mtr->use_p)
305 #endif
306 {
307 phi_count++;
308 avg_phi_diff += fabs(perr);
309 rms_phi_diff += sqrf(perr);
310 bufprintf(err_summ, "%f\t%f\tY\t", r2d(mtr->phi), r2d(perr));
311 }
312 else bufprintf(err_summ, "%f\t%f\t%c\t", r2d(mtr->phi), r2d(perr),
313 (mtr->state == RANGE_DROPPED || mtr->drop_p) ? 'N' : 'M');
314 bufprintf(err_summ, "%f\t%f\t%f\n", r2d(-mrr2->yaw), r2d(-mrr2->pitch), r2d(-mrr2->roll));
315 }
316
317 if (stats) {
318 stats->avg_range_diff = avg_range_diff;
319 stats->rms_range_diff = rms_range_diff;
320 stats->avg_theta_diff = avg_theta_diff;
321 stats->rms_theta_diff = rms_theta_diff;
322 stats->avg_phi_diff = avg_phi_diff;
323 stats->rms_phi_diff = rms_phi_diff;
324 stats->range_count = range_count;
325 stats->theta_count = theta_count;
326 stats->phi_count = phi_count;
327 }
328
329 return err_summ;
330 }
331
332
333 float last_drop_value = 0;
334 float min_drop_value = -1;
335 float min_xyz = -1;
336 float min_xyz_resid = 0;
337 int min_xyz_passes = 0;
338 int iterations = 0;
339
340 void dump_errors(ml_state_t *mls, int k)
341 {
342 buf_t *err_summ = mlat_errors_to_buf(mls, NULL);
343 char thefile[4096];
344 sprintf(thefile, "err-%d", k);
345 multilatbuf_to_file(mls, thefile, err_summ);
346 buf_free(err_summ);
347 }
348
349
350 /* compare the results to the ground truth.. */
351 void analyse_result(ml_state_t *mls)
352 {
353 /* compute raw average differences */
354 mreal avg_range_diff = 0;
355 mreal rms_range_diff = 0;
356 mreal avg_theta_diff = 0;
357 mreal rms_theta_diff = 0;
358 mreal avg_phi_diff = 0;
359 mreal rms_phi_diff = 0;
360 int range_count = 0;
361 int theta_count = 0;
362 int phi_count = 0;
363
364 struct error_stats stats = {};
365 buf_t *err_summ = mlat_errors_to_buf(mls, &stats);
366
367 avg_range_diff = stats.avg_range_diff;
368 rms_range_diff = stats.rms_range_diff;
369 avg_theta_diff = stats.avg_theta_diff;
370 rms_theta_diff = stats.rms_theta_diff;
371 avg_phi_diff = stats.avg_phi_diff;
372 rms_phi_diff = stats.rms_phi_diff;
373 range_count = stats.range_count;
374 theta_count = stats.theta_count;
375 phi_count = stats.phi_count;
376
377 if (range_count > 0) {
378 avg_range_diff /= (mreal)range_count;
379 avg_theta_diff /= (mreal)theta_count;
380 avg_phi_diff /= (mreal)phi_count;
381 rms_range_diff = sqrt(rms_range_diff / (mreal)range_count);
382 rms_theta_diff = sqrt(rms_theta_diff / (mreal)theta_count);
383 rms_phi_diff = sqrt(rms_phi_diff / (mreal)phi_count);
384 elog(LOG_WARNING, "AVG: r=%f t=%f p=%f",
385 avg_range_diff, r2d(avg_theta_diff), r2d(avg_phi_diff));
386 elog(LOG_WARNING, "RMS: r=%f t=%f p=%f",
387 rms_range_diff, r2d(rms_theta_diff), r2d(rms_phi_diff));
388
389 }
390
391 char thefile[4096];
392 sprintf(thefile, "final-err");
393 multilatbuf_to_file(mls, thefile, err_summ);
394 buf_free(err_summ);
395
396 #ifdef PROCRUSTES
397
398 /* compute centroid */
399
400 mreal cG[3] = {0,0,0};
401 mreal c[3] = {0,0,0};
402
403 multilat_result_t *node1, *node2;
404
405 int count = 0;
406
407 for (node1 = result_list_top(mls->multilat_lists);
408 node1; node1 = result_list_next(node1)) {
409
410 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
411 elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
412 continue;
413 }
414
415 node1->X[0] = node1->x;
416 node1->X[1] = node1->y;
417 node1->X[2] = node1->z;
418
419 node1->XG[0] = node1->real_x;
420 node1->XG[1] = node1->real_y;
421 node1->XG[2] = node1->real_z;
422
423 count++;
424 for (i=0; i<3; i++) {
425 c[i] += X[i];
426 cG[i] += XG[i];
427 }
428 }
429
430 for (i=0; i<3; i++) {
431 c[i] /= (mreal)count;
432 cG[i] /= (mreal)count;
433 }
434
435 /* compute centroid sizes */
436
437 mreal S = 0;
438 mreal SG = 0;
439
440 for (node1 = result_list_top(mls->multilat_lists);
441 node1; node1 = result_list_next(node1)) {
442
443 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
444 elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
445 continue;
446 }
447
448 for (i=0; i<3; i++) {
449 S += sqrf(node1->X[i] - c[i]);
450 SG += sqrf(node1->XG[i] - cG[i]);
451 }
452 }
453
454 S = sqrt(S);
455 SG = sqrt(SG);
456
457 /* pre-shape */
458
459 for (node1 = result_list_top(mls->multilat_lists);
460 node1; node1 = result_list_next(node1)) {
461
462 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
463 elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
464 continue;
465 }
466
467 for (i=0; i<3; i++) {
468 node1->X[i] = (node1->X[i] - c[i]) / S;
469 node1->XG[i] = (node1->XG[i] - cG[i]) / SG;
470 }
471 }
472
473 /* rotations.. */
474 /* $$$ */
475
476
477 #else
478 /* compute scaling factor */
479 mreal tot_comp = 1.0;
480 mreal tot_gt = 1.0;
481 mreal V;
482
483 multilat_result_t *node1, *node2;
484
485 mreal c[3] = {0,0,0};
486 mreal cG[3] = {0,0,0};
487 int count = 0;
488
489 for (node1 = result_list_top(mls->multilat_lists);
490 node1; node1 = result_list_next(node1)) {
491
492 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
493 elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
494 continue;
495 }
496
497 if (node1->nogt) {
498 elog(LOG_WARNING, "skipping node %d, no gt..", node1->node);
499 continue;
500 }
501
502 count++;
503 c[0] += node1->x;
504 c[1] += node1->y;
505 c[2] += node1->z;
506
507 cG[0] += node1->real_x;
508 cG[1] += node1->real_y;
509 cG[2] += node1->real_z;
510
511 mreal nD1=-1.0, nD2=-1.0;
512
513 for (node2 = result_list_top(mls->multilat_lists);
514 node2; node2 = result_list_next(node2)) {
515
516 if (node2->col < 0 && node2->state != RESULT_COORD_ROOT) continue;
517 if (node2->nogt) continue;
518
519 if (node1 != node2) {
520 mreal D1 = sqrt
521 (sqrf(node1->x - node2->x) +
522 sqrf(node1->y - node2->y) +
523 sqrf(node1->z - node2->z));
524
525 mreal D2 = sqrt
526 (sqrf(node1->real_x - node2->real_x) +
527 sqrf(node1->real_y - node2->real_y) +
528 sqrf(node1->real_z - node2->real_z));
529
530 if (nD1 < 0 || D1 < nD1) {
531 nD1 = D1;
532 nD2 = D2;
533 }
534 }
535 }
536
537 if (nD1 > 0) {
538 tot_comp += nD1;
539 tot_gt += nD2;
540 }
541 }
542
543 V = tot_comp / tot_gt;
544
545 /* centroid */
546 c[0] /= (mreal)count;
547 c[1] /= (mreal)count;
548 c[2] /= (mreal)count;
549
550 cG[0] /= (mreal)count;
551 cG[1] /= (mreal)count;
552 cG[2] /= (mreal)count;
553
554 #ifndef ROTATE_ABOUT_CENTROID
555 mreal min_dist = -1;
556
557 /* find center point */
558 for (node1 = result_list_top(mls->multilat_lists);
559 node1; node1 = result_list_next(node1)) {
560
561 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
562 if (node1->nogt) continue;
563
564 mreal D1 = sqrt
565 (sqrf(node1->x - c[0]) +
566 sqrf(node1->y - c[1]) +
567 sqrf(node1->z - c[2]));
568 if (min_dist < 0 || min_dist > D1) {
569 min_dist = D1;
570 node2 = node1;
571 }
572 }
573
574 /* node2 is closest to centroid */
575 elog(LOG_WARNING, "centroid is node %d", node2->node);
576
577 /* translate to center */
578 for (node1 = result_list_top(mls->multilat_lists);
579 node1; node1 = result_list_next(node1)) {
580
581 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
582
583 if (node1 != node2) {
584 node1->tx = node1->x - node2->x;
585 node1->ty = node1->y - node2->y;
586 node1->tz = node1->z - node2->z;
587
588 node1->trx = node1->real_x - node2->real_x;
589 node1->try = node1->real_y - node2->real_y;
590 node1->trz = node1->real_z - node2->real_z;
591 }
592 }
593
594 node2->tx = 0;
595 node2->ty = 0;
596 node2->tz = 0;
597
598 node2->trx = 0;
599 node2->try = 0;
600 node2->trz = 0;
601 #else
602 /* translate to centroid */
603 for (node1 = result_list_top(mls->multilat_lists);
604 node1; node1 = result_list_next(node1)) {
605
606 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
607
608 node1->tx = node1->x - c[0];
609 node1->ty = node1->y - c[1];
610 node1->tz = node1->z - c[2];
611
612 node1->trx = node1->real_x - cG[0];
613 node1->try = node1->real_y - cG[1];
614 node1->trz = node1->real_z - cG[2];
615 }
616 node2=NULL;
617 #endif
618
619 /* scale and init tyaw */
620 for (node1 = result_list_top(mls->multilat_lists);
621 node1; node1 = result_list_next(node1)) {
622
623 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
624
625 node1->tx /= V;
626 node1->ty /= V;
627 node1->tz /= V;
628 }
629
630 mreal rot;
631 mreal cumrot = 0;
632
633 goto retry_z;
634
635 flip:
636 /* FLIP */
637 elog(LOG_WARNING, "Flipped!");
638 cumrot = M_PI-cumrot;
639
640 retry_z:
641
642 #ifdef USEZ
643 init_cm();
644
645 /* determine Z axis rotation */
646 for (node1 = result_list_top(mls->multilat_lists);
647 node1; node1 = result_list_next(node1)) {
648
649 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
650 if (node1->nogt) continue;
651
652 if (node1 != node2) {
653 mreal D1 = sqrt(sqrf(node1->tx) + sqrf(node1->ty));
654
655 mreal dt =
656 atan2(node1->ty, node1->tx) -
657 atan2(node1->try, node1->trx);
658
659 add_cm(dt, D1);
660 }
661 }
662
663 rot = -finish_cm();
664
665 elog(LOG_WARNING, "Z axis rotation = %f", r2d(rot));
666
667 #endif
668
669 /* rotate */
670 for (node1 = result_list_top(mls->multilat_lists);
671 node1; node1 = result_list_next(node1)) {
672
673 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
674 elog(LOG_WARNING, "node %d is a root?", node1->node);
675 continue;
676 }
677
678 if (node1 != node2) {
679 mreal tmpx = node1->tx;
680 mreal tmpy = node1->ty;
681 node1->tx = (cos(rot)*tmpx - sin(rot)*tmpy);
682 node1->ty = (sin(rot)*tmpx + cos(rot)*tmpy);
683 }
684 }
685
686 cumrot += rot;
687
688 /* does our ground truth have a z axis? */
689 int have_gtz = 0;
690 for (node1 = result_list_top(mls->multilat_lists);
691 node1; node1 = result_list_next(node1)) {
692
693 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
694 if (node1->nogt) continue;
695
696 if (node1->real_z != 0) {
697 have_gtz = 1;
698 break;
699 }
700 }
701
702 if (have_gtz) {
703
704 /* determine Y axis rotation */
705
706 init_cm();
707
708 for (node1 = result_list_top(mls->multilat_lists);
709 node1; node1 = result_list_next(node1)) {
710
711 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
712 if (node1->nogt) continue;
713
714 if (node1 != node2) {
715 mreal D1 = sqrt(sqrf(node1->tx) + sqrf(node1->tz));
716
717 mreal dt =
718 atan2(node1->tz, node1->tx) -
719 atan2(node1->trz, node1->trx);
720
721 add_cm(dt, D1);
722 }
723 }
724
725 rot = -finish_cm();
726
727 elog(LOG_WARNING, "Y axis rotation = %f", r2d(rot));
728
729 /* rotate */
730 for (node1 = result_list_top(mls->multilat_lists);
731 node1; node1 = result_list_next(node1)) {
732
733 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
734
735 if (node1 != node2) {
736 mreal tmpx = node1->tx;
737 mreal tmpz = node1->tz;
738 node1->tx = (cos(rot)*tmpx - sin(rot)*tmpz);
739 node1->tz = (sin(rot)*tmpx + cos(rot)*tmpz);
740 }
741 }
742
743 if (r2d(rot) < -160 || r2d(rot) > 160) {
744 goto flip;
745 }
746
747 /* determine X axis rotation */
748
749 init_cm();
750
751 for (node1 = result_list_top(mls->multilat_lists);
752 node1; node1 = result_list_next(node1)) {
753
754 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
755 if (node1->nogt) continue;
756
757 if (node1 != node2) {
758 mreal D1 = sqrt(sqrf(node1->tz) + sqrf(node1->ty));
759
760 mreal dt =
761 atan2(node1->ty, node1->tz) -
762 atan2(node1->try, node1->trz);
763
764 add_cm(dt, D1);
765 }
766 }
767
768 rot = -finish_cm();
769
770 elog(LOG_WARNING, "X axis rotation = %f", r2d(rot));
771
772 /* rotate */
773 for (node1 = result_list_top(mls->multilat_lists);
774 node1; node1 = result_list_next(node1)) {
775
776 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
777
778 if (node1 != node2) {
779 mreal tmpz = node1->tz;
780 mreal tmpy = node1->ty;
781 node1->tz = (cos(rot)*tmpz - sin(rot)*tmpy);
782 node1->ty = (sin(rot)*tmpz + cos(rot)*tmpy);
783 }
784 }
785
786 if (r2d(rot) < -160 || r2d(rot) > 160) {
787 goto flip;
788 }
789 }
790
791 /* add in original yaw */
792 for (node1 = result_list_top(mls->multilat_lists);
793 node1; node1 = result_list_next(node1)) {
794 node1->tyaw = -node1->yaw + cumrot;
795 }
796
797 /* average translation */
798 mreal tr[3] = {0,0,0};
799 for (node1 = result_list_top(mls->multilat_lists);
800 node1; node1 = result_list_next(node1)) {
801
802 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
803 if (node1->nogt) continue;
804
805 mreal D = sqrt(sqrf(node1->trx-node1->tx) +
806 sqrf(node1->try-node1->ty) +
807 sqrf(node1->trz-node1->tz));
808
809 /* only add into average if < 1 meter away */
810 if (D < 100) {
811 tr[0] += node1->trx-node1->tx;
812 tr[1] += node1->try-node1->ty;
813 tr[2] += node1->trz-node1->tz;
814 }
815 }
816
817 tr[0] /= (mreal)count;
818 tr[1] /= (mreal)count;
819 tr[2] /= (mreal)count;
820
821 /* adjust nodes for translation */
822 for (node1 = result_list_top(mls->multilat_lists);
823 node1; node1 = result_list_next(node1)) {
824
825 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
826
827 node1->tx += tr[0];
828 node1->ty += tr[1];
829 node1->tz += tr[2];
830 }
831 #endif
832
833 /* compute the avg dist */
834 mreal avg_dist = 0;
835 mreal rms_dist = 0;
836 mreal avg_zdist = 0;
837 mreal rms_zdist = 0;
838 mreal worst_dist = 0;
839 mreal avg_zdist_d1 = 0;
840
841 mreal avg_yaw_err = 0;
842
843 mreal distarr[100];
844 mreal zdistarr[100];
845 mreal yawarr[100];
846 int distcount = 0;
847
848 buf_t *xybuf = buf_new();
849 buf_t *xyzbuf = buf_new();
850 buf_t *obuf = buf_new();
851
852 for (node1 = result_list_top(mls->multilat_lists);
853 node1; node1 = result_list_next(node1)) {
854
855 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
856 if (node1->nogt) continue;
857
858 mreal DZ = sqrt
859 (sqrf(node1->tx - node1->trx) +
860 sqrf(node1->ty - node1->try) +
861 sqrf(node1->tz - node1->trz));
862
863 mreal DXY = sqrt
864 (sqrf(node1->tx - node1->trx) +
865 sqrf(node1->ty - node1->try));
866
867 avg_dist += DXY;
868 rms_dist += sqrf(DXY);
869
870 avg_zdist += DZ;
871 rms_zdist += sqrf(DZ);
872
873 distarr[distcount] = DXY;
874 zdistarr[distcount] = DZ;
875
876 if (DZ > worst_dist) worst_dist = DZ;
877
878 mreal yaw_err = fmod(node1->real_yaw - r2d(node1->tyaw), 360.0);
879 if (yaw_err > 180) yaw_err -= 360.0;
880 if (yaw_err < -180) yaw_err += 360.0;
881
882 avg_yaw_err += fabs(yaw_err);
883 yawarr[distcount] = fabs(yaw_err);
884 distcount++;
885
886 elog(LOG_WARNING, "dist %d %f, yawerr=%f", node1->node, DXY, yaw_err);
887
888 bufprintf(xybuf, "%f %d\n", DXY, node1->node);
889 bufprintf(xyzbuf, "%f %d\n", DZ, node1->node);
890 bufprintf(obuf, "%f %d\n", yaw_err, node1->node);
891
892 }
893
894 qsort(distarr, distcount, sizeof(mreal), realcmp);
895 qsort(zdistarr, distcount, sizeof(mreal), realcmp);
896 qsort(yawarr, distcount, sizeof(mreal), realcmp);
897
898 mreal median_dist = distarr[distcount/2];
899 mreal median_zdist = zdistarr[distcount/2];
900 mreal median_yaw = yawarr[distcount/2];
901
902 #if 0
903 elog(LOG_WARNING, "distcount=%d, medianyaw=%f", distcount, median_yaw);
904 int klkl;
905 for (klkl=0;klkl<distcount;klkl++)
906 elog(LOG_WARNING, " yaw %d %f", klkl, yawarr[klkl]);
907 #endif
908
909 avg_dist /= (mreal)count;
910 rms_dist = sqrt(rms_dist / (mreal)count);
911 avg_yaw_err/= (mreal)count;
912
913 avg_zdist_d1 = (avg_zdist - worst_dist) / (mreal)(count-1);
914
915 avg_zdist /= (mreal)count;
916 rms_zdist = sqrt(rms_zdist / (mreal)count);
917
918 elog(LOG_WARNING, " V = %f", V);
919 elog(LOG_WARNING, " DIST = %f, RMS = %f, median = %f", avg_dist, rms_dist, median_dist);
920 elog(LOG_WARNING, " ZDIST = %f, RMS = %f, median = %f", avg_zdist, rms_zdist, median_zdist);
921 elog(LOG_WARNING, " YAWERR = %f", avg_yaw_err);
922
923 /* print out */
924 buf_t *firstpass = buf_new();
925 sprintf(thefile, "final");
926 bufprintf(firstpass, "#h node x y z yaw roll pitch gt_x gt_y gt_z\n");
927 multilat_result_t *rlt = result_list_top(mls->multilat_lists);
928 for ( ; rlt != NULL; rlt = result_list_next(rlt) ) {
929
930 if (rlt->col < 0 && rlt->state != RESULT_COORD_ROOT) continue;
931
932 bufprintf(firstpass, "%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", rlt->node, rlt->tx, rlt->ty, rlt->tz,
933 r2d(rlt->tyaw), r2d(-rlt->roll), r2d(-rlt->pitch),
934 rlt->trx, rlt->try, rlt->trz);
935 }
936
937 if (min_drop_value < 0 || last_drop_value < min_drop_value) min_drop_value = last_drop_value;
938 if (min_xyz < 0 || avg_zdist < min_xyz) {
939 min_xyz = avg_zdist;
940 min_xyz_resid = min_drop_value;
941 min_xyz_passes = mls->global_run;
942 }
943
944 bufprintf(firstpass, "# V = %f\n", V);
945 bufprintf(firstpass, "# avg_pos_err(xy) = %f, RMS = %f, med %f\n", avg_dist, rms_dist, median_dist);
946 bufprintf(firstpass, "# avg_pos_err(xyz) = %f, RMS = %f, med %f\n", avg_zdist, rms_zdist, median_zdist);
947 bufprintf(firstpass, "# avg_constraint_err: r=%f t=%f p=%f\n",
948 avg_range_diff, r2d(avg_theta_diff), r2d(avg_phi_diff));
949 bufprintf(firstpass, "# rms_constraint_err: r=%f t=%f p=%f\n",
950 rms_range_diff, r2d(rms_theta_diff), r2d(rms_phi_diff));
951 bufprintf(firstpass, "# yaw_avg %f median %f \n",
952 avg_yaw_err, median_yaw);
953
954 bufprintf(firstpass, "##%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d\t%f\t%f\t%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n\n",
955 V, avg_dist, avg_zdist, avg_range_diff, r2d(avg_theta_diff), r2d(avg_phi_diff),
956 last_drop_value, min_drop_value, count,
957 min_xyz, min_xyz_resid, min_xyz_passes, iterations, avg_zdist_d1,
958 median_dist, median_zdist, avg_yaw_err, median_yaw,
959 killpairs_prob, distarr[distcount-1]
960 );
961 multilatbuf_to_file(mls, thefile, firstpass);
962 buf_free(firstpass);
963
964 multilatbuf_to_file(mls, "3d-dist", xyzbuf);
965 multilatbuf_to_file(mls, "2d-dist", xybuf);
966 multilatbuf_to_file(mls, "yaw-dist", obuf);
967 buf_free(xyzbuf);
968 buf_free(xybuf);
969 buf_free(obuf);
970 }
971
972
973 #ifdef RTHETA
974
975 void update_minmax(mreal *min, mreal *max, mreal value)
976 {
977 if (value < *min) *min = value;
978 else if (value > *max) *max = value;
979 }
980
981 mreal do_xweight(multilat_range_t *mtr)
982 {
983 mreal min=0;
984 mreal max=0;
985 min = max = cos(mtr->theta + TSIG)*cos(mtr->phi + PSIG);
986 update_minmax(&min,&max,cos(mtr->theta - TSIG)*cos(mtr->phi + PSIG));
987 update_minmax(&min,&max,cos(mtr->theta + TSIG)*cos(mtr->phi - PSIG));
988 update_minmax(&min,&max,cos(mtr->theta - TSIG)*cos(mtr->phi - PSIG));
989
990 mreal w = sqrt(mtr->distance * (max-min));
991 if (w == 0) return 0;
992 return 1.0/w;
993 }
994
995 mreal do_yweight(multilat_range_t *mtr)
996 {
997 mreal min=0;
998 mreal max=0;
999 min = max = sin(mtr->theta + TSIG)*cos(mtr->phi + PSIG);
1000 update_minmax(&min,&max,sin(mtr->theta - TSIG)*cos(mtr->phi + PSIG));
1001 update_minmax(&min,&max,sin(mtr->theta + TSIG)*cos(mtr->phi - PSIG));
1002 update_minmax(&min,&max,sin(mtr->theta - TSIG)*cos(mtr->phi - PSIG));
1003
1004 mreal w = sqrt(mtr->distance * (max-min));
1005 if (w == 0) return 0;
1006 return 1.0/w;
1007 }
1008
1009 mreal do_zweight(multilat_range_t *mtr)
1010 {
1011 mreal min=0;
1012 mreal max=0;
1013 min = max = sin(mtr->phi + PSIG);
1014 update_minmax(&min,&max,sin(mtr->phi + PSIG));
1015 update_minmax(&min,&max,sin(mtr->phi - PSIG));
1016 update_minmax(&min,&max,sin(mtr->phi - PSIG));
1017
1018 mreal w = sqrt(mtr->distance * (max-min));
1019 if (w == 0) return 0;
1020 return 1.0/w;
1021 }
1022
1023 void multilat_solver(ml_state_t *mls) {
1024
1025 int i = 0;
1026
1027 /* the result_view array is the mapping of a 0 to num_nodes index of
1028 all the results... it gives us a convienient way to address the
1029 nodes without having to deal with incosistent node id's */
1030 int num_nodes = result_list_qlen(mls->multilat_lists);
1031 multilat_result_t *(result_view[num_nodes]);
1032 result_list_make_array(mls, result_view, num_nodes);
1033
1034 #ifdef USEZ
1035 int axes = 3;
1036 #else
1037 int axes = 2;
1038 #endif
1039
1040 /* row is chirp_from, column is data_from */
1041
1042 int num_ranges = range_list_qlen(mls->multilat_lists);
1043 multilat_range_t *mtr;
1044 int total_dups = 0;
1045
1046 int runagain = 0;
1047 int dropped_rows = 0;
1048 int num_roots = 0;
1049
1050 buf_t *droppedbuf = buf_new();
1051
1052 /******************/
1053 /* get dimensions */
1054 /******************/
1055 repeate:
1056
1057 if (mls->global_run != 0) {
1058 result_load(mls);
1059 }
1060
1061 num_roots = 0;
1062 for (i = 0; i < num_nodes; ++i) {
1063 if (result_view[i]->state == RESULT_COORD_ROOT)
1064 num_roots++;
1065 }
1066
1067 /* $$$ this would change with tiedowns.. not really right anyway */
1068 int num_rows = (num_ranges - total_dups - (num_roots*(num_roots-1)))*axes - dropped_rows;
1069 int num_cols = (num_nodes - num_roots)*axes;
1070
1071 elog(LOG_CRIT, "total ranges %i, dropped %i, rows,cols= %i,%i", num_ranges, dropped_rows,
1072 num_rows, num_cols);
1073
1074 if (num_cols > num_rows) {
1075 elog(LOG_CRIT, "Can not continue with SVD only have %i by %i matrix", num_rows, num_cols);
1076 exit(1);
1077 }
1078
1079 //mreal VV = 1;
1080
1081 /***********/
1082 /* iterate */
1083 /***********/
1084 int k = 0;
1085 int dobreak=0;
1086 for (k = 0; k < 100; ++k) {
1087
1088 /* setup the matrix */
1089 gsl_matrix *A;
1090 gsl_vector *b;
1091
1092 elog(LOG_DEBUG(0), "allocing %d,%d matrix", num_rows, num_cols);
1093 A = gsl_matrix_calloc(num_rows, num_cols);
1094 b = gsl_vector_calloc(num_rows);
1095
1096 int node1_loc = 0, node2_loc = 0, curr_row = 0, curr_col = 0;
1097 for (mtr = range_list_top(mls->multilat_lists), i = 0;
1098 i < num_ranges; ++i, mtr = range_list_next(mtr) ) {
1099
1100 //elog(LOG_CRIT, "currrow: %i, i is %i", curr_row, i);
1101 //elog(LOG_CRIT, "looking for df: %i cf: %i", mtr->data_from, mtr->chirp_from);
1102 node1_loc = result_list_array_lookup(mls, result_view, mtr->data_from);
1103 node2_loc = result_list_array_lookup(mls, result_view, mtr->chirp_from);
1104
1105 mtr->last_row = -1;
1106
1107 /* skip ranges between anchors */
1108 if ( (mtr->data_from == mls->anode1 && mtr->chirp_from == mls->anode2) ||
1109 (mtr->data_from == mls->anode2 && mtr->chirp_from == mls->anode1)
1110 ) {
1111 // elog(LOG_WARNING, "!!!! found the anchors");
1112 continue;
1113 }
1114
1115 /* skip if dropped */
1116 if ( mtr->state == RANGE_DROPPED ) {
1117 continue;
1118 }
1119
1120 mtr->last_row = curr_row;
1121
1122 /* find the columns if they exist... if not, set them */
1123 if (result_view[node1_loc]->col == -1 && result_view[node1_loc]->state != RESULT_COORD_ROOT) {
1124 result_view[node1_loc]->col = curr_col;
1125 curr_col += axes;
1126 }
1127 if (result_view[node2_loc]->col == -1 && result_view[node2_loc]->state != RESULT_COORD_ROOT) {
1128 result_view[node2_loc]->col = curr_col;
1129 curr_col += axes;
1130 }
1131
1132 /* node1_loc represents x1, y1, z1 ...
1133 node2_loc represents x2, y2, z2 */
1134 // node 2 chirps
1135 // node 1 receives
1136
1137 /* $$$ missing VV and tiedowns */
1138
1139 mtr->userange = mtr->distance + PHI_DIST_VARIATION*sin(mtr->phi);
1140
1141 if (result_view[node1_loc]->state == RESULT_COORD_ROOT) {
1142 /* add constraints */
1143 mreal w = do_xweight(mtr);
1144 gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 0, +1.0*w);
1145 gsl_vector_set(b, curr_row,
1146 (mtr->userange*cos(mtr->phi)*
1147 cos(mtr->theta-result_view[node1_loc]->yaw) +
1148 result_view[node1_loc]->x)*w);
1149 curr_row++;
1150
1151 w = do_yweight(mtr);
1152 gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 1, +1.0*w);
1153 gsl_vector_set(b, curr_row,
1154 (mtr->userange*cos(mtr->phi)*
1155 sin(mtr->theta-result_view[node1_loc]->yaw) +
1156 result_view[node1_loc]->y)*w);
1157 curr_row++;
1158
1159 if (axes>2) {
1160 w = do_zweight(mtr);
1161 gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 2, +1.0*w);
1162 gsl_vector_set(b, curr_row, (mtr->userange*sin(mtr->phi) +
1163 result_view[node1_loc]->z)*w);
1164 curr_row++;
1165 }
1166 }
1167
1168 else if (result_view[node2_loc]->state == RESULT_COORD_ROOT) {
1169 /* add constraints */
1170 mreal w = do_xweight(mtr);
1171 gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 0, -1.0*w);
1172 gsl_vector_set(b, curr_row,
1173 (mtr->userange*cos(mtr->phi)*
1174 cos(mtr->theta-result_view[node1_loc]->yaw) -
1175 result_view[node2_loc]->x)*w);
1176 curr_row++;
1177
1178 w = do_yweight(mtr);
1179 gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 1, -1.0*w);
1180 gsl_vector_set(b, curr_row,
1181 (mtr->userange*cos(mtr->phi)*
1182 sin(mtr->theta-result_view[node1_loc]->yaw) -
1183 result_view[node2_loc]->y)*w);
1184 curr_row++;
1185
1186 if (axes>2) {
1187 w = do_zweight(mtr);
1188 gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 2, -1.0*w);
1189 gsl_vector_set(b, curr_row, (mtr->userange*sin(mtr->phi) -
1190 result_view[node2_loc]->z)*w);
1191 curr_row++;
1192 }
1193 }
1194
1195 else {
1196 /* add constraints */
1197 mreal w = do_xweight(mtr);
1198 gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 0, -1.0*w);
1199 gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 0, +1.0*w);
1200 gsl_vector_set(b, curr_row,
1201 (mtr->userange*cos(mtr->phi)*
1202 cos(mtr->theta-result_view[node1_loc]->yaw))*w);
1203 curr_row++;
1204
1205 w = do_yweight(mtr);
1206 gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 1, -1*w);
1207 gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 1, +1*w);
1208 gsl_vector_set(b, curr_row,
1209 (mtr->userange*cos(mtr->phi)*
1210 sin(mtr->theta-result_view[node1_loc]->yaw))*w);
1211 curr_row++;
1212
1213 if (axes>2) {
1214 w = do_zweight(mtr);
1215 gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 2, -1*w);
1216 gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 2, +1*w);
1217 gsl_vector_set(b, curr_row, mtr->userange*sin(mtr->phi)*w);
1218 curr_row++;
1219 }
1220 }
1221
1222 }
1223 elog(LOG_CRIT, "Just added %i rows", curr_row);
1224
1225
1226 /************/
1227 /* SVDecomp */
1228 /************/
1229
1230 gsl_matrix *new_A;
1231 gsl_matrix *V;
1232 gsl_vector *s;
1233 gsl_vector *x_res;
1234
1235 new_A = gsl_matrix_calloc(num_rows, num_cols);
1236 V = gsl_matrix_calloc(num_cols, num_cols);
1237 s = gsl_vector_calloc(num_cols);
1238 x_res = gsl_vector_calloc(num_cols);
1239
1240 int res = gsl_matrix_memcpy(new_A, A);
1241 if (res != GSL_SUCCESS) {
1242 elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
1243 exit(1);
1244 }
1245
1246 elog(LOG_WARNING, "Starting svdecomp");
1247 res = gsl_linalg_SV_decomp_jacobi(new_A, V, s);
1248
1249 if (res != GSL_SUCCESS) {
1250 elog(LOG_CRIT, "Error doing svdecomp: %s\n", gsl_strerror(res));
1251 exit(1);
1252 }
1253 elog(LOG_WARNING, "Starting SV solve");
1254 res = gsl_linalg_SV_solve(new_A, V, s, b, x_res);
1255 if (res != GSL_SUCCESS) {
1256 elog(LOG_CRIT, "Error doing svdecomp Solve: %s\n", gsl_strerror(res));
1257 exit(1);
1258 }
1259 elog(LOG_WARNING, "Printing state");
1260 print_multitlat_state(mls, A, new_A, V, s, b, x_res, k);
1261 elog(LOG_WARNING, "Done");
1262
1263 /*********************************/
1264 /* update the results with x_res */
1265 /*********************************/
1266 for (i = 0; i < num_nodes; ++i) {
1267 if (result_view[i]->state != RESULT_COORD_ROOT && result_view[i]->col >= 0) {
1268 result_view[i]->x = gsl_vector_get(x_res, result_view[i]->col + 0);
1269 result_view[i]->y = gsl_vector_get(x_res, result_view[i]->col + 1);
1270 if (axes>2)
1271 result_view[i]->z = gsl_vector_get(x_res, result_view[i]->col + 2);
1272 }
1273 }
1274
1275 /*************************/
1276 /* update yaw/pitch/roll */
1277 /*************************/
1278
1279 mreal totx=0;
1280 mreal toty=0;
1281 int totsum=0;
1282 //mreal max_err=0;
1283
1284 int converged = 1;
1285 multilat_result_t *node;
1286 for (i = 0; i < num_nodes; ++i) {
1287 multilat_result_t *node = result_view[i];
1288 /* compute orientation for roots if there is more than one of them.
1289 * otherwise, lock in specified orientation */
1290 if (num_roots > 1 || (result_view[i]->state != RESULT_COORD_ROOT)) {
1291 mreal yaw[2] = {0,0};
1292 mreal pitch[2] = {0,0};
1293 mreal roll[2] = {0,0};
1294
1295 for (mtr = range_list_top(mls->multilat_lists);
1296 mtr != NULL; mtr = range_list_next(mtr)) {
1297
1298 if (mtr->data_from == result_view[i]->node) {
1299 int chirp_index = result_list_array_lookup(mls, result_view, mtr->chirp_from);
1300 multilat_result_t *chirp_node = result_view[chirp_index];
1301
1302 mreal c[3];
1303 mreal ctheta,cphi;
1304 mreal dtheta,dphi;
1305
1306 /* unit vector of computed angle */
1307 c[0] = chirp_node->x - node->x;
1308 c[1] = chirp_node->y - node->y;
1309 c[2] = chirp_node->z - node->z;
1310
1311 mreal D = sqrt(sqrf(c[0]) + sqrf(c[1]) + sqrf(c[2]));
1312
1313 c[0] /= D;
1314 c[1] /= D;
1315 c[2] /= D;
1316
1317 /* zenith and azimuth angles */
1318 ctheta = atan2(c[1], c[0]);
1319 cphi = misc_normalize_angle_rad_d(atan2(c[2], sqrt(sqrf(c[0])+sqrf(c[1]))));
1320
1321 /* diffs */
1322 dtheta = ctheta - mtr->theta;
1323 dphi = cphi - misc_normalize_angle_rad_d(mtr->phi);
1324
1325 if (!mtr->drop_t) {
1326 /* sum (could weight by confidence..) */
1327 yaw[0] += cos(dtheta)*cos(cphi);
1328 yaw[1] += sin(dtheta)*cos(cphi);
1329 }
1330
1331 if (!mtr->drop_p) {// && fabs(dphi) < (6/180.0*M_PI)) {
1332 elog(LOG_DEBUG(20),
1333 "dphi %f ctheta %f p += %f %f ",
1334 r2d(dphi), r2d(ctheta),
1335 fabs(cos(ctheta))*cos(dphi),
1336 cos(ctheta)*sin(dphi));
1337 elog(LOG_DEBUG(20),
1338 "dphi %f ctheta %f r += %f %f ",
1339 r2d(dphi), r2d(ctheta),
1340 fabs(sin(ctheta))*cos(dphi),
1341 sin(ctheta)*sin(dphi));
1342
1343 pitch[0] += fabs(cos(ctheta))*cos(dphi);
1344 pitch[1] += cos(ctheta)*sin(dphi);
1345
1346 roll[0] += fabs(sin(ctheta))*cos(dphi);
1347 roll[1] += sin(ctheta)*sin(dphi);
1348
1349 elog(LOG_DEBUG(20),
1350 " == %f,%f (%f)",
1351 pitch[0],pitch[1],
1352 r2d(atan2(pitch[1],pitch[0])));
1353 elog(LOG_DEBUG(20),
1354 " == %f,%f (%f)",
1355 roll[0],roll[1],
1356 r2d(atan2(roll[1],roll[0])));
1357 }
1358 else {
1359 elog(LOG_DEBUG(11), "skipping dphi=%f", dphi*180.0/M_PI);
1360 }
1361 }
1362 }
1363
1364 mreal new_yaw = -atan2(yaw[1],yaw[0]);
1365 if (fabs(node->yaw - new_yaw) > 0.01)
1366 converged = 0;
1367 node->yaw = new_yaw;
1368 node->pitch = -atan2(pitch[1],pitch[0]);
1369 node->roll = -atan2(roll[1],roll[0]);
1370
1371 elog(LOG_DEBUG(11), "****node %d: avg yaw=%f pitch=%f roll=%f [%f,%f,%f] ",
1372 node->node, r2d(node->yaw),
1373 r2d(node->pitch),
1374 r2d(node->roll),
1375 sqrt(sqrf(yaw[0])+sqrf(yaw[1])),
1376 sqrt(sqrf(pitch[0])+sqrf(pitch[1])),
1377 sqrt(sqrf(roll[0])+sqrf(roll[1])));
1378
1379 totx += cos(node->yaw);
1380 toty += sin(node->yaw);
1381 totsum++;
1382 }
1383 }
1384
1385 mreal avgor = atan2(toty,totx);
1386 elog(LOG_WARNING, "average orientation is %f", avgor);
1387
1388 #ifndef ORIENT_BY_ROOT
1389 if (converged) {
1390 mreal max_diff = -1;
1391
1392 /* rotate all points and unorient */
1393 for (node = result_list_top(mls->multilat_lists);
1394 node; node = result_list_next(node)) {
1395 node->yaw -= avgor;
1396 mreal tmpx = node->x;
1397 mreal tmpy = node->y;
1398 node->x = cos(avgor)*tmpx - sin(avgor)*tmpy;
1399 node->y = sin(avgor)*tmpx + cos(avgor)*tmpy;
1400
1401 if (node->state != RESULT_COORD_ROOT && node->col >= 0) {
1402 mreal diff;
1403 diff = fabs(gsl_vector_get(x_res, node->col + 0) +
1404 (node->x - tmpx));
1405 if (diff > max_diff) max_diff = diff;
1406 diff = fabs(gsl_vector_get(x_res, node->col + 1) +
1407 (node->y - tmpy));
1408 if (diff > max_diff) max_diff = diff;
1409 }
1410 }
1411
1412 elog(LOG_WARNING, "MAX DIFF = %f", max_diff);
1413 }
1414 #endif
1415 if (converged) dobreak = 1;
1416
1417 /******************/
1418 /* out put errors */
1419 /******************/
1420
1421 /*
1422 * This can be done in the mess above, but to keep it from getting
1423 * too messy, we just repeat down here
1424 */
1425
1426 buf_t *resbuf = buf_new();
1427 bufprintf(resbuf, "#h receiver chirper axis resid\n");
1428
1429 // declared earlier int node1_loc = 0, node2_loc = 0;
1430 // for (i = 0; i < b->size; ++i) {
1431 for (i = 0; i < num_rows; i+=axes) {
1432
1433 /* find the ranges */
1434 for (mtr = range_list_top(mls->multilat_lists) ;
1435 mtr != NULL; mtr = range_list_next(mtr)) {
1436 if (i == mtr->last_row) {
1437 break;
1438 }
1439 }
1440
1441 if (mtr == NULL) {
1442 elog(LOG_WARNING, "Could not find range for that row!");
1443 exit(1);
1444 }
1445
1446 /* find the new calculated positions and the range */
1447 node1_loc = result_list_array_lookup(mls, result_view, mtr->data_from);
1448 node2_loc = result_list_array_lookup(mls, result_view, mtr->chirp_from);
1449
1450 int k;
1451 for (k=0; k<axes; k++) {
1452 int kk;
1453 double sum = 0;
1454 for (kk=0; kk<num_cols; kk++)
1455 sum += (gsl_matrix_get(A, i+k, kk) * gsl_vector_get(x_res, kk));
1456 sum -= gsl_vector_get(b, i+k);
1457 bufprintf(resbuf, "%i %i %i %f\n",
1458 mtr->data_from, mtr->chirp_from, k, sum);
1459
1460 /* $$ could also include range, angle error */
1461 }
1462 }
1463
1464 #if 0
1465 char angleres[4096];
1466 sprintf(angleres, "angleres%i", k);
1467 multilatbuf_to_file(mls, angleres, ares);
1468 buf_free(ares);
1469 #endif
1470
1471 char errors[4096];
1472 sprintf(errors, "errors%i", k);
1473 multilatbuf_to_file(mls, errors, resbuf);
1474 buf_free(resbuf);
1475
1476 /************************/
1477 /* studentized residual */
1478 /************************/
1479 // H = X ( XT X )-1 XT.
1480 if (dobreak && (mls->studthresh != 0)) {
1481
1482 /* num_rows is the number of ranges */
1483
1484 int l = 0;
1485 gsl_matrix *X = gsl_matrix_calloc(num_rows, 2);
1486 gsl_matrix *X2 = gsl_matrix_calloc(num_rows, 2);
1487 gsl_matrix *XTrans = gsl_matrix_calloc(2, num_rows);
1488 gsl_matrix *XTrans2 = gsl_matrix_calloc(2, num_rows);
1489 gsl_matrix *XXTrans = gsl_matrix_calloc(2, 2);
1490 gsl_matrix *H = gsl_matrix_calloc(num_rows, num_rows);
1491 gsl_vector *stud = gsl_vector_calloc(num_rows);
1492 double mean = 0.0;
1493 double var = 0.0;
1494
1495
1496 for (l = 0; l < num_rows; l++) {
1497 mean += gsl_vector_get(b, l);
1498 gsl_matrix_set(X, l, 0, 1);
1499 gsl_matrix_set(X, l, 1, gsl_vector_get(b, l));
1500 gsl_matrix_set(X2, l, 0, 1);
1501 gsl_matrix_set(X2, l, 1, gsl_vector_get(b, l));
1502 gsl_matrix_set(XTrans, 0, l, 1);
1503 gsl_matrix_set(XTrans, 1, l, gsl_vector_get(b, l));
1504 gsl_matrix_set(XTrans2, 0, l, 1);
1505 gsl_matrix_set(XTrans2, 1, l, gsl_vector_get(b, l));
1506 }
1507 mean = mean / (num_rows + 0.0);
1508
1509 elog(LOG_WARNING, "mean is %f", mean);
1510 gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, XTrans, X, 1.0, XXTrans);
1511 gsl_blas_dtrsm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
1512 // upper or lower does not matter // gsl_blas_dtrsm(CblasRight, CblasLower, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
1513 gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, X2, XTrans2, 1.0, H);
1514
1515 /* calculate the variance */
1516 for (l = 0; l < num_rows; ++l) {
1517 var += m_pow_2(gsl_vector_get(b,l) - mean);
1518 // elog(LOG_WARNING, "%f", gsl_matrix_get(H,l,l));
1519 }
1520 var = var / (num_rows + 0.0);
1521 elog(LOG_WARNING, "var is %f", var);
1522
1523
1524 buf_t *studbuf = buf_new();
1525 bufprintf(studbuf, "#h res stud real chirpfrom datafrom\n");
1526
1527 mreal maxstud = 0;
1528 for (l = 0; l < num_rows; l++) {
1529 double a = gsl_vector_get(b, l) / (sqrtf(var) * sqrtf(1.0 - gsl_matrix_get(H,l,l)));
1530 // gsl_vector_set(stud, l, a);
1531 gsl_vector_set(stud, l, a >= 0 ? a : -a);
1532
1533 if (gsl_vector_get(stud, l) > mls->studthresh) {
1534 if (gsl_vector_get(stud, l) > maxstud) {
1535 maxstud = gsl_vector_get(stud, l);
1536 }
1537 }
1538 }
1539
1540 if (maxstud > mls->studthresh)
1541 for (l = 0; l < num_rows; l++) {
1542 int range_index = (l/axes)*axes;
1543
1544 if (gsl_vector_get(stud, l) == maxstud) {
1545
1546 /* important! */
1547 runagain = 1;
1548
1549 elog(LOG_WARNING, "val is %f at row %i", gsl_vector_get(stud, l), l);
1550 last_drop_value = maxstud;
1551
1552 for (mtr = range_list_top(mls->multilat_lists) ;
1553 mtr != NULL; mtr = range_list_next(mtr)) {
1554 if (range_index == mtr->last_row) {
1555 break;
1556 }
1557 }
1558
1559 if (mtr) {
1560 if (mtr->state != RANGE_DROPPED) {
1561 dropped_rows += axes;
1562 mtr->state = RANGE_DROPPED;
1563 int from = mtr->chirp_from;
1564 int to = mtr->data_from;
1565
1566 bufprintf(droppedbuf, "%i %i %f %f %i\n", from, to,
1567 gsl_vector_get(b,l),
1568 gsl_vector_get(stud,l),
1569 mls->global_run
1570 );
1571
1572 for (mtr = range_list_top(mls->multilat_lists) ;
1573 mtr != NULL; mtr = range_list_next(mtr)) {
1574 if ((from == mtr->chirp_from && to == mtr->data_from) ||
1575 (to == mtr->chirp_from && from == mtr->data_from)) {
1576 if (mtr->state != RANGE_DROPPED) {
1577 dropped_rows += axes;
1578 mtr->state = RANGE_DROPPED;
1579 }
1580 }
1581 }
1582 }
1583 }
1584 else
1585 elog(LOG_CRIT, "cant' find range.. ");
1586 }
1587
1588 for (mtr = range_list_top(mls->multilat_lists) ;
1589 mtr != NULL; mtr = range_list_next(mtr)) {
1590 if (range_index == mtr->last_row) {
1591 break;
1592 }
1593 }
1594
1595 if (mtr) {
1596 multilat_result_t * mrr1 = result_list_get(mls, mtr->chirp_from);
1597 multilat_result_t * mrr2 = result_list_get(mls, mtr->data_from);
1598 bufprintf(studbuf, "%f %f %f %i %i\n",
1599 gsl_vector_get(b,l),
1600 gsl_vector_get(stud,l),
1601 mtr->real_distance -
1602 sqrtf(m_pow_2(mrr1->x - mrr2->x) +
1603 m_pow_2(mrr1->y - mrr2->y) +
1604 m_pow_2(mrr1->z - mrr2->z)),
1605 mtr->chirp_from,
1606 mtr->data_from
1607 );
1608 }
1609 else
1610 elog(LOG_CRIT, "cant' find range for index %d (%d).. 2",
1611 range_index, l);
1612 }
1613
1614 multilatbuf_to_file(mls, "compare", studbuf);
1615 buf_free(studbuf);
1616
1617 elog(LOG_WARNING, " ----- All done!");
1618
1619 gsl_matrix_free(X);
1620 gsl_matrix_free(X2);
1621 gsl_matrix_free(XTrans);
1622 gsl_matrix_free(XTrans2);
1623 gsl_matrix_free(XXTrans);
1624 gsl_matrix_free(H);
1625 gsl_vector_free(stud);
1626 }
1627
1628 mreal avg_resid = 0;
1629 mreal rms_resid = 0;
1630 int range_count = 0;
1631 int l;
1632 for (l = 0; l < num_rows; ++l) {
1633 avg_resid += fabs(gsl_vector_get(b,l));
1634 rms_resid += sqrf(gsl_vector_get(b,l));
1635 range_count++;
1636 }
1637
1638 if (range_count > 0) {
1639 avg_resid /= (float)range_count;
1640 rms_resid = sqrt(rms_resid/(float)range_count);
1641 }
1642
1643 elog(LOG_CRIT, "**** average residuals:");
1644 elog(LOG_CRIT, "AVG: %f RMS: %f",
1645 avg_resid, rms_resid);
1646
1647
1648 /**************/
1649 /* free stuff */
1650 /**************/
1651
1652 gsl_matrix_free(A);
1653 gsl_vector_free(b);
1654 gsl_matrix_free(new_A);
1655 gsl_matrix_free(V);
1656 gsl_vector_free(s);
1657 // gsl_vector_free(work);
1658 gsl_vector_free(x_res);
1659 // gsl_vector_free(check);
1660
1661 buf_t *firstpass = buf_new();
1662 char thefile[4096];
1663 memset(thefile, 0, 4096);
1664 sprintf(thefile, "pass%i", k);
1665 result_list_print(mls, firstpass);
1666 multilatbuf_to_file(mls, thefile, firstpass);
1667 buf_free(firstpass);
1668
1669 if (dobreak) {
1670 dobreak = 0;
1671 break;
1672 }
1673 }
1674
1675 /* print out the final error stuff */
1676 buf_t *ce = buf_new();
1677 bufprintf(ce, "#h node coorderror\n");
1678 multilat_result_t *mlr = result_list_top(mls->multilat_lists);
1679 mreal rms = 0;
1680 int count = 0;
1681 for ( ; mlr != NULL; mlr = result_list_next(mlr) ) {
1682 if (mlr->state != RESULT_COORD_ROOT) {
1683 bufprintf(ce, "%i %f\n", mlr->node,
1684 sqrtf(m_pow_2(mlr->x - mlr->real_x) +
1685 m_pow_2(mlr->y - mlr->real_y) +
1686 m_pow_2(mlr->z - mlr->real_z))
1687 );
1688 rms += m_pow_2(mlr->x - mlr->real_x) +
1689 m_pow_2(mlr->y - mlr->real_y) +
1690 m_pow_2(mlr->z - mlr->real_z);
1691 count++;
1692 }
1693
1694 }
1695 bufprintf(ce, "# rms: %f\n", sqrtf(rms / (count + 0.0)));
1696 bufprintf(ce, "# Grms: %f\n", mls->guess_rms);
1697 multilatbuf_to_file(mls, "coorderr", ce);
1698 buf_free(ce);
1699
1700 iterations = k;
1701 analyse_result(mls);
1702
1703 /*************/
1704 /* increment */
1705 /*************/
1706
1707 if (runagain) {
1708 mls->global_run++;
1709 runagain = 0;
1710 goto repeate;
1711 }
1712
1713 multilatbuf_to_file(mls, "dropped", droppedbuf);
1714 buf_free(droppedbuf);
1715
1716 mls->global_run = 1000;
1717
1718 {
1719 buf_t *firstpass = buf_new();
1720 char thefile[4096];
1721 memset(thefile, 0, 4096);
1722 sprintf(thefile, "last");
1723 result_list_print(mls, firstpass);
1724 multilatbuf_to_file(mls, thefile, firstpass);
1725 buf_free(firstpass);
1726 }
1727
1728 analyse_result(mls);
1729
1730 return;
1731 }
1732
1733
1734 /******************************************************************************/
1735 /******************************************************************************/
1736 /******************************************************************************/
1737 /******************************************************************************/
1738 /******************************************************************************/
1739 /******************************************************************************/
1740 /******************************************************************************/
1741 /******************************************************************************/
1742 /******************************************************************************/
1743 /******************************************************************************/
1744 /******************************************************************************/
1745
1746
1747 #else //!RTHETA
1748
1749
1750 void reset_matrix(ml_state_t *mls, int num_rows)
1751 {
1752 if (mls->row_info) free(mls->row_info);
1753 mls->row_info = g_new0(struct row_info, num_rows);
1754 mls->row_count = 0;
1755 }
1756
1757 int addrow(ml_state_t *mls, int type, multilat_range_t *new_range)
1758 {
1759 mls->row_info[mls->row_count].c_type = type;
1760 mls->row_info[mls->row_count].range = new_range;
1761 mls->row_count++;
1762 return mls->row_count;
1763 }
1764
1765 int get_row_type(ml_state_t *mls, int row)
1766 {
1767 return mls->row_info[row].c_type;
1768 }
1769
1770 multilat_range_t *get_row_range(ml_state_t *mls, int row)
1771 {
1772 return mls->row_info[row].range;
1773 }
1774
1775
1776 /* compare the forward and reverse paths and compute
1777 * estimates of the common delta for each node.
1778 * the theory is that there may be a fixed delay for each node
1779 * that accounts for the fwd/rev differences
1780 *
1781 * (Rij, i is the receiver)
1782 * Rij = T + Di - Dj
1783 * Rji = T + Dj - Di
1784 * Rij - Di + Dj = Rji - Dj + Di
1785 * Rij - Rji = 2Di - 2Dj
1786 */
1787
1788 void compute_deltas(ml_state_t *mls)
1789 {
1790 multilat_result_t *node;
1791 multilat_range_t *mtr;
1792 int rows = 0;
1793
1794 /* clear delta_in flags */
1795 for (node = result_list_top(mls->multilat_lists);
1796 node; node = result_list_next(node)) {
1797 node->delta_in = 0;
1798 node->col = -1;
1799 }
1800
1801 /* count number of rows */
1802 for (mtr = range_list_top(mls->multilat_lists);
1803 mtr; mtr = range_list_next(mtr)) {
1804
1805 mtr->last_row = -1;
1806
1807 /* sort out ranges */
1808 if (!mtr->drop_r && !mtr->peer->drop_r) {
1809 if (fabs(mtr->peer->distance - mtr->distance) > MAX_BIDIR_DIFF ||
1810 (mtr->peer->distance > MAX_BIDIR_DIFF &&
1811 fabs(mtr->peer->distance - mtr->distance) > (mtr->distance * 0.1))) {
1812 elog(LOG_WARNING, "dropping bogus range and angles..");
1813 if (mtr->peer->distance > mtr->distance) {
1814 mtr->peer->drop_r = mtr->peer->drop_p = mtr->peer->drop_t = 1;
1815 }
1816 else {
1817 mtr->drop_r = mtr->drop_p = mtr->drop_t = 1;
1818 }
1819 }
1820 }
1821
1822 if (!mtr->drop_r && !mtr->peer->drop_r) {
1823 if (fabs(mtr->peer->distance - mtr->distance) < 20) {
1824 mtr->result->delta_in++;
1825 mtr->peer->result->delta_in++;
1826 rows++;
1827 }
1828 }
1829 }
1830
1831 rows /= 2;
1832
1833 #ifdef COMPUTE_DELTAS
1834 /* count cols */
1835 int cols = 0;
1836 for (node = result_list_top(mls->multilat_lists);
1837 node; node = result_list_next(node)) {
1838 if (node->delta_in) cols++;
1839 }
1840
1841 /* set up matrices */
1842 gsl_matrix *A;
1843 gsl_vector *b;
1844
1845 elog(LOG_WARNING, "allocing %d by %d matrix", rows, cols);
1846 A = gsl_matrix_calloc(rows, cols);
1847 b = gsl_vector_calloc(rows);
1848
1849 int curr_row = 0;
1850 int curr_col = 0;
1851 for (mtr = range_list_top(mls->multilat_lists);
1852 mtr; mtr = range_list_next(mtr)) {
1853
1854 /* skip if... */
1855 if (mtr->last_row >= 0) continue;
1856 if (mtr->drop_r || mtr->peer->drop_r) continue;
1857 if (fabs(mtr->peer->distance - mtr->distance) >= 20) continue;
1858
1859 /* ok, add it */
1860 if (mtr->result->col < 0)
1861 mtr->result->col = curr_col++;
1862 if (mtr->peer->result->col < 0)
1863 mtr->peer->result->col = curr_col++;
1864
1865 mtr->last_row = curr_row;
1866 mtr->peer->last_row = curr_row;
1867
1868 /* b = Rij - Rji */
1869 gsl_vector_set(b, curr_row, mtr->distance - mtr->peer->distance);
1870 /* A = 2Di - 2Dj */
1871 gsl_matrix_set(A, curr_row, mtr->result->col, +2);
1872 gsl_matrix_set(A, curr_row, mtr->peer->result->col, -2);
1873
1874 curr_row++;
1875 }
1876
1877 gsl_matrix *new_A;
1878 gsl_matrix *V;
1879 gsl_vector *s;
1880 gsl_vector *x_res;
1881
1882 new_A = gsl_matrix_calloc(rows, cols);
1883 V = gsl_matrix_calloc(cols, cols);
1884 s = gsl_vector_calloc(cols);
1885 x_res = gsl_vector_calloc(cols);
1886
1887 buf_t *bn = buf_new();
1888 print_matrix(bn, A);
1889 multilatbuf_to_file(mls, "DeltatAis", bn);
1890 buf_free(bn);
1891
1892 int res = gsl_matrix_memcpy(new_A, A);
1893 if (res != GSL_SUCCESS) {
1894 elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
1895 exit(1);
1896 }
1897
1898 elog(LOG_WARNING, "Starting svdecomp");
1899 res = gsl_linalg_SV_decomp_jacobi(new_A, V, s);
1900
1901 if (res != GSL_SUCCESS) {
1902 elog(LOG_CRIT, "Error doing svdecomp: %s\n", gsl_strerror(res));
1903 goto mlat_failed;
1904 }
1905 elog(LOG_WARNING, "Starting SV solve");
1906 res = gsl_linalg_SV_solve(new_A, V, s, b, x_res);
1907 if (res != GSL_SUCCESS) {
1908 elog(LOG_CRIT, "Error doing svdecomp Solve: %s\n", gsl_strerror(res));
1909 goto mlat_failed;
1910 }
1911
1912 /* ok, should now have the answers! */
1913 for (node = result_list_top(mls->multilat_lists);
1914 node; node = result_list_next(node)) {
1915 if (node->col >= 0) {
1916 node->delta = gsl_vector_get(x_res, node->col);
1917 elog(LOG_WARNING, "node %d, delta=%.2f", node->node, node->delta);
1918 }
1919 }
1920 #endif
1921
1922 }
1923
1924
1925 int multilat_solver(ml_state_t *mls) {
1926
1927 buf_t *studbuf = NULL;
1928 mls->failed_convergence = 3;
1929 int total_iterations = 0;
1930 int total_passes = 0;
1931
1932 /*
1933 * configure peer relationships
1934 */
1935
1936 multilat_range_t *mtr;
1937
1938 LOG_ENTRY("****starting multilat");
1939
1940 /* clear peers */
1941 for (mtr = range_list_top(mls->multilat_lists);
1942 mtr; mtr = range_list_next(mtr)) {
1943 mtr->peer = NULL;
1944 if (mtr->distance > 0)
1945 mtr->drop_r = mtr->drop_t = mtr->drop_p = 0;
1946 }
1947
1948 /* set peers and results */
1949 for (mtr = range_list_top(mls->multilat_lists);
1950 mtr; mtr = range_list_next(mtr)) {
1951 if (mtr->peer == NULL) {
1952 multilat_range_t *mtr2;
1953 for (mtr2 = range_list_top(mls->multilat_lists);
1954 mtr2; mtr2 = range_list_next(mtr2)) {
1955 if (mtr != mtr2 &&
1956 mtr->chirp_from == mtr2->data_from &&
1957 mtr->data_from == mtr2->chirp_from &&
1958 mtr2->peer == NULL) {
1959 mtr->peer = mtr2;
1960 mtr2->peer = mtr;
1961 break;
1962 }
1963 }
1964
1965 mtr->result = result_list_get(mls, mtr->data_from);
1966 if (mtr->peer == NULL) {
1967 mtr2 = g_new0(multilat_range_t, 1);
1968 range_list_push(mls->multilat_lists, mtr2);
1969 mtr->peer = mtr2;
1970 mtr2->peer = mtr;
1971 mtr2->chirp_from = mtr->data_from;
1972 mtr2->data_from = mtr->chirp_from;
1973 mtr2->drop_r = mtr2->drop_t = mtr2->drop_p = 1;
1974 }
1975 mtr->peer->result = result_list_get(mls, mtr->peer->data_from);
1976 }
1977 }
1978
1979 /* compute the deltas.. */
1980 compute_deltas(mls);
1981
1982 for (mtr = range_list_top(mls->multilat_lists);
1983 mtr; mtr = range_list_next(mtr)) {
1984
1985 if (!mtr->drop_r && !mtr->peer->drop_r) {
1986 if (mtr->peer->distance > mtr->distance) {
1987 mtr->peer->userange = mtr->userange = mtr->distance;
1988 }
1989 else {
1990 mtr->peer->userange = mtr->userange = mtr->peer->distance;
1991 }
1992
1993 /* 8cm*sin(phi) accounts for excess length when coming in off plane */
1994 mtr->userange += sin(mtr->phi) * PHI_DIST_VARIATION;
1995 mtr->peer->userange += sin(mtr->peer->phi) * PHI_DIST_VARIATION;
1996 }
1997
1998 else {
1999 if (!mtr->drop_r)
2000 mtr->userange = mtr->distance + sin(mtr->phi) * PHI_DIST_VARIATION;
2001 if (!mtr->peer->drop_r)
2002 mtr->peer->userange = mtr->peer->distance + sin(mtr->peer->phi) * PHI_DIST_VARIATION;
2003 }
2004
2005 }
2006
2007 /* clear column assignments */
2008 multilat_result_t *node;
2009 for (node = result_list_top(mls->multilat_lists);
2010 node; node = result_list_next(node)) {
2011 node->col = -1;
2012 }
2013
2014 /* compute number of roots */
2015 int num_roots = 0;
2016 multilat_result_t *mlr;
2017 for (mlr = result_list_top(mls->multilat_lists);
2018 mlr; mlr = result_list_next(mlr)) {
2019 if (mlr->state == RESULT_COORD_ROOT)
2020 num_roots++;
2021 }
2022
2023 int runagain = 0;
2024 int num_rows = 0;
2025 int num_cols = 0;
2026
2027 buf_t *droppedbuf = buf_new();
2028
2029 int useVV = 0;
2030 #ifdef USEZ
2031 int axes = 3;
2032 #else
2033 int axes = 2;
2034 #endif
2035
2036 /******************/
2037 /* get dimensions */
2038 /******************/
2039 repeate:
2040
2041 if (mls->global_run != 0) {
2042 result_load(mls);
2043 }
2044
2045 mreal VV = 1;
2046
2047
2048 /***********/
2049 /* iterate */
2050 /***********/
2051 int k = 0;
2052 int orient_count = 0;
2053 for (k = 0; k < 100; ++k) {
2054
2055 elog(LOG_WARNING, "starting iterations %d", k);
2056 total_iterations++;
2057
2058 /* reset usage bits and columns */
2059 for (mtr = range_list_top(mls->multilat_lists);
2060 mtr; mtr = range_list_next(mtr)) {
2061 mtr->use_r = mtr->use_t = mtr->use_p = mtr->use_set = 0;
2062 mtr->result->col = -1;
2063 }
2064
2065 /* for each pair, decide which to use */
2066 for (mtr = range_list_top(mls->multilat_lists);
2067 mtr; mtr = range_list_next(mtr)) {
2068
2069 /* continue if already processed */
2070 if (mtr->use_set) continue;
2071
2072 /* set bit */
2073 mtr->use_set = 1;
2074 if (mtr->peer) mtr->peer->use_set = 1;
2075
2076 /* continue if between two roots */
2077 if (mtr->result->state == RESULT_COORD_ROOT &&
2078 mtr->peer && mtr->peer->result->state == RESULT_COORD_ROOT)
2079 continue;
2080
2081 /* if range is not dropped, use it */
2082 if (!mtr->drop_r)
2083 mtr->use_r = 1;
2084 else if (!mtr->peer->drop_r)
2085 mtr->peer->use_r = 1;
2086
2087 if (mls->useangles) {
2088
2089 /* use this theta angle if not dropped */
2090 if (!mtr->drop_t && mtr->peer && !mtr->peer->drop_t) {
2091 if (mtr->angle_conf > mtr->peer->angle_conf)
2092 mtr->use_t = 1;
2093 else
2094 mtr->peer->use_t = 1;
2095 }
2096 else if (!mtr->drop_t)
2097 mtr->use_t = 1;
2098 else if (mtr->peer && !mtr->peer->drop_t)
2099 mtr->peer->use_t = 1;
2100
2101 #ifndef HACK_OUT_PHI
2102 if (axes > 2) {
2103 /* use this phi angle if not dropped */
2104 if (!mtr->drop_p && mtr->peer && !mtr->peer->drop_p) {
2105 if (mtr->angle_conf > mtr->peer->angle_conf)
2106 mtr->use_p = 1;
2107 else
2108 mtr->peer->use_p = 1;
2109 }
2110 else if (!mtr->drop_p)
2111 mtr->use_p = 1;
2112 else if (mtr->peer && !mtr->peer->drop_p)
2113 mtr->peer->use_p = 1;
2114 }
2115 #endif
2116 }
2117 }
2118
2119 /* count rows and columns */
2120 num_rows = 0;
2121 num_cols = 0;
2122 for (mtr = range_list_top(mls->multilat_lists);
2123 mtr; mtr = range_list_next(mtr)) {
2124
2125 int eqns = (mtr->use_r + mtr->use_p + mtr->use_t);
2126 num_rows += eqns;
2127
2128 if (mtr->peer)
2129 eqns += (mtr->peer->use_r + mtr->peer->use_p + mtr->peer->use_t);
2130
2131 if (eqns > 0 && mtr->result->col == -1 && mtr->result->state != RESULT_COORD_ROOT) {
2132 mtr->result->col = num_cols;
2133 num_cols += axes;
2134 }
2135 }
2136
2137 elog(LOG_CRIT, "total rows %d, num cols %d", num_rows, num_cols);
2138 if (num_cols > num_rows) {
2139 elog(LOG_CRIT, "Can not continue with SVD only have %i by %i matrix", num_rows, num_cols);
2140 goto mlat_failed;
2141 }
2142
2143 if (num_cols == 0 || num_rows == 0) {
2144 elog(LOG_CRIT, "must have rows or cols");
2145 goto mlat_failed;
2146 }
2147
2148 /* clear row map */
2149 reset_matrix(mls, num_rows);
2150
2151 /* setup the matrix */
2152 gsl_matrix *A;
2153 gsl_vector *b;
2154
2155 int Vcol;
2156 elog(LOG_DEBUG(0), "allocing %d,%d matrix", num_rows, num_cols+useVV);
2157 A = gsl_matrix_calloc(num_rows, num_cols+useVV);
2158 b = gsl_vector_calloc(num_rows);
2159 Vcol = num_cols;
2160
2161 int curr_row = 0;
2162 for (mtr = range_list_top(mls->multilat_lists);
2163 mtr; mtr = range_list_next(mtr)) {
2164
2165 if (mtr->peer == NULL) {
2166 elog(LOG_WARNING, "no peer??");
2167 continue;
2168 }
2169
2170 multilat_result_t *node1 = mtr->result;
2171 multilat_result_t *node2 = mtr->peer->result;
2172
2173 mreal DX = node2->x - node1->x;
2174 mreal DY = node2->y - node1->y;
2175 mreal DZ = node2->z - node1->z;
2176 mreal D = sqrtf(sqrf(DX) + sqrf(DY) + sqrf(DZ));
2177
2178 if (mtr->use_r) {
2179
2180 /* protect against explosion */
2181 if (D < 1.0) {
2182 D = 1.0;
2183 }
2184
2185 mreal w = 1.0; //(mls->include_conf ? mtr->conf / 10.0 : 1.0);
2186
2187 if (node1->state != RESULT_COORD_ROOT) {
2188 gsl_matrix_set(A, curr_row, node1->col + 0, -VV/D*DX*w);
2189 gsl_matrix_set(A, curr_row, node1->col + 1, -VV/D*DY*w);
2190 if (axes > 2)
2191 gsl_matrix_set(A, curr_row, node1->col + 2, -VV/D*DZ*w);
2192 }
2193
2194 if (node2->state != RESULT_COORD_ROOT) {
2195 gsl_matrix_set(A, curr_row, node2->col + 0, VV/D*DX*w);
2196 gsl_matrix_set(A, curr_row, node2->col + 1, VV/D*DY*w);
2197 if (axes > 2)
2198 gsl_matrix_set(A, curr_row, node2->col + 2, VV/D*DZ*w);
2199 }
2200
2201 if (useVV)
2202 gsl_matrix_set(A, curr_row, Vcol, D*w);
2203
2204 gsl_vector_set(b, curr_row, (mtr->userange - VV*D)*w);
2205
2206 curr_row = addrow(mls, ML_CONSTRAINT_RANGE, mtr);
2207 }
2208
2209 /************/
2210 /* angle */
2211 /************/
2212
2213 // node 2 chirps
2214 // node 1 send data
2215
2216 if (mls->useangles) {
2217
2218 if (mtr->use_t) {
2219
2220 if (fabsf(DX) < 1.0 || fabsf(DY) < 1.0) {
2221 elog(LOG_WARNING, "skipping angle for %d->%d", node1->node, node2->node);
2222 if (k>10) {
2223 elog(LOG_CRIT, "DROPPING angle for %d->%d", node1->node, node2->node);
2224 mtr->drop_t = 1;
2225 }
2226 goto skipangle;
2227 }
2228
2229 double T = 1.0/(1.0+sqrf(DY/DX));
2230 double mply = 226 * mtr->angle_conf;
2231
2232 if (node1->state != RESULT_COORD_ROOT) {
2233 gsl_matrix_set(A, curr_row, node1->col + 0, mply * +T * (DY / sqrf(DX)));
2234 gsl_matrix_set(A, curr_row, node1->col + 1, mply * -T * (1 / DX));
2235 }
2236
2237 if (node2->state != RESULT_COORD_ROOT) {
2238 gsl_matrix_set(A, curr_row, node2->col + 0, mply * -T * (DY / sqrf(DX)));
2239 gsl_matrix_set(A, curr_row, node2->col + 1, mply * +T * (1 / DX));
2240 }
2241
2242 gsl_vector_set(b, curr_row, mply* misc_normalize_angle_rad_d
2243 ((mtr->theta - node1->yaw) - (atan2f(DY, DX))));
2244
2245 skipangle:
2246 curr_row = addrow(mls, ML_CONSTRAINT_THETA, mtr);
2247 }
2248
2249 if (axes > 2 && mtr->use_p) {
2250
2251 /*
2252 * weighing scheme:
2253 * sigma for range 3.8 cm
2254 * sigma for AZI 0.96 deg = 226x
2255 * sigma for ZEN 45-45 3.6 deg = 60x
2256 * sigma for ZEN 45-90 4.5 deg = 48x
2257 */
2258
2259 mreal use_phi = misc_normalize_angle_rad_d(mtr->phi);
2260
2261 #ifdef CORRECT_PHI
2262 mreal corr = misc_normalize_angle_rad_d
2263 (sin(mtr->theta)*node1->pitch + cos(mtr->theta)*node1->roll);
2264 elog(LOG_DEBUG(11), "phi was %f, roll correction %f, now is %f",
2265 r2d(use_phi), r2d(corr), r2d(corr+use_phi));
2266 use_phi += corr;
2267 #endif
2268
2269 double w = ((fabs(use_phi) > (45/180.0*M_PI)) ? 48.0 : 60.0) *
2270 3.0 * mtr->angle_conf; //3.0
2271
2272 float D_sqr = sqrf(DX) + sqrf(DY);
2273 if (D_sqr < 1.0) D_sqr = 1.0;
2274
2275 float D_prime = sqrtf(D_sqr);
2276
2277 float T_prime = 1.0/(1.0 + sqrf(DZ/D_prime));
2278
2279 float K = DZ * powf(D_sqr, -3.0/2.0);
2280
2281 if (node1->state != RESULT_COORD_ROOT) {
2282 gsl_matrix_set(A, curr_row, node1->col + 0, w*T_prime*K*DX);
2283 gsl_matrix_set(A, curr_row, node1->col + 1, w*T_prime*K*DY);
2284 gsl_matrix_set(A, curr_row, node1->col + 2, -w*T_prime/D_prime);
2285 }
2286
2287 if (node2->state != RESULT_COORD_ROOT) {
2288 gsl_matrix_set(A, curr_row, node2->col + 0, -w*T_prime*K*DX);
2289 gsl_matrix_set(A, curr_row, node2->col + 1, -w*T_prime*K*DY);
2290 gsl_matrix_set(A, curr_row, node2->col + 2, +w*T_prime/D_prime);
2291 }
2292
2293 gsl_vector_set(b, curr_row,
2294 w*misc_normalize_angle_rad_d
2295 (use_phi - misc_normalize_angle_rad_d(atan2f(DZ, D_prime))));
2296
2297 curr_row = addrow(mls, ML_CONSTRAINT_PHI, mtr);
2298 }
2299 }
2300 }
2301 elog(LOG_CRIT, "Just added %i rows", curr_row);
2302
2303
2304 /************/
2305 /* SVDecomp */
2306 /************/
2307
2308 gsl_matrix *new_A;
2309 gsl_matrix *V;
2310 gsl_vector *s;
2311 gsl_vector *x_res;
2312
2313 new_A = gsl_matrix_calloc(num_rows, num_cols + useVV);
2314 V = gsl_matrix_calloc(num_cols + useVV, num_cols + useVV);
2315 s = gsl_vector_calloc(num_cols + useVV);
2316 x_res = gsl_vector_calloc(num_cols + useVV);
2317
2318 buf_t *bn = buf_new();
2319 print_matrix(bn, A);
2320 multilatbuf_to_file(mls, "Ais", bn);
2321 buf_free(bn);
2322
2323 int res = gsl_matrix_memcpy(new_A, A);
2324 if (res != GSL_SUCCESS) {
2325 elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
2326 exit(1);
2327 }
2328
2329 elog(LOG_WARNING, "Starting svdecomp");
2330 res = gsl_linalg_SV_decomp_jacobi(new_A, V, s);
2331
2332 if (res != GSL_SUCCESS) {
2333 elog(LOG_CRIT, "Error doing svdecomp: %s\n", gsl_strerror(res));
2334 goto mlat_failed;
2335 }
2336 elog(LOG_WARNING, "Starting SV solve");
2337 res = gsl_linalg_SV_solve(new_A, V, s, b, x_res);
2338 if (res != GSL_SUCCESS) {
2339 elog(LOG_CRIT, "Error doing svdecomp Solve: %s\n", gsl_strerror(res));
2340 goto mlat_failed;
2341 }
2342 elog(LOG_WARNING, "Printing state");
2343 print_multitlat_state(mls, A, new_A, V, s, b, x_res, k);
2344 elog(LOG_WARNING, "Done");
2345
2346
2347
2348 /*********************************/
2349 /* update the results with x_res */
2350 /*********************************/
2351 int dobreak = 1;
2352 if (useVV) {
2353 VV += gsl_vector_get(x_res, Vcol);
2354 elog(LOG_WARNING, "V is now %f", VV);
2355 }
2356
2357 multilat_result_t *node;
2358 for (node = result_list_top(mls->multilat_lists);
2359 node; node = result_list_next(node)) {
2360 if (node->state != RESULT_COORD_ROOT && node->col >= 0) {
2361 node->x += (gsl_vector_get(x_res, node->col + 0));
2362 node->y += (gsl_vector_get(x_res, node->col + 1));
2363 if (axes > 2)
2364 node->z += (gsl_vector_get(x_res, node->col + 2));
2365 if (fabs(gsl_vector_get(x_res, node->col + 0)) > 0.01 ||
2366 fabs(gsl_vector_get(x_res, node->col + 1)) > 0.01 ||
2367 ((axes > 2) && fabs(gsl_vector_get(x_res, node->col + 2)) > 0.01)) {
2368 dobreak = 0;
2369 }
2370 }
2371 }
2372
2373 #ifdef STANDALONE_MULTILAT
2374 /* allow outlier rejection even if convergence failed */
2375 if (k > 50) dobreak = 1;
2376 #endif
2377
2378 /* only update orienation for first 10 iterations */
2379 if (orient_count<10) {
2380
2381 orient_count++;
2382
2383 /*************************/
2384 /* update yaw/pitch/roll */
2385 /*************************/
2386
2387 mreal totx=0;
2388 mreal toty=0;
2389 int totsum=0;
2390 //mreal max_err=0;
2391 //int reset_orient_count = 0;
2392
2393 /* for angle-check-mode */
2394 multilat_range_t *worst_angle = NULL;
2395 mreal worst_angle_value = 0;
2396
2397 for (node = result_list_top(mls->multilat_lists);
2398 node; node = result_list_next(node)) {
2399 /* compute orientation for roots if there is more than one of them.
2400 * otherwise, lock in specified orientation */
2401 //if (num_roots > 1 || (result_view[i]->state != RESULT_COORD_ROOT))
2402 {
2403 mreal yaw[2] = {0,0};
2404 mreal pitch[2] = {0,0};
2405 mreal roll[2] = {0,0};
2406
2407 for (mtr = range_list_top(mls->multilat_lists);
2408 mtr != NULL; mtr = range_list_next(mtr)) {
2409
2410 if (mtr->result == node && mtr->peer) {
2411
2412 multilat_result_t *chirp_node = mtr->peer->result;
2413
2414 mreal c[3];
2415 mreal ctheta,cphi;
2416 mreal dtheta,dphi;
2417
2418 /* unit vector of computed angle */
2419 c[0] = chirp_node->x - node->x;
2420 c[1] = chirp_node->y - node->y;
2421 c[2] = chirp_node->z - node->z;
2422
2423 mreal D = sqrt(sqrf(c[0]) + sqrf(c[1]) + sqrf(c[2]));
2424
2425 c[0] /= D;
2426 c[1] /= D;
2427 c[2] /= D;
2428
2429 /* zenith and azimuth angles */
2430 ctheta = atan2(c[1], c[0]);
2431 cphi = misc_normalize_angle_rad_d(atan2(c[2], sqrt(sqrf(c[0])+sqrf(c[1]))));
2432
2433 /* diffs */
2434 dtheta = ctheta - mtr->theta;
2435 dphi = cphi - misc_normalize_angle_rad_d(mtr->phi);
2436
2437 /* if orient_count == 10, then we are in angle-check mode. */
2438 if (orient_count == 10 && !mtr->drop_t) {
2439 mreal diff = misc_normalize_angle_rad_d
2440 (fabs(misc_normalize_angle_rad_d(dtheta) +
2441 misc_normalize_angle_rad_d(node->yaw)));
2442 if (diff > worst_angle_value) {
2443 worst_angle_value = diff;
2444 worst_angle = mtr;
2445 }
2446 }
2447
2448 if (!mtr->drop_t) {
2449 /* sum (could weight by confidence..) */
2450 yaw[0] += cos(dtheta)*cos(cphi);
2451 yaw[1] += sin(dtheta)*cos(cphi);
2452 }
2453
2454 if (!mtr->drop_p) { // && fabs(dphi) < (6/180.0*M_PI)) {
2455 elog(LOG_DEBUG(20),
2456 "dphi %f ctheta %f p += %f %f ",
2457 r2d(dphi), r2d(ctheta),
2458 fabs(cos(ctheta))*cos(dphi),
2459 cos(ctheta)*sin(dphi));
2460 elog(LOG_DEBUG(20),
2461 "dphi %f ctheta %f r += %f %f ",
2462 r2d(dphi), r2d(ctheta),
2463 fabs(sin(ctheta))*cos(dphi),
2464 sin(ctheta)*sin(dphi));
2465
2466 pitch[0] += fabs(cos(ctheta))*cos(dphi);
2467 pitch[1] += cos(ctheta)*sin(dphi);
2468
2469 roll[0] += fabs(sin(ctheta))*cos(dphi);
2470 roll[1] += sin(ctheta)*sin(dphi);
2471
2472 elog(LOG_DEBUG(20),
2473 " == %f,%f (%f)",
2474 pitch[0],pitch[1],
2475 r2d(atan2(pitch[1],pitch[0])));
2476 elog(LOG_DEBUG(20),
2477 " == %f,%f (%f)",
2478 roll[0],roll[1],
2479 r2d(atan2(roll[1],roll[0])));
2480 }
2481 else {
2482 elog(LOG_DEBUG(11), "skipping dphi=%f", dphi*180.0/M_PI);
2483 }
2484 }
2485 }
2486
2487 node->yaw = -atan2(yaw[1],yaw[0]);
2488 node->pitch = -atan2(pitch[1],pitch[0]);
2489 node->roll = -atan2(roll[1],roll[0]);
2490
2491 elog(LOG_DEBUG(11), "****node %d: avg yaw=%f pitch=%f roll=%f [%f,%f,%f] ",
2492 node->node, r2d(node->yaw),
2493 r2d(node->pitch),
2494 r2d(node->roll),
2495 sqrt(sqrf(yaw[0])+sqrf(yaw[1])),
2496 sqrt(sqrf(pitch[0])+sqrf(pitch[1])),
2497 sqrt(sqrf(roll[0])+sqrf(roll[1])));
2498
2499 totx += cos(node->yaw);
2500 toty += sin(node->yaw);
2501 totsum++;
2502 }
2503 }
2504
2505 /* if orient_count == 10, then we are in angle-check mode. */
2506 if (orient_count == 10) {
2507 if (!mls->noreject && worst_angle_value > d2r(20)) {
2508 elog(LOG_CRIT, "***DROPPING RANGE %d->%d because angle is off by %f!!!!",
2509 worst_angle->chirp_from, worst_angle->data_from, r2d(worst_angle_value));
2510 worst_angle->drop_r = worst_angle->drop_t = worst_angle->drop_p = 1;
2511 //reset_orient_count = 1;
2512 /* keep doing orientation if we dropped a range */
2513 // if (reset_orient_count)
2514 orient_count = 7;
2515 }
2516 }
2517
2518 mreal avgor = atan2(toty,totx);
2519 elog(LOG_WARNING, "average orientation is %f", avgor);
2520
2521 #ifdef ORIENT_BY_ROOT
2522 if (num_roots == 1) {
2523 for (node = result_list_top(mls->multilat_lists);
2524 node; node = result_list_next(node)) {
2525 if (node->state == RESULT_COORD_ROOT) {
2526 avgor = node->yaw;
2527 elog(LOG_WARNING, "root node orientation is %f", avgor);
2528 }
2529 }
2530 }
2531 #endif
2532
2533 mreal max_diff = -1;
2534
2535 /* rotate all points and unorient */
2536 for (node = result_list_top(mls->multilat_lists);
2537 node; node = result_list_next(node)) {
2538
2539 if (node->col < 0 && node->state != RESULT_COORD_ROOT) continue;
2540
2541 node->yaw -= avgor;
2542 mreal tmpx = node->x;
2543 mreal tmpy = node->y;
2544 node->x = cos(avgor)*tmpx - sin(avgor)*tmpy;
2545 node->y = sin(avgor)*tmpx + cos(avgor)*tmpy;
2546
2547 if (node->state != RESULT_COORD_ROOT && node->col >= 0) {
2548 mreal diff;
2549 diff = fabs(gsl_vector_get(x_res, node->col + 0) +
2550 (node->x - tmpx));
2551 if (diff > max_diff) max_diff = diff;
2552 diff = fabs(gsl_vector_get(x_res, node->col + 1) +
2553 (node->y - tmpy));
2554 if (diff > max_diff) max_diff = diff;
2555 }
2556 }
2557
2558 elog(LOG_WARNING, "MAX DIFF = %f", max_diff);
2559 if (max_diff >= 0 && max_diff < 0.01) dobreak = 1;
2560
2561 /* don't break until we've completed orientation stuff */
2562 dobreak = 0;
2563 }
2564
2565 #if 0
2566 /******************/
2567 /* out put errors */
2568 /******************/
2569
2570 /*
2571 * This can be done in the mess above, but to keep it from getting
2572 * too messy, we just repeat down here
2573 */
2574
2575 buf_t *resbuf = buf_new();
2576 bufprintf(resbuf,
2577 "#h chirper listener residual realrange realtheta realphi "
2578 "calcrange calctheta calcphi real-calcrange real-calctheta real-calcphi "
2579 "introdisterr introthetaerr introphierr\n");
2580
2581 buf_t *ares = buf_new();
2582 bufprintf(ares, "#h c l angle res type dz\n");
2583
2584 for (i = 0; i < num_rows; ++i) {
2585 mtr = get_row_range(mls, i);
2586 if (mtr == NULL) {
2587 elog(LOG_WARNING, "can't find range for row %d!!", i);
2588 exit(1);
2589 }
2590 multilat_result_t *node1 = mtr->result;
2591 multilat_result_t *node2 = mtr->peer->result;
2592
2593 /******************/
2594
2595 switch (get_row_type(mls, i)) {
2596 case ML_CONSTRAINT_RANGE:
2597 bufprintf(resbuf, "%i %i %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
2598 mtr->chirp_from,
2599 mtr->data_from,
2600 gsl_vector_get(b, i),
2601 mtr->real_distance,
2602 mtr->real_theta,
2603 mtr->real_phi,
2604 sqrtf(m_pow_2(node1->x - node2->x) + m_pow_2(node1->y - node2->y)),
2605 atan2f(node2->y - node1->y, node2->x - node1->x),
2606 0.0,
2607 mtr->real_distance -
2608 sqrtf(m_pow_2(node1->x - node2->x) +
2609 m_pow_2(node1->y - node2->y)),
2610 mtr->real_theta -
2611 atan2f(node2->y - node1->y, node2->x - node1->x),
2612 0.0,
2613 mtr->introerr_distance,
2614 mtr->introerr_theta,
2615 mtr->introerr_phi);
2616 break;
2617
2618 case ML_CONSTRAINT_THETA:
2619 bufprintf(ares, "%i %i %f %f theta 0\n",
2620 mtr->chirp_from,
2621 mtr->data_from,
2622 r2d(mtr->theta),
2623 gsl_vector_get(b, i));
2624 break;
2625
2626 case ML_CONSTRAINT_PHI:
2627 bufprintf(ares, "%i %i %f %f phi %f\n",
2628 mtr->chirp_from,
2629 mtr->data_from,
2630 r2d(mtr->phi),
2631 gsl_vector_get(b, i),
2632 mtr->peer->result->z - mtr->result->z);
2633 break;
2634
2635 default:
2636 break;
2637 }
2638 }
2639
2640 char angleres[4096];
2641 sprintf(angleres, "angleres%i", k);
2642 multilatbuf_to_file(mls, angleres, ares);
2643 buf_free(ares);
2644
2645 char errors[4096];
2646 sprintf(errors, "errors%i", k);
2647 multilatbuf_to_file(mls, errors, resbuf);
2648 buf_free(resbuf);
2649 #endif
2650
2651 /************************/
2652 /* studentized residual */
2653 /************************/
2654 // H = X ( XT X )-1 XT.
2655 {
2656 studbuf = buf_new();
2657
2658 #if 0 /* use martin's weird but working version.. */
2659 bufprintf(studbuf, "#h res stud real chirpfrom datafrom\n");
2660
2661 int l = 0;
2662 gsl_matrix *X = gsl_matrix_calloc(num_rows, num_cols);
2663 gsl_matrix *X2 = gsl_matrix_calloc(num_rows, num_cols);
2664 gsl_matrix *XTrans = gsl_matrix_calloc(num_cols, num_rows);
2665 gsl_matrix *XTrans2 = gsl_matrix_calloc(num_cols, num_rows);
2666 gsl_matrix *XXTrans = gsl_matrix_calloc(num_cols, num_cols);
2667 gsl_matrix *H = gsl_matrix_calloc(num_rows, num_rows);
2668 gsl_vector *stud = gsl_vector_calloc(num_rows);
2669 gsl_vector *eps = gsl_vector_calloc(num_rows);
2670 double sum = 0.0;
2671
2672 int res = gsl_matrix_memcpy(X, A);
2673 if (res != GSL_SUCCESS) {
2674 elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
2675 exit(1);
2676 }
2677
2678 res = gsl_matrix_transpose_memcpy(XTrans, X);
2679 if (res != GSL_SUCCESS) {
2680 elog(LOG_CRIT, "Error transposing new matrix: %s\n", gsl_strerror(res));
2681 exit(1);
2682 }
2683
2684 gsl_matrix_memcpy(XTrans2, XTrans);
2685 gsl_matrix_memcpy(X2, X);
2686
2687 gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, XTrans, X, 1.0, XXTrans);
2688 gsl_blas_dtrsm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
2689 gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, X2, XTrans2, 1.0, H);
2690
2691 /* compute residuals and the sum of squares */
2692 sum = 0;
2693 for (l = 0; l < num_rows; ++l) {
2694 mreal eps_sum = 0;
2695 int k;
2696 for (k = 0; k < num_cols; ++k) {
2697 eps_sum += gsl_matrix_get(A, l, k) * gsl_vector_get(x_res, k);
2698 }
2699 //elog(LOG_WARNING, "epssum=%f, b=%f", eps_sum, gsl_vector_get(b, l));
2700 eps_sum -= gsl_vector_get(b, l);
2701 gsl_vector_set(eps, l, eps_sum);
2702 sum += sqrf(eps_sum);
2703 }
2704
2705 /* compute the weighted residuals */
2706 mreal max_stud = 0;
2707 for (l = 0; l < num_rows; ++l) {
2708 mreal sigma = sqrt((sum - sqrf(gsl_vector_get(eps, l))) / (mreal)(num_rows - 3));
2709 mreal hat = sqrt(1.0 - gsl_matrix_get(H, l, l));
2710 mreal a = fabs(gsl_vector_get(eps, l) / (sigma * hat));
2711 gsl_vector_set(stud, l, a);
2712
2713 if (a > max_stud) max_stud = a;
2714
2715 mtr = get_row_range(mls, l);
2716 multilat_result_t *mrr2 = mtr->result;
2717 multilat_result_t *mrr1 = mtr->peer->result;
2718
2719 bufprintf(studbuf, "%f %f %f %i %i\n",
2720 gsl_vector_get(eps,l),
2721 gsl_vector_get(stud,l),
2722 /* $$$ customize based on type!! */
2723 mtr->real_distance - sqrtf(m_pow_2(mrr1->x - mrr2->x) + m_pow_2(mrr1->y - mrr2->y)),
2724 mtr->chirp_from,
2725 mtr->data_from
2726 );
2727 }
2728
2729 bufprintf(studbuf, "\n");
2730 gsl_vector_free(eps);
2731 #else
2732 bufprintf(studbuf, "#h res stud measured chirpfrom datafrom\n");
2733
2734 int l = 0;
2735 gsl_matrix *X = gsl_matrix_calloc(num_rows, 2);
2736 gsl_matrix *X2 = gsl_matrix_calloc(num_rows, 2);
2737 gsl_matrix *XTrans = gsl_matrix_calloc(2, num_rows);
2738 gsl_matrix *XTrans2 = gsl_matrix_calloc(2, num_rows);
2739 gsl_matrix *XXTrans = gsl_matrix_calloc(2, 2);
2740 gsl_matrix *H = gsl_matrix_calloc(num_rows, num_rows);
2741 gsl_vector *stud = gsl_vector_calloc(num_rows);
2742 double mean = 0.0;
2743 double var = 0.0;
2744
2745 for (l = 0; l < num_rows; ++l) {
2746 mreal resid = gsl_vector_get(b, l);
2747 mean += resid;
2748 gsl_matrix_set(X, l, 0, 1);
2749 gsl_matrix_set(X, l, 1, resid);
2750 gsl_matrix_set(X2, l, 0, 1);
2751 gsl_matrix_set(X2, l, 1, resid);
2752 gsl_matrix_set(XTrans, 0, l, 1);
2753 gsl_matrix_set(XTrans, 1, l, resid);
2754 gsl_matrix_set(XTrans2, 0, l, 1);
2755 gsl_matrix_set(XTrans2, 1, l, resid);
2756 }
2757 mean = mean / (num_rows + 0.0);
2758
2759 elog(LOG_WARNING, "mean is %f", mean);
2760 gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, XTrans, X, 1.0, XXTrans);
2761 gsl_blas_dtrsm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
2762 // upper or lower does not matter // gsl_blas_dtrsm(CblasRight, CblasLower, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
2763 gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, X2, XTrans2, 1.0, H);
2764
2765 /* calculate the variance */
2766 for (l = 0; l < num_rows; ++l) {
2767 var += m_pow_2(gsl_vector_get(b,l) - mean);
2768 // elog(LOG_WARNING, "%f", gsl_matrix_get(H,l,l));
2769 }
2770 var = var / (num_rows + 0.0);
2771 elog(LOG_WARNING, "var is %f", var);
2772
2773 mreal max_stud = 0;
2774 for (l = 0; l < num_rows; ++l) {
2775 double a = gsl_vector_get(b, l) / (sqrtf(var) * sqrtf(1.0 - gsl_matrix_get(H,l,l)));
2776 // gsl_vector_set(stud, l, a);
2777 gsl_vector_set(stud, l, a >= 0 ? a : -a);
2778
2779 if (gsl_vector_get(stud, l) > max_stud)
2780 max_stud = gsl_vector_get(stud, l);
2781
2782 mtr = get_row_range(mls, l);
2783 //multilat_result_t *mrr2 = mtr->result;
2784 //multilat_result_t *mrr1 = mtr->peer->result;
2785
2786 if (get_row_type(mls, l) == ML_CONSTRAINT_RANGE)
2787 bufprintf(studbuf, "%f %f %f %i %i\n",
2788 gsl_vector_get(b,l),
2789 gsl_vector_get(stud,l),
2790 mtr->distance,
2791 mtr->chirp_from,
2792 mtr->data_from
2793 );
2794 }
2795
2796 bufprintf(studbuf, "\n");
2797 #endif
2798
2799 if (dobreak && mls->studthresh != 0) {
2800 if (runagain == 0 && max_stud > mls->studthresh) {
2801 for (l = 0; l < num_rows; ++l) {
2802 if (gsl_vector_get(stud, l) == max_stud) {
2803
2804 last_drop_value = max_stud;
2805
2806 /* important! */
2807 runagain = 1;
2808
2809 mtr = get_row_range(mls, l);
2810 char *type = NULL;
2811
2812 switch (get_row_type(mls, l)) {
2813 case ML_CONSTRAINT_RANGE:
2814 type = "range";
2815 mtr->drop_r = 1; mtr->drop_t = 1; mtr->drop_p = 1;
2816 break;
2817 case ML_CONSTRAINT_THETA:
2818 type = "theta";
2819 mtr->drop_t = 1; break;
2820 case ML_CONSTRAINT_PHI:
2821 type = "phi";
2822 mtr->drop_p = 1; break;
2823 }
2824
2825 elog(LOG_WARNING, "dropping %s %d->%d (val is %f at row %i)",
2826 type, mtr->chirp_from, mtr->data_from, gsl_vector_get(stud, l), l);
2827
2828 bufprintf(droppedbuf, "%i %i %s %f %f %i\n", mtr->chirp_from, mtr->data_from, type,
2829 gsl_vector_get(b,l),
2830 gsl_vector_get(stud,l),
2831 mls->global_run
2832 );
2833
2834 multilatbuf_to_file(mls, "dropped", droppedbuf);
2835 }
2836 }
2837 }
2838
2839 gsl_matrix_free(X);
2840 gsl_matrix_free(X2);
2841 gsl_matrix_free(XTrans);
2842 gsl_matrix_free(XTrans2);
2843 gsl_matrix_free(XXTrans);
2844 gsl_matrix_free(H);
2845 gsl_vector_free(stud);
2846
2847 }
2848
2849 multilatbuf_to_file(mls, "compare", studbuf);
2850
2851 elog(LOG_WARNING, " ----- All done!");
2852 }
2853
2854 mreal avg_range_resid = 0;
2855 mreal avg_angle_resid = 0;
2856 mreal avg_phi_resid = 0;
2857 mreal rms_range_resid = 0;
2858 mreal rms_angle_resid = 0;
2859 mreal rms_phi_resid = 0;
2860 int range_count = 0;
2861 int theta_count = 0;
2862 int phi_count = 0;
2863 int l;
2864 for (l = 0; l < num_rows; ++l) {
2865 switch (get_row_type(mls, l)) {
2866 case ML_CONSTRAINT_RANGE:
2867 avg_range_resid += fabs(gsl_vector_get(b,l));
2868 rms_range_resid += sqrf(gsl_vector_get(b,l));
2869 range_count++;
2870 break;
2871 case ML_CONSTRAINT_THETA:
2872 avg_angle_resid += fabs(gsl_vector_get(b,l));
2873 rms_angle_resid += sqrf(gsl_vector_get(b,l));
2874 theta_count++;
2875 break;
2876 case ML_CONSTRAINT_PHI:
2877 avg_phi_resid += fabs(gsl_vector_get(b,l));
2878 rms_phi_resid += sqrf(gsl_vector_get(b,l));
2879 phi_count++;
2880 break;
2881 }
2882 }
2883
2884 if (range_count > 0) {
2885 avg_range_resid /= (float)range_count;
2886 rms_range_resid = sqrt(rms_range_resid/(float)range_count);
2887 }
2888
2889 if (theta_count > 0) {
2890 avg_angle_resid /= (float)theta_count;
2891 rms_angle_resid = sqrt(rms_angle_resid/(float)theta_count);
2892 }
2893
2894 if (phi_count > 0) {
2895 avg_phi_resid /= (float)phi_count;
2896 rms_phi_resid = sqrt(rms_phi_resid/(float)phi_count);
2897 }
2898
2899 elog(LOG_CRIT, "**** average residuals:");
2900 elog(LOG_CRIT, " range: %f angle: %f phi %f",
2901 avg_range_resid, avg_angle_resid, avg_phi_resid);
2902 elog(LOG_CRIT, "RMS range: %f angle: %f phi %f",
2903 rms_range_resid, rms_angle_resid, rms_phi_resid);
2904
2905 /**************/
2906 /* free stuff */
2907 /**************/
2908
2909 gsl_matrix_free(A);
2910 gsl_vector_free(b);
2911 gsl_matrix_free(new_A);
2912 gsl_matrix_free(V);
2913 gsl_vector_free(s);
2914 // gsl_vector_free(work);
2915 gsl_vector_free(x_res);
2916 // gsl_vector_free(check);
2917
2918 buf_t *firstpass = buf_new();
2919 char thefile[4096];
2920 memset(thefile, 0, 4096);
2921 sprintf(thefile, "pass%i", k);
2922 result_list_print(mls, firstpass);
2923 multilatbuf_to_file(mls, thefile, firstpass);
2924 buf_free(firstpass);
2925
2926 #ifndef STANDALONE_MULTILAT
2927 /*
2928 * check for explosion and update the status!
2929 */
2930
2931 /* pack up the answers and push them to the main thread */
2932 buf_t *result = buf_new();
2933 buf_t *answer = buf_new();
2934
2935 multilat_result_t *node1;
2936 for (node1 = result_list_top(mls->multilat_lists);
2937 node1; node1 = result_list_next(node1)) {
2938
2939 if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
2940 elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
2941 continue;
2942 }
2943
2944 if (fabs(node1->x) > 100000 || fabs(node1->y) > 100000 ||
2945 fabs(node1->z) > 100000 || fabs(node1->yaw) > 100000 ||
2946 !isnormal(node1->x) || !isnormal(node1->y) ||
2947 !isnormal(node1->z) || !isnormal(node1->yaw)) {
2948 elog(LOG_CRIT, "oops.. it blew up!");
2949 mls->failed_convergence = 1;
2950 buf_free(answer);
2951 buf_free(result);
2952 goto mlat_failed;
2953 }
2954
2955 coord_entry_t ce = {
2956 node: node1->node,
2957 coord: { node1->x*10, node1->y*10, node1->z*10 },
2958 rpy: { r2d(-node1->roll)*10, r2d(-node1->pitch)*10, r2d(-node1->yaw)*10 },
2959 valid: 1
2960 };
2961
2962 bufcpy(answer, &ce, sizeof(ce));
2963 }
2964
2965 buf_t *errors = mlat_errors_to_buf(mls, NULL);
2966
2967 bufcpy(result, &answer, sizeof(answer));
2968 bufcpy(result, &errors, sizeof(errors));
2969 bufcpy(result, &studbuf, sizeof(studbuf));
2970 studbuf = NULL;
2971
2972 g_msg_queue_push(mls->results_queue, result);
2973 #endif
2974
2975 if (dobreak) {
2976 dobreak = 0;
2977 break;
2978 }
2979
2980 //#ifdef STANDALONE_MULTILAT
2981 dump_errors(mls, k);
2982 //#endif
2983 }
2984
2985 /* print out the final error stuff */
2986 buf_t *ce = buf_new();
2987 bufprintf(ce, "#h node coorderror\n");
2988 mreal rms = 0;
2989 int count = 0;
2990 for (mlr = result_list_top(mls->multilat_lists);
2991 mlr; mlr = result_list_next(mlr)) {
2992 if (mlr->state != RESULT_COORD_ROOT) {
2993 bufprintf(ce, "%i %f\n", mlr->node,
2994 sqrtf(m_pow_2(mlr->x - mlr->real_x) +
2995 m_pow_2(mlr->y - mlr->real_y) +
2996 m_pow_2(mlr->z - mlr->real_z))
2997 );
2998 rms += m_pow_2(mlr->x - mlr->real_x) +
2999 m_pow_2(mlr->y - mlr->real_y) +
3000 m_pow_2(mlr->z - mlr->real_z);
3001 count++;
3002 }
3003
3004 }
3005 bufprintf(ce, "# rms: %f\n", sqrtf(rms / (count + 0.0)));
3006 bufprintf(ce, "# Grms: %f\n", mls->guess_rms);
3007 multilatbuf_to_file(mls, "coorderr", ce);
3008 buf_free(ce);
3009
3010 iterations = k;
3011 #ifdef STANDALONE_MULTILAT
3012 analyse_result(mls);
3013 #endif
3014
3015 /*************/
3016 /* increment */
3017 /*************/
3018
3019 if (runagain) {
3020 total_passes++;
3021 mls->global_run++;
3022 runagain = 0;
3023 goto repeate;
3024 }
3025
3026 multilatbuf_to_file(mls, "dropped", droppedbuf);
3027 buf_free(droppedbuf);
3028
3029 mls->global_run = 1000;
3030
3031 {
3032 buf_t *firstpass = buf_new();
3033 char thefile[4096];
3034 memset(thefile, 0, 4096);
3035 sprintf(thefile, "last");
3036 result_list_print(mls, firstpass);
3037 multilatbuf_to_file(mls, thefile, firstpass);
3038 buf_free(firstpass);
3039 }
3040
3041 #ifdef STANDALONE_MULTILAT
3042 analyse_result(mls);
3043 #endif
3044 mls->failed_convergence = 2;
3045
3046 LOG_ENTRY("****multilat success, %d, %d", total_iterations, total_passes);
3047 return 0;
3048
3049 mlat_failed:
3050 LOG_ENTRY("****multilat failed, %d, %d", total_iterations, total_passes);
3051 #ifdef STANDALONE_MULTILAT
3052 exit(1);
3053 #else
3054 return -1;
3055 #endif
3056 }
3057 #endif
3058
3059 void *multilat_thread_main(void *arg) {
3060
3061 ml_state_t *mls = (ml_state_t *) arg;
3062
3063 /* nice down */
3064 nice(10);
3065
3066 do_multilat:
3067
3068 #ifndef STANDALONE_MULTILAT
3069 /* wait for new data to appear */
3070 pthread_mutex_lock(&(mls->range_snapshot_mutex));
3071 while (!mls->master_mode || mls->latest_table == NULL) {
3072 pthread_cond_wait(&(mls->range_snapshot_cond),
3073 &(mls->range_snapshot_mutex));
3074 }
3075
3076 int i;
3077 multilat_range_t *mlr;
3078
3079 /* clear the last range set */
3080 while ((mlr = range_list_pop(mls->multilat_lists))) {
3081 free(mlr);
3082 }
3083 coord_guess_t *cgt;
3084 while ((cgt = guess_list_pop(mls->multilat_lists))) {
3085 free(cgt);
3086 }
3087 multilat_result_t *mrt;
3088 while ((mrt = result_list_pop(mls->multilat_lists))) {
3089 free(mrt);
3090 }
3091 mls->global_run = 0;
3092
3093 /* fill the new range set */
3094 for (i = 0; i < mls->latest_table_count; ++i) {
3095 mlr = g_new0(multilat_range_t, 1);
3096
3097 mlr->chirp_from = mls->latest_table[i].range_entry.source;
3098 mlr->data_from = mls->latest_table[i].header.flow_id.src;
3099
3100 if (mlr->chirp_from == mlr->data_from ||
3101 mlr->chirp_from == 0 || mlr->data_from == 0)
3102 goto free_it;
3103
3104 /* from tenths of degrees to radians */
3105 mlr->theta = (M_PI * (float) mls->latest_table[i].range_entry.theta) / 1800.0;
3106 mlr->phi = (M_PI * (float) mls->latest_table[i].range_entry.phi) / 1800.0;
3107
3108 mlr->distance = mls->latest_table[i].range_entry.distance / 10.0;
3109
3110 if (mlr->distance == 0)
3111 goto free_it;
3112
3113 #ifdef EMBEDDED_USECONF
3114 mlr->angle_conf = mls->latest_table[i].range_entry.a_conf / 10.0;
3115 mlr->conf = mls->latest_table[i].range_entry.conf / 10.0;
3116 #else
3117 mlr->angle_conf = 1;
3118 mlr->conf = 10;
3119 #endif
3120
3121 elog(LOG_WARNING, "Processing %d->%d, r=%f (cm) t=%f (%f) p=%f",
3122 mlr->chirp_from, mlr->data_from, mlr->distance, mlr->theta, r2d(mlr->theta), mlr->phi);
3123
3124 range_list_push(mls->multilat_lists, mlr);
3125 continue;
3126
3127 free_it:
3128 free(mlr);
3129 }
3130
3131 free(mls->latest_table);
3132 mls->latest_table = NULL;
3133 mls->latest_table_count = 0;
3134 pthread_mutex_unlock(&(mls->range_snapshot_mutex));
3135 #endif
3136
3137 elog(LOG_WARNING, "Other thread got new data, %d entries", range_list_qlen(mls->multilat_lists));
3138 update_result_list(mls); /* add new nodes to the result list */
3139
3140 /* dump the current range table */
3141 multilat_range_t *tmp;
3142 elog(LOG_WARNING, "DUMPING RANGE TABLE:");
3143 elog(LOG_WARNING, "src dst distance theta phi aconf conf state ur dr dt dp ur ut up us:");
3144 for (tmp = range_list_top(mls->multilat_lists); tmp; tmp=range_list_next(tmp)) {
3145 elog(LOG_WARNING, "%d %d %.1f %.2f %.2f %.1f %.1f %d %.1f %d %d %d %d %d %d %d",
3146 tmp->chirp_from, tmp->data_from, tmp->distance, tmp->theta, tmp->phi,
3147 tmp->angle_conf, tmp->conf, tmp->state, tmp->userange,
3148 tmp->drop_r, tmp->drop_t, tmp->drop_p,
3149 tmp->use_r, tmp->use_t, tmp->use_p, tmp->use_set);
3150 }
3151
3152 initialize_guess_stage(mls);
3153 if (!guess_coords(mls)) {
3154 goto do_multilat;
3155 }
3156
3157 buf_t *guessx = buf_new();
3158 result_list_print(mls, guessx);
3159 multilatbuf_to_file(mls, "step1", guessx);
3160 buf_free(guessx);
3161
3162 #ifdef STANDALONE_MULTILAT
3163 if (mls->addfuzz) {
3164 add_fuzz_to_results(mls);
3165 }
3166 #endif
3167
3168 guessx = buf_new();
3169 result_list_print(mls, guessx);
3170 multilatbuf_to_file(mls, "step2", guessx);
3171 buf_free(guessx);
3172
3173 #ifdef STANDALONE_MULTILAT
3174 multilat_result_t *res = result_list_top(mls->multilat_lists);
3175 mreal rms = 0;
3176 int count = 0;
3177 for ( ; res != NULL; res = result_list_next(res) ) {
3178 if (res->state != RESULT_COORD_ROOT) {
3179 rms += m_pow_2(res->x - res->real_x) +
3180 m_pow_2(res->y - res->real_y) +
3181 m_pow_2(res->z - res->real_z);
3182 count++;
3183 }
3184
3185 }
3186 mls->guess_rms = sqrtf(rms / (count + 0.0));
3187 #endif
3188
3189 /* note counts of nodes and ranges */
3190 mls->result_element_count = result_list_qlen(mls->multilat_lists);
3191 mls->range_element_count = range_list_qlen(mls->multilat_lists);
3192
3193 result_save(mls);
3194 multilat_solver(mls);
3195
3196 #ifdef STANDALONE_MULTILAT
3197 exit(1);
3198 #endif
3199
3200 goto do_multilat;
3201
3202 }
3203
3204
3205 void multilat_thread_init(ml_state_t *mls) {
3206
3207 /* setup mutex and cond */
3208 pthread_mutex_init(&(mls->range_snapshot_mutex), NULL);
3209 pthread_cond_init(&(mls->range_snapshot_cond), NULL);
3210
3211 /* spawn thread */
3212 pthread_attr_t thread_attr;
3213 pthread_attr_init(&thread_attr);
3214 if (pthread_create(&(mls->multilat_thread), &thread_attr, multilat_thread_main, mls) < 0) {
3215 elog(LOG_CRIT, "Failed to spawn compute thread: %m");
3216 exit(1);
3217 }
3218
3219 }
3220
3221
3222
3223 #ifdef STANDALONE_MULTILAT
3224
3225
3226 int killpairs[3][20];
3227 int killpairs_count = 0;
3228 char *killpairs_file = NULL;
3229
3230 void read_killpairs()
3231 {
3232 FILE *f = fopen(killpairs_file, "r");
3233 while (2 == fscanf(f, "%d %d", &(killpairs[0][killpairs_count]),
3234 &(killpairs[1][killpairs_count]))) {
3235 printf("read killpair %d %d\n",
3236 killpairs[0][killpairs_count],
3237 killpairs[1][killpairs_count]);
3238 killpairs[2][killpairs_count] = (random()%100 < killpairs_prob*100);
3239 killpairs_count++;
3240 }
3241 fclose(f);
3242 }
3243
3244
3245 /*
3246 * for median finding
3247 */
3248
3249 struct sorter {
3250 int index;
3251 float value;
3252 };
3253
3254 int sorter_cmp(const void *x, const void *y) {
3255 return ((struct sorter *)x)->value - ((struct sorter *)y)->value;
3256 }
3257
3258
3259 int read_in_file(ml_state_t *mls)
3260 {
3261 if ((mls->fakedata = fopen(mls->filename, "r")) == NULL) {
3262 elog(LOG_CRIT, "Unable to open file %s", mls->filename);
3263 exit(1);
3264 }
3265
3266
3267 int res = 0;
3268 int chirper = 0;
3269 int listener = 0;
3270 float distance = 0;
3271 float theta = 0;
3272 float phi = 0;
3273 float temp = 0;
3274 float real_distance = 0;
3275 float real_theta = 0;
3276 float real_phi = 0;
3277 float real_x = 0;
3278 float real_y = 0;
3279 float real_z = 0;
3280 float real_yaw = 0;
3281 float conf = 10;
3282 float aconf = 1;
3283
3284 char input[8192];
3285 memset(&input, 0, 8192);
3286
3287 while (1) {
3288
3289 if (fgets(input,8192,mls->fakedata) == NULL) {
3290 break;
3291 }
3292
3293 if (input[0] == '#') {
3294 continue;
3295 }
3296
3297 if (mls->include_conf) {
3298 res = sscanf(input, "%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
3299 &chirper, &listener, &distance, &theta, &phi,
3300 &real_x, &real_y, &real_z, &temp, &temp, &temp, &temp,
3301 &temp, &real_distance, &real_theta, &real_phi,
3302 &conf, &aconf);
3303 } else {
3304 res = sscanf(input, "%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
3305 &chirper, &listener, &distance, &theta, &phi,
3306 &real_x, &real_y, &real_z, &temp, &temp, &temp, &temp,
3307 &temp, &real_distance, &real_theta, &real_phi);
3308 }
3309 if (res == EOF) {
3310 elog(LOG_WARNING, "End of input file %s", mls->filename);
3311 break;
3312 }
3313 if (res != (mls->include_conf ? 18:16)) {
3314 elog(LOG_WARNING, "error in file %s; status was %d", mls->filename, res);
3315 continue;
3316 }
3317
3318 multilat_range_t *mlr = g_new0(multilat_range_t, 1);
3319 mlr->chirp_from = chirper;
3320 mlr->data_from = listener;
3321 if (mls->input_in_degrees) {
3322 mlr->theta = theta/180.0*M_PI;
3323 mlr->phi = phi/180.0*M_PI;
3324 }
3325 else {
3326 mlr->theta = theta;
3327 mlr->phi = phi;
3328 }
3329 mlr->distance = distance;
3330 mlr->real_distance = real_distance;
3331 mlr->real_theta = real_theta;
3332 mlr->real_phi = real_phi;
3333 mlr->introerr_distance = mlr->real_distance - mlr->distance;
3334 mlr->introerr_theta = mlr->real_theta - mlr->theta;
3335 mlr->introerr_phi = mlr->real_phi - mlr->phi;
3336 mlr->conf = conf;
3337 mlr->angle_conf = aconf;
3338
3339 int ll;
3340 for (ll=0; ll<killpairs_count; ll++) {
3341 if ((chirper == killpairs[0][ll] &&
3342 listener == killpairs[1][ll]) ||
3343 (chirper == killpairs[1][ll] &&
3344 listener == killpairs[0][ll])) {
3345 if (killpairs[2][ll]) {
3346
3347 /*
3348 * used for simulating reflections.\
3349 * you can set a list of pairs to remove in a file you
3350 * pass in. then you can also specify specific
3351 * ranges to cause to reflect.
3352 * reflections add 10m and may or may not add a 45 degree
3353 * angular error.
3354 */
3355
3356 int disterr = 1000;
3357 int angleerr = 0 * (chirper == killpairs[1][ll] ? +1 : -1);
3358 printf("offsetting %d,%d by %d cm, %d deg\n",
3359 chirper, listener, disterr, angleerr);
3360 mlr->distance += disterr;
3361 mlr->theta += angleerr*M_PI/180.0;
3362 }
3363 else {
3364 printf("nuking %d,%d\n",
3365 chirper, listener);
3366 mlr->conf = 0;
3367 }
3368 break;
3369 }
3370 }
3371
3372 if (mlr->conf == 0 || mlr->distance == 0) {
3373 elog(LOG_WARNING, "skipping 0 range %d->%d",
3374 mlr->chirp_from, mlr->data_from);
3375 free(mlr);
3376 goto skip;
3377 }
3378
3379 range_list_push(mls->multilat_lists, mlr);
3380
3381 result_list_initnode(mls, listener);
3382 result_list_initnode(mls, chirper);
3383 multilat_result_t *mtr = result_list_get(mls, chirper);
3384 mtr->real_x = real_x;
3385 mtr->real_y = real_y;
3386 mtr->real_z = real_z;
3387 //mtr->nogt = 0; // get it from the map file!!
3388
3389 // update_result_list(mls); /* add new nodes to the result list */
3390
3391 skip:
3392 elog(LOG_DEBUG(0), "read entry: %d %d %f %f %f", chirper, listener, theta, phi, distance);
3393 }
3394
3395 fclose(mls->fakedata);
3396
3397 if (mls->gt_filename) {
3398 FILE *f = fopen(mls->gt_filename, "r");
3399 if (f == NULL) {
3400 elog(LOG_WARNING, "unable to read gt from file %s", mls->gt_filename);
3401 exit(1);
3402 }
3403
3404 while (1) {
3405 if (fgets(input,8192,f) == NULL) {
3406 break;
3407 }
3408
3409 if (input[0] == '#') {
3410 continue;
3411 }
3412
3413 int node;
3414 res = sscanf(input, "%d %f %f %f %f",
3415 &node, &real_x, &real_y, &real_z, &real_yaw);
3416 if (res == EOF) {
3417 elog(LOG_WARNING, "End of input file %s", mls->gt_filename);
3418 break;
3419 }
3420 if (res != 5) {
3421 elog(LOG_WARNING, "error in file %s", mls->filename);
3422 continue;
3423 }
3424
3425 result_list_initnode(mls, node);
3426 multilat_result_t *mtr = result_list_get(mls, node);
3427 mtr->real_x = real_x*100.0;
3428 mtr->real_y = real_y*100.0;
3429 mtr->real_z = real_z*100.0;
3430 mtr->real_yaw = real_yaw;
3431 mtr->nogt = 0;
3432
3433 elog(LOG_WARNING, "read %d %f %f %f",
3434 node, real_x, real_y, real_z);
3435 }
3436
3437 fclose(f);
3438 }
3439
3440 /* prune duplicates, keep median phi angle */
3441 multilat_range_t *ptr;
3442 multilat_range_t *tmp;
3443 for (ptr = range_list_top(mls->multilat_lists); ptr; ptr = tmp) {
3444
3445 multilat_range_t *ptr2;
3446 multilat_range_t *tmp2;
3447 multilat_stages_t lists;
3448 memset(&lists, 0, sizeof(lists));
3449
3450 /* move dups into list */
3451 for (ptr2 = range_list_next(ptr); ptr2; ptr2 = tmp2) {
3452 tmp2 = range_list_next(ptr2);
3453 if (ptr->chirp_from == ptr2->chirp_from &&
3454 ptr->data_from == ptr2->data_from) {
3455 range_list_remove(mls->multilat_lists, ptr2);
3456 range_list_push(&lists, ptr2);
3457 }
3458 }
3459
3460 /* inc and move this one into list */
3461 tmp = range_list_next(ptr);
3462 range_list_remove(mls->multilat_lists, ptr);
3463 range_list_push(&lists, ptr);
3464 int r_count = range_list_qlen(&lists);
3465 struct sorter s[r_count];
3466
3467 /* now process list */
3468 int i;
3469 int q[5] = {0,0,0,0,0};
3470 for (i=0, ptr2 = range_list_top(&lists); ptr2; i++, ptr2 = range_list_next(ptr2)) {
3471 s[i].index = i;
3472 s[i].value = ptr2->theta;
3473 if (s[i].value < 0)
3474 s[i].value += (M_PI*2.0);
3475 q[(int)(s[i].value/(M_PI/2.0))]++;
3476 }
3477
3478 q[3] += q[4];
3479 q[4] = 0;
3480
3481 int maxq;
3482 int maxind=-1;
3483 for (i=0; i<4; i++) {
3484 int thisq = q[i] + q[(i+1)%4];
3485 if (maxind < 0 || thisq > maxq) {
3486 maxind = i;
3487 maxq = thisq;
3488 }
3489 }
3490
3491 /* flip? */
3492 if (maxind == 3) {
3493 for (i=0; i<r_count; i++)
3494 if (s[i].value > M_PI/2.0 && s[i].value < 3.0*M_PI/2.0)
3495 s[i].index = -1;
3496 else
3497 if (s[i].value > M_PI)
3498 s[i].value -= (2.0*M_PI);
3499 }
3500
3501 else {
3502 for (i=0; i<r_count; i++) {
3503 int q_num = ((int)(s[i].value / (M_PI/2.0)));
3504 if (q_num == 4) q_num = 3;
3505 if (q_num != maxind && q_num != maxind+1)
3506 s[i].index = -1;
3507 }
3508 }
3509
3510 /* filter stuff out of 2 quadrants */
3511 int drops=0;
3512 for (i=0; i<r_count; i++) {
3513 if (s[i].index < 0) {
3514 elog(LOG_WARNING, "dropping outlier value %f, for pair %d->%d",
3515 s[i].value, ptr->chirp_from, ptr->data_from);
3516 drops++;
3517 }
3518 else if (drops > 0) {
3519 s[i-drops] = s[i];
3520 }
3521 }
3522 r_count -= drops;
3523
3524 /* sort and keep median */
3525 if (r_count > 0) {
3526 qsort(&s, r_count, sizeof(struct sorter), sorter_cmp);
3527
3528 /* keep median */
3529 int med_idx = s[r_count/2].index;
3530 for (i=0, ptr2=range_list_top(&lists); i<med_idx;
3531 i++, ptr2=range_list_next(ptr2));
3532
3533 range_list_remove(&lists, ptr2);
3534 range_list_push_front(mls->multilat_lists, ptr2);
3535 elog(LOG_WARNING, "keeping median: %d->%d %f %f %f",
3536 ptr2->chirp_from, ptr2->data_from, ptr2->theta,
3537 ptr2->phi, ptr2->distance);
3538 }
3539
3540 /* free dups */
3541 while ((ptr2 = range_list_pop(&lists))) free(ptr2);
3542 }
3543
3544 return 0;
3545 }
3546
3547 #define OUTPUTPREFIX "/tmp/"
3548
3549 int main(int argc, char **argv) {
3550
3551
3552 ml_state_t mls = {
3553 master_mode: 1
3554 };
3555
3556 misc_init(&argc, argv, CVSTAG);
3557 char *tempstr1;
3558 char *tempstr2;
3559 float studthresh = 0;
3560
3561 mls.filename = misc_parse_out_option(&argc, argv, "file", 'f');
3562 if (mls.filename == NULL || strcmp(mls.filename, "") == 0) {
3563 elog(LOG_CRIT, "Please input valid filename!");
3564 exit(1);
3565 }
3566
3567 char *kp;
3568 if ((kp = misc_parse_out_option(&argc, argv, "kp", 0))) {
3569 sscanf(kp, "%d,%d", &killpairs[0][0], &killpairs[1][0]);
3570 killpairs[2][0] = 1;
3571 killpairs_count = 1;
3572 }
3573
3574 misc_parse_option_as_float(&argc, argv, "kprob", 0, &killpairs_prob);
3575 killpairs_file = misc_parse_out_option(&argc, argv, "killpairs", 0);
3576 if (killpairs_file)
3577 read_killpairs(killpairs_file);
3578
3579 mls.gt_filename = misc_parse_out_option(&argc, argv, "gt", 0);
3580
3581 mls.outputprefix = misc_parse_out_option(&argc, argv, "outputprefix", 'p');
3582 if (mls.outputprefix == NULL || strcmp(mls.outputprefix, "") == 0) {
3583 elog (LOG_CRIT, "no output prefix set, setting to /tmp");
3584 mls.outputprefix = OUTPUTPREFIX;
3585 }
3586
3587 mls.noreject = misc_parse_out_switch(&argc, argv, "no_reject", 0);
3588 mls.addfuzz = misc_parse_out_switch(&argc, argv, "fuzz", 'z');
3589 mls.useangles = misc_parse_out_switch(&argc, argv, "useangle", 'a');
3590 mls.input_in_degrees = misc_parse_out_switch(&argc, argv, "degrees", 0);
3591 mls.include_conf = misc_parse_out_switch(&argc, argv, "useconf", 0);
3592
3593 if (misc_parse_option_as_float(&argc, argv, "studres", 's', &studthresh) != 0) {
3594 studthresh = 0;
3595 }
3596 mls.studthresh = studthresh;
3597
3598 tempstr1 = misc_parse_out_option(&argc, argv, "one", '1');
3599 if (tempstr1 != NULL) {
3600 sscanf(tempstr1, "%i:%lf,%lf,%lf,%lf",
3601 &mls.anode1, &mls.x1, &mls.y1, &mls.z1, &mls.a1);
3602 mls.a1 = mls.a1/180.0*M_PI;
3603 elog(LOG_WARNING, "fixed node %i at %f %f %f, angle %f",
3604 mls.anode1, mls.x1, mls.y1, mls.z1, mls.a1);
3605 }
3606
3607 tempstr2 = misc_parse_out_option(&argc, argv, "two", '2');
3608 if (tempstr2 != NULL) {
3609 sscanf(tempstr2, "%i:%lf,%lf,%lf,%lf",
3610 &mls.anode2, &mls.x2, &mls.y2, &mls.z2,&mls.a2);
3611 mls.a2 = mls.a2/180.0*M_PI;
3612 elog(LOG_WARNING, "fixed node %i at %f %f %f angle %f",
3613 mls.anode2, mls.x2, mls.y2, mls.z2, mls.a2);
3614 }
3615
3616 mls.multilat_lists = g_new0(multilat_stages_t, 1);
3617 range_list_init(mls.multilat_lists);
3618 guess_list_init(mls.multilat_lists);
3619 result_list_init(mls.multilat_lists);
3620
3621 read_in_file(&mls);
3622
3623 multilat_thread_main(&mls);
3624
3625 exit(1);
3626 }
3627
3628
3629 #endif
3630
3631
3632
3633
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.