~ [ 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   
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 

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

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.