~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

Linux Cross Reference
cvs/emstar/devel/loc/multilat/multilat.c


  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