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