Show multilat.c syntax highlighted
/*
*
* Copyright (c) 2005 The Regents of the University of California. All
* rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* - Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* - Neither the name of the University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
* OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#include <stdio.h>
#include <math.h>
#include <emrun/emrun.h>
#include <libmisc/misc.h>
#include "multilat_i.h"
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_blas.h>
QUEUE_FUNCTION_INSTANTIATIONS(range_list,_,ranges,multilat_range_t,struct _multilat_stages);
float killpairs_prob = 0;
#define PHI_DIST_VARIATION 6.0
#define PSIG d2r(3.5/2.0)
#define TSIG d2r(0.96/2.0)
#define MAX_BIDIR_DIFF 300
#define MIN_BIDIR_DIFF 5
#undef AVERAGE_SMALL
#undef EMBEDDED_USECONF
int isnormal(double d)
{
if (isnan(d)) return 0;
if (isinf(d)) return 0;
return 1;
}
double misc_normalize_angle_rad_d(double rad_angle)
{
double result = fmod(rad_angle, 2.0 * M_PI);
if (result > M_PI) result -= (2.0 * M_PI);
else if (result < - M_PI) result += (2.0 * M_PI);
return result;
}
inline float m_pow_2(float x) { return x*x; }
void multilatbuf_to_file(ml_state_t *mls, char *filename, buf_t *buf) {
if (!mls->logs_off) {
// int thetime = time(NULL);
char f[4096];
// sprintf(f, "/tmp/%s-%d", filename, thetime);
sprintf(f, "%s/%04i-%s", mls->outputprefix, mls->global_run, filename);
elog(LOG_WARNING, "outputing to %s", f);
buf_to_file(f, buf);
}
else
elog(LOG_WARNING, "suppressing log file output..");
}
void print_matrix(buf_t *buf, gsl_matrix *m) {
int i = 0;
int j = 0;
for (i = 0; i < m->size1; ++i) {
for (j = 0; j < m->size2; ++j) {
bufprintf(buf, "%f ", gsl_matrix_get(m, i, j));
}
bufprintf(buf, "\n");
}
}
void print_vector(buf_t *buf, gsl_vector *v) {
int i = 0;
for (i = 0; i < v->size; ++i) {
bufprintf(buf, "%f ", gsl_vector_get(v, i));
}
bufprintf(buf, "\n");
}
void print_multitlat_state(ml_state_t *mls,
gsl_matrix *A,
gsl_matrix *new_A,
gsl_matrix *V,
gsl_vector *s,
gsl_vector *b,
gsl_vector *x,
int state)
{
buf_t *buf = buf_new();
char file[4096];
bufprintf(buf, "Current State:\n");
bufprintf(buf, "A was:\n");
print_matrix(buf, A);
bufprintf(buf, "\n\n");
bufprintf(buf, "A is:\n");
print_matrix(buf, new_A);
bufprintf(buf, "\nV is:\n");
print_matrix(buf, V);
bufprintf(buf,"\n\n s is:\n");
print_vector(buf, s);
bufprintf(buf,"\n\n b is:\n");
print_vector(buf, b);
bufprintf(buf,"\n\n x is:\n");
print_vector(buf, x);
bufprintf(buf, "\n\n\n");
sprintf(file, "state%i", state);
multilatbuf_to_file(mls, file, buf);
buf_free(buf);
}
struct aw {
mreal angle;
mreal weight;
mreal total;
};
struct aw angles[100];
int angle_count;
void init_cm()
{
angle_count = 0;
}
void add_cm(mreal theta, mreal weight)
{
angles[angle_count].angle = misc_normalize_angle_rad_d(theta);
angles[angle_count].weight = weight;
angles[angle_count].total = 0;
angle_count++;
}
int aw_cmp(const void *a,const void *b)
{
return ((struct aw *)a)->angle - ((struct aw *)b)->angle;
}
int realcmp(const void *a,const void *b)
{
mreal cmp = *((mreal *)a) - *((mreal *)b);
if (cmp < 0) return -1;
if (cmp > 0) return 1;
return 0;
}
mreal finish_cm()
{
qsort(angles, angle_count, sizeof(struct aw), aw_cmp);
int i;
float max_weight = 0;
int max_w_index = -1;
for (i=0; i<angle_count; i++) {
int j;
for (j=0; j<angle_count; j++)
if (fabs(misc_normalize_angle_rad_d(angles[j].angle - angles[i].angle)) < d2r(10))
angles[i].total += angles[j].weight;
if (angles[i].total > max_weight || max_w_index < 0) {
max_weight = angles[i].total;
max_w_index = i;
}
}
if (max_w_index >= 0) {
mreal t[2] = {0,0};
int j;
for (j=0; j<angle_count; j++) {
if (fabs(misc_normalize_angle_rad_d(angles[j].angle - angles[max_w_index].angle)) < d2r(10)) {
t[0] += angles[j].weight*cos(angles[j].angle);
t[1] += angles[j].weight*sin(angles[j].angle);
}
}
return atan2(t[1], t[0]);
}
elog(LOG_WARNING, "couldnt compute sloppy mode");
return 0;
}
struct error_stats {
mreal avg_range_diff;
mreal rms_range_diff;
mreal avg_theta_diff;
mreal rms_theta_diff;
mreal avg_phi_diff;
mreal rms_phi_diff;
int range_count;
int theta_count;
int phi_count;
};
buf_t *mlat_errors_to_buf(ml_state_t *mls, struct error_stats *stats)
{
mreal avg_range_diff = 0;
mreal rms_range_diff = 0;
mreal avg_theta_diff = 0;
mreal rms_theta_diff = 0;
mreal avg_phi_diff = 0;
mreal rms_phi_diff = 0;
int range_count = 0;
int theta_count = 0;
int phi_count = 0;
multilat_range_t *mtr;
buf_t *err_summ = buf_new();
bufprintf(err_summ, "#h chirper receiver r r_diff user t terr uset p perr usep yaw pitch roll\n");
for (mtr = range_list_top(mls->multilat_lists) ;
mtr != NULL; mtr = range_list_next(mtr))
if (mtr->distance > 0) {
multilat_result_t * mrr1 = result_list_get(mls, mtr->chirp_from);
multilat_result_t * mrr2 = result_list_get(mls, mtr->data_from);
mreal dist = sqrt(sqrf(mrr1->x - mrr2->x) +
sqrf(mrr1->y - mrr2->y) +
sqrf(mrr1->z - mrr2->z));
mreal phi = atan2(mrr1->z - mrr2->z,
sqrt(sqrf(mrr1->x - mrr2->x) +
sqrf(mrr1->y - mrr2->y)));
mreal theta = atan2(mrr1->y - mrr2->y, mrr1->x - mrr2->x);
mreal rerr = (mtr->userange != 0) ? mtr->userange - dist :
mtr->distance - dist;
mreal perr = misc_normalize_angle_rad_d(mtr->phi) -
misc_normalize_angle_rad_d(phi);
mreal terr =
misc_normalize_angle_rad_d
(misc_normalize_angle_rad_d(mtr->theta) -
misc_normalize_angle_rad_d(mrr2->yaw) -
misc_normalize_angle_rad_d(theta));
bufprintf(err_summ, "%d\t%d\t%f\t%f\t%c\t",
mtr->chirp_from, mtr->data_from, mtr->distance, rerr,
(mtr->state == RANGE_DROPPED || mtr->drop_r) ? 'N' :
#ifdef RTHETA
'Y'
#else
((mtr->use_r) ? 'Y' : 'M')
#endif
);
#ifdef RTHETA
if (!(mtr->state == RANGE_DROPPED || mtr->drop_r))
#else
if (mtr->use_r)
#endif
{
range_count++;
avg_range_diff += fabs(rerr);
rms_range_diff += sqrf(rerr);
}
#ifdef RTHETA
if (mtr->state != RANGE_DROPPED)
#else
if (mtr->use_t)
#endif
{
avg_theta_diff += fabs(terr);
rms_theta_diff += sqrf(terr);
theta_count++;
bufprintf(err_summ, "%f\t%f\tY\t", r2d(mtr->theta), r2d(terr));
}
else bufprintf(err_summ, "%f\t%f\t%c\t", r2d(mtr->theta), r2d(terr),
(mtr->state == RANGE_DROPPED || mtr->drop_t) ? 'N' : 'M');
#ifdef RTHETA
if (mtr->state != RANGE_DROPPED)
#else
if (mtr->use_p)
#endif
{
phi_count++;
avg_phi_diff += fabs(perr);
rms_phi_diff += sqrf(perr);
bufprintf(err_summ, "%f\t%f\tY\t", r2d(mtr->phi), r2d(perr));
}
else bufprintf(err_summ, "%f\t%f\t%c\t", r2d(mtr->phi), r2d(perr),
(mtr->state == RANGE_DROPPED || mtr->drop_p) ? 'N' : 'M');
bufprintf(err_summ, "%f\t%f\t%f\n", r2d(-mrr2->yaw), r2d(-mrr2->pitch), r2d(-mrr2->roll));
}
if (stats) {
stats->avg_range_diff = avg_range_diff;
stats->rms_range_diff = rms_range_diff;
stats->avg_theta_diff = avg_theta_diff;
stats->rms_theta_diff = rms_theta_diff;
stats->avg_phi_diff = avg_phi_diff;
stats->rms_phi_diff = rms_phi_diff;
stats->range_count = range_count;
stats->theta_count = theta_count;
stats->phi_count = phi_count;
}
return err_summ;
}
float last_drop_value = 0;
float min_drop_value = -1;
float min_xyz = -1;
float min_xyz_resid = 0;
int min_xyz_passes = 0;
int iterations = 0;
void dump_errors(ml_state_t *mls, int k)
{
buf_t *err_summ = mlat_errors_to_buf(mls, NULL);
char thefile[4096];
sprintf(thefile, "err-%d", k);
multilatbuf_to_file(mls, thefile, err_summ);
buf_free(err_summ);
}
/* compare the results to the ground truth.. */
void analyse_result(ml_state_t *mls)
{
/* compute raw average differences */
mreal avg_range_diff = 0;
mreal rms_range_diff = 0;
mreal avg_theta_diff = 0;
mreal rms_theta_diff = 0;
mreal avg_phi_diff = 0;
mreal rms_phi_diff = 0;
int range_count = 0;
int theta_count = 0;
int phi_count = 0;
struct error_stats stats = {};
buf_t *err_summ = mlat_errors_to_buf(mls, &stats);
avg_range_diff = stats.avg_range_diff;
rms_range_diff = stats.rms_range_diff;
avg_theta_diff = stats.avg_theta_diff;
rms_theta_diff = stats.rms_theta_diff;
avg_phi_diff = stats.avg_phi_diff;
rms_phi_diff = stats.rms_phi_diff;
range_count = stats.range_count;
theta_count = stats.theta_count;
phi_count = stats.phi_count;
if (range_count > 0) {
avg_range_diff /= (mreal)range_count;
avg_theta_diff /= (mreal)theta_count;
avg_phi_diff /= (mreal)phi_count;
rms_range_diff = sqrt(rms_range_diff / (mreal)range_count);
rms_theta_diff = sqrt(rms_theta_diff / (mreal)theta_count);
rms_phi_diff = sqrt(rms_phi_diff / (mreal)phi_count);
elog(LOG_WARNING, "AVG: r=%f t=%f p=%f",
avg_range_diff, r2d(avg_theta_diff), r2d(avg_phi_diff));
elog(LOG_WARNING, "RMS: r=%f t=%f p=%f",
rms_range_diff, r2d(rms_theta_diff), r2d(rms_phi_diff));
}
char thefile[4096];
sprintf(thefile, "final-err");
multilatbuf_to_file(mls, thefile, err_summ);
buf_free(err_summ);
#ifdef PROCRUSTES
/* compute centroid */
mreal cG[3] = {0,0,0};
mreal c[3] = {0,0,0};
multilat_result_t *node1, *node2;
int count = 0;
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
continue;
}
node1->X[0] = node1->x;
node1->X[1] = node1->y;
node1->X[2] = node1->z;
node1->XG[0] = node1->real_x;
node1->XG[1] = node1->real_y;
node1->XG[2] = node1->real_z;
count++;
for (i=0; i<3; i++) {
c[i] += X[i];
cG[i] += XG[i];
}
}
for (i=0; i<3; i++) {
c[i] /= (mreal)count;
cG[i] /= (mreal)count;
}
/* compute centroid sizes */
mreal S = 0;
mreal SG = 0;
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
continue;
}
for (i=0; i<3; i++) {
S += sqrf(node1->X[i] - c[i]);
SG += sqrf(node1->XG[i] - cG[i]);
}
}
S = sqrt(S);
SG = sqrt(SG);
/* pre-shape */
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
continue;
}
for (i=0; i<3; i++) {
node1->X[i] = (node1->X[i] - c[i]) / S;
node1->XG[i] = (node1->XG[i] - cG[i]) / SG;
}
}
/* rotations.. */
/* $$$ */
#else
/* compute scaling factor */
mreal tot_comp = 1.0;
mreal tot_gt = 1.0;
mreal V;
multilat_result_t *node1, *node2;
mreal c[3] = {0,0,0};
mreal cG[3] = {0,0,0};
int count = 0;
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
continue;
}
if (node1->nogt) {
elog(LOG_WARNING, "skipping node %d, no gt..", node1->node);
continue;
}
count++;
c[0] += node1->x;
c[1] += node1->y;
c[2] += node1->z;
cG[0] += node1->real_x;
cG[1] += node1->real_y;
cG[2] += node1->real_z;
mreal nD1=-1.0, nD2=-1.0;
for (node2 = result_list_top(mls->multilat_lists);
node2; node2 = result_list_next(node2)) {
if (node2->col < 0 && node2->state != RESULT_COORD_ROOT) continue;
if (node2->nogt) continue;
if (node1 != node2) {
mreal D1 = sqrt
(sqrf(node1->x - node2->x) +
sqrf(node1->y - node2->y) +
sqrf(node1->z - node2->z));
mreal D2 = sqrt
(sqrf(node1->real_x - node2->real_x) +
sqrf(node1->real_y - node2->real_y) +
sqrf(node1->real_z - node2->real_z));
if (nD1 < 0 || D1 < nD1) {
nD1 = D1;
nD2 = D2;
}
}
}
if (nD1 > 0) {
tot_comp += nD1;
tot_gt += nD2;
}
}
V = tot_comp / tot_gt;
/* centroid */
c[0] /= (mreal)count;
c[1] /= (mreal)count;
c[2] /= (mreal)count;
cG[0] /= (mreal)count;
cG[1] /= (mreal)count;
cG[2] /= (mreal)count;
#ifndef ROTATE_ABOUT_CENTROID
mreal min_dist = -1;
/* find center point */
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
if (node1->nogt) continue;
mreal D1 = sqrt
(sqrf(node1->x - c[0]) +
sqrf(node1->y - c[1]) +
sqrf(node1->z - c[2]));
if (min_dist < 0 || min_dist > D1) {
min_dist = D1;
node2 = node1;
}
}
/* node2 is closest to centroid */
elog(LOG_WARNING, "centroid is node %d", node2->node);
/* translate to center */
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
if (node1 != node2) {
node1->tx = node1->x - node2->x;
node1->ty = node1->y - node2->y;
node1->tz = node1->z - node2->z;
node1->trx = node1->real_x - node2->real_x;
node1->try = node1->real_y - node2->real_y;
node1->trz = node1->real_z - node2->real_z;
}
}
node2->tx = 0;
node2->ty = 0;
node2->tz = 0;
node2->trx = 0;
node2->try = 0;
node2->trz = 0;
#else
/* translate to centroid */
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
node1->tx = node1->x - c[0];
node1->ty = node1->y - c[1];
node1->tz = node1->z - c[2];
node1->trx = node1->real_x - cG[0];
node1->try = node1->real_y - cG[1];
node1->trz = node1->real_z - cG[2];
}
node2=NULL;
#endif
/* scale and init tyaw */
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
node1->tx /= V;
node1->ty /= V;
node1->tz /= V;
}
mreal rot;
mreal cumrot = 0;
goto retry_z;
flip:
/* FLIP */
elog(LOG_WARNING, "Flipped!");
cumrot = M_PI-cumrot;
retry_z:
#ifdef USEZ
init_cm();
/* determine Z axis rotation */
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
if (node1->nogt) continue;
if (node1 != node2) {
mreal D1 = sqrt(sqrf(node1->tx) + sqrf(node1->ty));
mreal dt =
atan2(node1->ty, node1->tx) -
atan2(node1->try, node1->trx);
add_cm(dt, D1);
}
}
rot = -finish_cm();
elog(LOG_WARNING, "Z axis rotation = %f", r2d(rot));
#endif
/* rotate */
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
elog(LOG_WARNING, "node %d is a root?", node1->node);
continue;
}
if (node1 != node2) {
mreal tmpx = node1->tx;
mreal tmpy = node1->ty;
node1->tx = (cos(rot)*tmpx - sin(rot)*tmpy);
node1->ty = (sin(rot)*tmpx + cos(rot)*tmpy);
}
}
cumrot += rot;
/* does our ground truth have a z axis? */
int have_gtz = 0;
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
if (node1->nogt) continue;
if (node1->real_z != 0) {
have_gtz = 1;
break;
}
}
if (have_gtz) {
/* determine Y axis rotation */
init_cm();
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
if (node1->nogt) continue;
if (node1 != node2) {
mreal D1 = sqrt(sqrf(node1->tx) + sqrf(node1->tz));
mreal dt =
atan2(node1->tz, node1->tx) -
atan2(node1->trz, node1->trx);
add_cm(dt, D1);
}
}
rot = -finish_cm();
elog(LOG_WARNING, "Y axis rotation = %f", r2d(rot));
/* rotate */
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
if (node1 != node2) {
mreal tmpx = node1->tx;
mreal tmpz = node1->tz;
node1->tx = (cos(rot)*tmpx - sin(rot)*tmpz);
node1->tz = (sin(rot)*tmpx + cos(rot)*tmpz);
}
}
if (r2d(rot) < -160 || r2d(rot) > 160) {
goto flip;
}
/* determine X axis rotation */
init_cm();
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
if (node1->nogt) continue;
if (node1 != node2) {
mreal D1 = sqrt(sqrf(node1->tz) + sqrf(node1->ty));
mreal dt =
atan2(node1->ty, node1->tz) -
atan2(node1->try, node1->trz);
add_cm(dt, D1);
}
}
rot = -finish_cm();
elog(LOG_WARNING, "X axis rotation = %f", r2d(rot));
/* rotate */
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
if (node1 != node2) {
mreal tmpz = node1->tz;
mreal tmpy = node1->ty;
node1->tz = (cos(rot)*tmpz - sin(rot)*tmpy);
node1->ty = (sin(rot)*tmpz + cos(rot)*tmpy);
}
}
if (r2d(rot) < -160 || r2d(rot) > 160) {
goto flip;
}
}
/* add in original yaw */
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
node1->tyaw = -node1->yaw + cumrot;
}
/* average translation */
mreal tr[3] = {0,0,0};
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
if (node1->nogt) continue;
mreal D = sqrt(sqrf(node1->trx-node1->tx) +
sqrf(node1->try-node1->ty) +
sqrf(node1->trz-node1->tz));
/* only add into average if < 1 meter away */
if (D < 100) {
tr[0] += node1->trx-node1->tx;
tr[1] += node1->try-node1->ty;
tr[2] += node1->trz-node1->tz;
}
}
tr[0] /= (mreal)count;
tr[1] /= (mreal)count;
tr[2] /= (mreal)count;
/* adjust nodes for translation */
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
node1->tx += tr[0];
node1->ty += tr[1];
node1->tz += tr[2];
}
#endif
/* compute the avg dist */
mreal avg_dist = 0;
mreal rms_dist = 0;
mreal avg_zdist = 0;
mreal rms_zdist = 0;
mreal worst_dist = 0;
mreal avg_zdist_d1 = 0;
mreal avg_yaw_err = 0;
mreal distarr[100];
mreal zdistarr[100];
mreal yawarr[100];
int distcount = 0;
buf_t *xybuf = buf_new();
buf_t *xyzbuf = buf_new();
buf_t *obuf = buf_new();
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) continue;
if (node1->nogt) continue;
mreal DZ = sqrt
(sqrf(node1->tx - node1->trx) +
sqrf(node1->ty - node1->try) +
sqrf(node1->tz - node1->trz));
mreal DXY = sqrt
(sqrf(node1->tx - node1->trx) +
sqrf(node1->ty - node1->try));
avg_dist += DXY;
rms_dist += sqrf(DXY);
avg_zdist += DZ;
rms_zdist += sqrf(DZ);
distarr[distcount] = DXY;
zdistarr[distcount] = DZ;
if (DZ > worst_dist) worst_dist = DZ;
mreal yaw_err = fmod(node1->real_yaw - r2d(node1->tyaw), 360.0);
if (yaw_err > 180) yaw_err -= 360.0;
if (yaw_err < -180) yaw_err += 360.0;
avg_yaw_err += fabs(yaw_err);
yawarr[distcount] = fabs(yaw_err);
distcount++;
elog(LOG_WARNING, "dist %d %f, yawerr=%f", node1->node, DXY, yaw_err);
bufprintf(xybuf, "%f %d\n", DXY, node1->node);
bufprintf(xyzbuf, "%f %d\n", DZ, node1->node);
bufprintf(obuf, "%f %d\n", yaw_err, node1->node);
}
qsort(distarr, distcount, sizeof(mreal), realcmp);
qsort(zdistarr, distcount, sizeof(mreal), realcmp);
qsort(yawarr, distcount, sizeof(mreal), realcmp);
mreal median_dist = distarr[distcount/2];
mreal median_zdist = zdistarr[distcount/2];
mreal median_yaw = yawarr[distcount/2];
#if 0
elog(LOG_WARNING, "distcount=%d, medianyaw=%f", distcount, median_yaw);
int klkl;
for (klkl=0;klkl<distcount;klkl++)
elog(LOG_WARNING, " yaw %d %f", klkl, yawarr[klkl]);
#endif
avg_dist /= (mreal)count;
rms_dist = sqrt(rms_dist / (mreal)count);
avg_yaw_err/= (mreal)count;
avg_zdist_d1 = (avg_zdist - worst_dist) / (mreal)(count-1);
avg_zdist /= (mreal)count;
rms_zdist = sqrt(rms_zdist / (mreal)count);
elog(LOG_WARNING, " V = %f", V);
elog(LOG_WARNING, " DIST = %f, RMS = %f, median = %f", avg_dist, rms_dist, median_dist);
elog(LOG_WARNING, " ZDIST = %f, RMS = %f, median = %f", avg_zdist, rms_zdist, median_zdist);
elog(LOG_WARNING, " YAWERR = %f", avg_yaw_err);
/* print out */
buf_t *firstpass = buf_new();
sprintf(thefile, "final");
bufprintf(firstpass, "#h node x y z yaw roll pitch gt_x gt_y gt_z\n");
multilat_result_t *rlt = result_list_top(mls->multilat_lists);
for ( ; rlt != NULL; rlt = result_list_next(rlt) ) {
if (rlt->col < 0 && rlt->state != RESULT_COORD_ROOT) continue;
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,
r2d(rlt->tyaw), r2d(-rlt->roll), r2d(-rlt->pitch),
rlt->trx, rlt->try, rlt->trz);
}
if (min_drop_value < 0 || last_drop_value < min_drop_value) min_drop_value = last_drop_value;
if (min_xyz < 0 || avg_zdist < min_xyz) {
min_xyz = avg_zdist;
min_xyz_resid = min_drop_value;
min_xyz_passes = mls->global_run;
}
bufprintf(firstpass, "# V = %f\n", V);
bufprintf(firstpass, "# avg_pos_err(xy) = %f, RMS = %f, med %f\n", avg_dist, rms_dist, median_dist);
bufprintf(firstpass, "# avg_pos_err(xyz) = %f, RMS = %f, med %f\n", avg_zdist, rms_zdist, median_zdist);
bufprintf(firstpass, "# avg_constraint_err: r=%f t=%f p=%f\n",
avg_range_diff, r2d(avg_theta_diff), r2d(avg_phi_diff));
bufprintf(firstpass, "# rms_constraint_err: r=%f t=%f p=%f\n",
rms_range_diff, r2d(rms_theta_diff), r2d(rms_phi_diff));
bufprintf(firstpass, "# yaw_avg %f median %f \n",
avg_yaw_err, median_yaw);
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",
V, avg_dist, avg_zdist, avg_range_diff, r2d(avg_theta_diff), r2d(avg_phi_diff),
last_drop_value, min_drop_value, count,
min_xyz, min_xyz_resid, min_xyz_passes, iterations, avg_zdist_d1,
median_dist, median_zdist, avg_yaw_err, median_yaw,
killpairs_prob, distarr[distcount-1]
);
multilatbuf_to_file(mls, thefile, firstpass);
buf_free(firstpass);
multilatbuf_to_file(mls, "3d-dist", xyzbuf);
multilatbuf_to_file(mls, "2d-dist", xybuf);
multilatbuf_to_file(mls, "yaw-dist", obuf);
buf_free(xyzbuf);
buf_free(xybuf);
buf_free(obuf);
}
#ifdef RTHETA
void update_minmax(mreal *min, mreal *max, mreal value)
{
if (value < *min) *min = value;
else if (value > *max) *max = value;
}
mreal do_xweight(multilat_range_t *mtr)
{
mreal min=0;
mreal max=0;
min = max = cos(mtr->theta + TSIG)*cos(mtr->phi + PSIG);
update_minmax(&min,&max,cos(mtr->theta - TSIG)*cos(mtr->phi + PSIG));
update_minmax(&min,&max,cos(mtr->theta + TSIG)*cos(mtr->phi - PSIG));
update_minmax(&min,&max,cos(mtr->theta - TSIG)*cos(mtr->phi - PSIG));
mreal w = sqrt(mtr->distance * (max-min));
if (w == 0) return 0;
return 1.0/w;
}
mreal do_yweight(multilat_range_t *mtr)
{
mreal min=0;
mreal max=0;
min = max = sin(mtr->theta + TSIG)*cos(mtr->phi + PSIG);
update_minmax(&min,&max,sin(mtr->theta - TSIG)*cos(mtr->phi + PSIG));
update_minmax(&min,&max,sin(mtr->theta + TSIG)*cos(mtr->phi - PSIG));
update_minmax(&min,&max,sin(mtr->theta - TSIG)*cos(mtr->phi - PSIG));
mreal w = sqrt(mtr->distance * (max-min));
if (w == 0) return 0;
return 1.0/w;
}
mreal do_zweight(multilat_range_t *mtr)
{
mreal min=0;
mreal max=0;
min = max = sin(mtr->phi + PSIG);
update_minmax(&min,&max,sin(mtr->phi + PSIG));
update_minmax(&min,&max,sin(mtr->phi - PSIG));
update_minmax(&min,&max,sin(mtr->phi - PSIG));
mreal w = sqrt(mtr->distance * (max-min));
if (w == 0) return 0;
return 1.0/w;
}
void multilat_solver(ml_state_t *mls) {
int i = 0;
/* the result_view array is the mapping of a 0 to num_nodes index of
all the results... it gives us a convienient way to address the
nodes without having to deal with incosistent node id's */
int num_nodes = result_list_qlen(mls->multilat_lists);
multilat_result_t *(result_view[num_nodes]);
result_list_make_array(mls, result_view, num_nodes);
#ifdef USEZ
int axes = 3;
#else
int axes = 2;
#endif
/* row is chirp_from, column is data_from */
int num_ranges = range_list_qlen(mls->multilat_lists);
multilat_range_t *mtr;
int total_dups = 0;
int runagain = 0;
int dropped_rows = 0;
int num_roots = 0;
buf_t *droppedbuf = buf_new();
/******************/
/* get dimensions */
/******************/
repeate:
if (mls->global_run != 0) {
result_load(mls);
}
num_roots = 0;
for (i = 0; i < num_nodes; ++i) {
if (result_view[i]->state == RESULT_COORD_ROOT)
num_roots++;
}
/* $$$ this would change with tiedowns.. not really right anyway */
int num_rows = (num_ranges - total_dups - (num_roots*(num_roots-1)))*axes - dropped_rows;
int num_cols = (num_nodes - num_roots)*axes;
elog(LOG_CRIT, "total ranges %i, dropped %i, rows,cols= %i,%i", num_ranges, dropped_rows,
num_rows, num_cols);
if (num_cols > num_rows) {
elog(LOG_CRIT, "Can not continue with SVD only have %i by %i matrix", num_rows, num_cols);
exit(1);
}
//mreal VV = 1;
/***********/
/* iterate */
/***********/
int k = 0;
int dobreak=0;
for (k = 0; k < 100; ++k) {
/* setup the matrix */
gsl_matrix *A;
gsl_vector *b;
elog(LOG_DEBUG(0), "allocing %d,%d matrix", num_rows, num_cols);
A = gsl_matrix_calloc(num_rows, num_cols);
b = gsl_vector_calloc(num_rows);
int node1_loc = 0, node2_loc = 0, curr_row = 0, curr_col = 0;
for (mtr = range_list_top(mls->multilat_lists), i = 0;
i < num_ranges; ++i, mtr = range_list_next(mtr) ) {
//elog(LOG_CRIT, "currrow: %i, i is %i", curr_row, i);
//elog(LOG_CRIT, "looking for df: %i cf: %i", mtr->data_from, mtr->chirp_from);
node1_loc = result_list_array_lookup(mls, result_view, mtr->data_from);
node2_loc = result_list_array_lookup(mls, result_view, mtr->chirp_from);
mtr->last_row = -1;
/* skip ranges between anchors */
if ( (mtr->data_from == mls->anode1 && mtr->chirp_from == mls->anode2) ||
(mtr->data_from == mls->anode2 && mtr->chirp_from == mls->anode1)
) {
// elog(LOG_WARNING, "!!!! found the anchors");
continue;
}
/* skip if dropped */
if ( mtr->state == RANGE_DROPPED ) {
continue;
}
mtr->last_row = curr_row;
/* find the columns if they exist... if not, set them */
if (result_view[node1_loc]->col == -1 && result_view[node1_loc]->state != RESULT_COORD_ROOT) {
result_view[node1_loc]->col = curr_col;
curr_col += axes;
}
if (result_view[node2_loc]->col == -1 && result_view[node2_loc]->state != RESULT_COORD_ROOT) {
result_view[node2_loc]->col = curr_col;
curr_col += axes;
}
/* node1_loc represents x1, y1, z1 ...
node2_loc represents x2, y2, z2 */
// node 2 chirps
// node 1 receives
/* $$$ missing VV and tiedowns */
mtr->userange = mtr->distance + PHI_DIST_VARIATION*sin(mtr->phi);
if (result_view[node1_loc]->state == RESULT_COORD_ROOT) {
/* add constraints */
mreal w = do_xweight(mtr);
gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 0, +1.0*w);
gsl_vector_set(b, curr_row,
(mtr->userange*cos(mtr->phi)*
cos(mtr->theta-result_view[node1_loc]->yaw) +
result_view[node1_loc]->x)*w);
curr_row++;
w = do_yweight(mtr);
gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 1, +1.0*w);
gsl_vector_set(b, curr_row,
(mtr->userange*cos(mtr->phi)*
sin(mtr->theta-result_view[node1_loc]->yaw) +
result_view[node1_loc]->y)*w);
curr_row++;
if (axes>2) {
w = do_zweight(mtr);
gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 2, +1.0*w);
gsl_vector_set(b, curr_row, (mtr->userange*sin(mtr->phi) +
result_view[node1_loc]->z)*w);
curr_row++;
}
}
else if (result_view[node2_loc]->state == RESULT_COORD_ROOT) {
/* add constraints */
mreal w = do_xweight(mtr);
gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 0, -1.0*w);
gsl_vector_set(b, curr_row,
(mtr->userange*cos(mtr->phi)*
cos(mtr->theta-result_view[node1_loc]->yaw) -
result_view[node2_loc]->x)*w);
curr_row++;
w = do_yweight(mtr);
gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 1, -1.0*w);
gsl_vector_set(b, curr_row,
(mtr->userange*cos(mtr->phi)*
sin(mtr->theta-result_view[node1_loc]->yaw) -
result_view[node2_loc]->y)*w);
curr_row++;
if (axes>2) {
w = do_zweight(mtr);
gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 2, -1.0*w);
gsl_vector_set(b, curr_row, (mtr->userange*sin(mtr->phi) -
result_view[node2_loc]->z)*w);
curr_row++;
}
}
else {
/* add constraints */
mreal w = do_xweight(mtr);
gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 0, -1.0*w);
gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 0, +1.0*w);
gsl_vector_set(b, curr_row,
(mtr->userange*cos(mtr->phi)*
cos(mtr->theta-result_view[node1_loc]->yaw))*w);
curr_row++;
w = do_yweight(mtr);
gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 1, -1*w);
gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 1, +1*w);
gsl_vector_set(b, curr_row,
(mtr->userange*cos(mtr->phi)*
sin(mtr->theta-result_view[node1_loc]->yaw))*w);
curr_row++;
if (axes>2) {
w = do_zweight(mtr);
gsl_matrix_set(A, curr_row, result_view[node1_loc]->col + 2, -1*w);
gsl_matrix_set(A, curr_row, result_view[node2_loc]->col + 2, +1*w);
gsl_vector_set(b, curr_row, mtr->userange*sin(mtr->phi)*w);
curr_row++;
}
}
}
elog(LOG_CRIT, "Just added %i rows", curr_row);
/************/
/* SVDecomp */
/************/
gsl_matrix *new_A;
gsl_matrix *V;
gsl_vector *s;
gsl_vector *x_res;
new_A = gsl_matrix_calloc(num_rows, num_cols);
V = gsl_matrix_calloc(num_cols, num_cols);
s = gsl_vector_calloc(num_cols);
x_res = gsl_vector_calloc(num_cols);
int res = gsl_matrix_memcpy(new_A, A);
if (res != GSL_SUCCESS) {
elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
exit(1);
}
elog(LOG_WARNING, "Starting svdecomp");
res = gsl_linalg_SV_decomp_jacobi(new_A, V, s);
if (res != GSL_SUCCESS) {
elog(LOG_CRIT, "Error doing svdecomp: %s\n", gsl_strerror(res));
exit(1);
}
elog(LOG_WARNING, "Starting SV solve");
res = gsl_linalg_SV_solve(new_A, V, s, b, x_res);
if (res != GSL_SUCCESS) {
elog(LOG_CRIT, "Error doing svdecomp Solve: %s\n", gsl_strerror(res));
exit(1);
}
elog(LOG_WARNING, "Printing state");
print_multitlat_state(mls, A, new_A, V, s, b, x_res, k);
elog(LOG_WARNING, "Done");
/*********************************/
/* update the results with x_res */
/*********************************/
for (i = 0; i < num_nodes; ++i) {
if (result_view[i]->state != RESULT_COORD_ROOT && result_view[i]->col >= 0) {
result_view[i]->x = gsl_vector_get(x_res, result_view[i]->col + 0);
result_view[i]->y = gsl_vector_get(x_res, result_view[i]->col + 1);
if (axes>2)
result_view[i]->z = gsl_vector_get(x_res, result_view[i]->col + 2);
}
}
/*************************/
/* update yaw/pitch/roll */
/*************************/
mreal totx=0;
mreal toty=0;
int totsum=0;
//mreal max_err=0;
int converged = 1;
multilat_result_t *node;
for (i = 0; i < num_nodes; ++i) {
multilat_result_t *node = result_view[i];
/* compute orientation for roots if there is more than one of them.
* otherwise, lock in specified orientation */
if (num_roots > 1 || (result_view[i]->state != RESULT_COORD_ROOT)) {
mreal yaw[2] = {0,0};
mreal pitch[2] = {0,0};
mreal roll[2] = {0,0};
for (mtr = range_list_top(mls->multilat_lists);
mtr != NULL; mtr = range_list_next(mtr)) {
if (mtr->data_from == result_view[i]->node) {
int chirp_index = result_list_array_lookup(mls, result_view, mtr->chirp_from);
multilat_result_t *chirp_node = result_view[chirp_index];
mreal c[3];
mreal ctheta,cphi;
mreal dtheta,dphi;
/* unit vector of computed angle */
c[0] = chirp_node->x - node->x;
c[1] = chirp_node->y - node->y;
c[2] = chirp_node->z - node->z;
mreal D = sqrt(sqrf(c[0]) + sqrf(c[1]) + sqrf(c[2]));
c[0] /= D;
c[1] /= D;
c[2] /= D;
/* zenith and azimuth angles */
ctheta = atan2(c[1], c[0]);
cphi = misc_normalize_angle_rad_d(atan2(c[2], sqrt(sqrf(c[0])+sqrf(c[1]))));
/* diffs */
dtheta = ctheta - mtr->theta;
dphi = cphi - misc_normalize_angle_rad_d(mtr->phi);
if (!mtr->drop_t) {
/* sum (could weight by confidence..) */
yaw[0] += cos(dtheta)*cos(cphi);
yaw[1] += sin(dtheta)*cos(cphi);
}
if (!mtr->drop_p) {// && fabs(dphi) < (6/180.0*M_PI)) {
elog(LOG_DEBUG(20),
"dphi %f ctheta %f p += %f %f ",
r2d(dphi), r2d(ctheta),
fabs(cos(ctheta))*cos(dphi),
cos(ctheta)*sin(dphi));
elog(LOG_DEBUG(20),
"dphi %f ctheta %f r += %f %f ",
r2d(dphi), r2d(ctheta),
fabs(sin(ctheta))*cos(dphi),
sin(ctheta)*sin(dphi));
pitch[0] += fabs(cos(ctheta))*cos(dphi);
pitch[1] += cos(ctheta)*sin(dphi);
roll[0] += fabs(sin(ctheta))*cos(dphi);
roll[1] += sin(ctheta)*sin(dphi);
elog(LOG_DEBUG(20),
" == %f,%f (%f)",
pitch[0],pitch[1],
r2d(atan2(pitch[1],pitch[0])));
elog(LOG_DEBUG(20),
" == %f,%f (%f)",
roll[0],roll[1],
r2d(atan2(roll[1],roll[0])));
}
else {
elog(LOG_DEBUG(11), "skipping dphi=%f", dphi*180.0/M_PI);
}
}
}
mreal new_yaw = -atan2(yaw[1],yaw[0]);
if (fabs(node->yaw - new_yaw) > 0.01)
converged = 0;
node->yaw = new_yaw;
node->pitch = -atan2(pitch[1],pitch[0]);
node->roll = -atan2(roll[1],roll[0]);
elog(LOG_DEBUG(11), "****node %d: avg yaw=%f pitch=%f roll=%f [%f,%f,%f] ",
node->node, r2d(node->yaw),
r2d(node->pitch),
r2d(node->roll),
sqrt(sqrf(yaw[0])+sqrf(yaw[1])),
sqrt(sqrf(pitch[0])+sqrf(pitch[1])),
sqrt(sqrf(roll[0])+sqrf(roll[1])));
totx += cos(node->yaw);
toty += sin(node->yaw);
totsum++;
}
}
mreal avgor = atan2(toty,totx);
elog(LOG_WARNING, "average orientation is %f", avgor);
#ifndef ORIENT_BY_ROOT
if (converged) {
mreal max_diff = -1;
/* rotate all points and unorient */
for (node = result_list_top(mls->multilat_lists);
node; node = result_list_next(node)) {
node->yaw -= avgor;
mreal tmpx = node->x;
mreal tmpy = node->y;
node->x = cos(avgor)*tmpx - sin(avgor)*tmpy;
node->y = sin(avgor)*tmpx + cos(avgor)*tmpy;
if (node->state != RESULT_COORD_ROOT && node->col >= 0) {
mreal diff;
diff = fabs(gsl_vector_get(x_res, node->col + 0) +
(node->x - tmpx));
if (diff > max_diff) max_diff = diff;
diff = fabs(gsl_vector_get(x_res, node->col + 1) +
(node->y - tmpy));
if (diff > max_diff) max_diff = diff;
}
}
elog(LOG_WARNING, "MAX DIFF = %f", max_diff);
}
#endif
if (converged) dobreak = 1;
/******************/
/* out put errors */
/******************/
/*
* This can be done in the mess above, but to keep it from getting
* too messy, we just repeat down here
*/
buf_t *resbuf = buf_new();
bufprintf(resbuf, "#h receiver chirper axis resid\n");
// declared earlier int node1_loc = 0, node2_loc = 0;
// for (i = 0; i < b->size; ++i) {
for (i = 0; i < num_rows; i+=axes) {
/* find the ranges */
for (mtr = range_list_top(mls->multilat_lists) ;
mtr != NULL; mtr = range_list_next(mtr)) {
if (i == mtr->last_row) {
break;
}
}
if (mtr == NULL) {
elog(LOG_WARNING, "Could not find range for that row!");
exit(1);
}
/* find the new calculated positions and the range */
node1_loc = result_list_array_lookup(mls, result_view, mtr->data_from);
node2_loc = result_list_array_lookup(mls, result_view, mtr->chirp_from);
int k;
for (k=0; k<axes; k++) {
int kk;
double sum = 0;
for (kk=0; kk<num_cols; kk++)
sum += (gsl_matrix_get(A, i+k, kk) * gsl_vector_get(x_res, kk));
sum -= gsl_vector_get(b, i+k);
bufprintf(resbuf, "%i %i %i %f\n",
mtr->data_from, mtr->chirp_from, k, sum);
/* $$ could also include range, angle error */
}
}
#if 0
char angleres[4096];
sprintf(angleres, "angleres%i", k);
multilatbuf_to_file(mls, angleres, ares);
buf_free(ares);
#endif
char errors[4096];
sprintf(errors, "errors%i", k);
multilatbuf_to_file(mls, errors, resbuf);
buf_free(resbuf);
/************************/
/* studentized residual */
/************************/
// H = X ( XT X )-1 XT.
if (dobreak && (mls->studthresh != 0)) {
/* num_rows is the number of ranges */
int l = 0;
gsl_matrix *X = gsl_matrix_calloc(num_rows, 2);
gsl_matrix *X2 = gsl_matrix_calloc(num_rows, 2);
gsl_matrix *XTrans = gsl_matrix_calloc(2, num_rows);
gsl_matrix *XTrans2 = gsl_matrix_calloc(2, num_rows);
gsl_matrix *XXTrans = gsl_matrix_calloc(2, 2);
gsl_matrix *H = gsl_matrix_calloc(num_rows, num_rows);
gsl_vector *stud = gsl_vector_calloc(num_rows);
double mean = 0.0;
double var = 0.0;
for (l = 0; l < num_rows; l++) {
mean += gsl_vector_get(b, l);
gsl_matrix_set(X, l, 0, 1);
gsl_matrix_set(X, l, 1, gsl_vector_get(b, l));
gsl_matrix_set(X2, l, 0, 1);
gsl_matrix_set(X2, l, 1, gsl_vector_get(b, l));
gsl_matrix_set(XTrans, 0, l, 1);
gsl_matrix_set(XTrans, 1, l, gsl_vector_get(b, l));
gsl_matrix_set(XTrans2, 0, l, 1);
gsl_matrix_set(XTrans2, 1, l, gsl_vector_get(b, l));
}
mean = mean / (num_rows + 0.0);
elog(LOG_WARNING, "mean is %f", mean);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, XTrans, X, 1.0, XXTrans);
gsl_blas_dtrsm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
// upper or lower does not matter // gsl_blas_dtrsm(CblasRight, CblasLower, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, X2, XTrans2, 1.0, H);
/* calculate the variance */
for (l = 0; l < num_rows; ++l) {
var += m_pow_2(gsl_vector_get(b,l) - mean);
// elog(LOG_WARNING, "%f", gsl_matrix_get(H,l,l));
}
var = var / (num_rows + 0.0);
elog(LOG_WARNING, "var is %f", var);
buf_t *studbuf = buf_new();
bufprintf(studbuf, "#h res stud real chirpfrom datafrom\n");
mreal maxstud = 0;
for (l = 0; l < num_rows; l++) {
double a = gsl_vector_get(b, l) / (sqrtf(var) * sqrtf(1.0 - gsl_matrix_get(H,l,l)));
// gsl_vector_set(stud, l, a);
gsl_vector_set(stud, l, a >= 0 ? a : -a);
if (gsl_vector_get(stud, l) > mls->studthresh) {
if (gsl_vector_get(stud, l) > maxstud) {
maxstud = gsl_vector_get(stud, l);
}
}
}
if (maxstud > mls->studthresh)
for (l = 0; l < num_rows; l++) {
int range_index = (l/axes)*axes;
if (gsl_vector_get(stud, l) == maxstud) {
/* important! */
runagain = 1;
elog(LOG_WARNING, "val is %f at row %i", gsl_vector_get(stud, l), l);
last_drop_value = maxstud;
for (mtr = range_list_top(mls->multilat_lists) ;
mtr != NULL; mtr = range_list_next(mtr)) {
if (range_index == mtr->last_row) {
break;
}
}
if (mtr) {
if (mtr->state != RANGE_DROPPED) {
dropped_rows += axes;
mtr->state = RANGE_DROPPED;
int from = mtr->chirp_from;
int to = mtr->data_from;
bufprintf(droppedbuf, "%i %i %f %f %i\n", from, to,
gsl_vector_get(b,l),
gsl_vector_get(stud,l),
mls->global_run
);
for (mtr = range_list_top(mls->multilat_lists) ;
mtr != NULL; mtr = range_list_next(mtr)) {
if ((from == mtr->chirp_from && to == mtr->data_from) ||
(to == mtr->chirp_from && from == mtr->data_from)) {
if (mtr->state != RANGE_DROPPED) {
dropped_rows += axes;
mtr->state = RANGE_DROPPED;
}
}
}
}
}
else
elog(LOG_CRIT, "cant' find range.. ");
}
for (mtr = range_list_top(mls->multilat_lists) ;
mtr != NULL; mtr = range_list_next(mtr)) {
if (range_index == mtr->last_row) {
break;
}
}
if (mtr) {
multilat_result_t * mrr1 = result_list_get(mls, mtr->chirp_from);
multilat_result_t * mrr2 = result_list_get(mls, mtr->data_from);
bufprintf(studbuf, "%f %f %f %i %i\n",
gsl_vector_get(b,l),
gsl_vector_get(stud,l),
mtr->real_distance -
sqrtf(m_pow_2(mrr1->x - mrr2->x) +
m_pow_2(mrr1->y - mrr2->y) +
m_pow_2(mrr1->z - mrr2->z)),
mtr->chirp_from,
mtr->data_from
);
}
else
elog(LOG_CRIT, "cant' find range for index %d (%d).. 2",
range_index, l);
}
multilatbuf_to_file(mls, "compare", studbuf);
buf_free(studbuf);
elog(LOG_WARNING, " ----- All done!");
gsl_matrix_free(X);
gsl_matrix_free(X2);
gsl_matrix_free(XTrans);
gsl_matrix_free(XTrans2);
gsl_matrix_free(XXTrans);
gsl_matrix_free(H);
gsl_vector_free(stud);
}
mreal avg_resid = 0;
mreal rms_resid = 0;
int range_count = 0;
int l;
for (l = 0; l < num_rows; ++l) {
avg_resid += fabs(gsl_vector_get(b,l));
rms_resid += sqrf(gsl_vector_get(b,l));
range_count++;
}
if (range_count > 0) {
avg_resid /= (float)range_count;
rms_resid = sqrt(rms_resid/(float)range_count);
}
elog(LOG_CRIT, "**** average residuals:");
elog(LOG_CRIT, "AVG: %f RMS: %f",
avg_resid, rms_resid);
/**************/
/* free stuff */
/**************/
gsl_matrix_free(A);
gsl_vector_free(b);
gsl_matrix_free(new_A);
gsl_matrix_free(V);
gsl_vector_free(s);
// gsl_vector_free(work);
gsl_vector_free(x_res);
// gsl_vector_free(check);
buf_t *firstpass = buf_new();
char thefile[4096];
memset(thefile, 0, 4096);
sprintf(thefile, "pass%i", k);
result_list_print(mls, firstpass);
multilatbuf_to_file(mls, thefile, firstpass);
buf_free(firstpass);
if (dobreak) {
dobreak = 0;
break;
}
}
/* print out the final error stuff */
buf_t *ce = buf_new();
bufprintf(ce, "#h node coorderror\n");
multilat_result_t *mlr = result_list_top(mls->multilat_lists);
mreal rms = 0;
int count = 0;
for ( ; mlr != NULL; mlr = result_list_next(mlr) ) {
if (mlr->state != RESULT_COORD_ROOT) {
bufprintf(ce, "%i %f\n", mlr->node,
sqrtf(m_pow_2(mlr->x - mlr->real_x) +
m_pow_2(mlr->y - mlr->real_y) +
m_pow_2(mlr->z - mlr->real_z))
);
rms += m_pow_2(mlr->x - mlr->real_x) +
m_pow_2(mlr->y - mlr->real_y) +
m_pow_2(mlr->z - mlr->real_z);
count++;
}
}
bufprintf(ce, "# rms: %f\n", sqrtf(rms / (count + 0.0)));
bufprintf(ce, "# Grms: %f\n", mls->guess_rms);
multilatbuf_to_file(mls, "coorderr", ce);
buf_free(ce);
iterations = k;
analyse_result(mls);
/*************/
/* increment */
/*************/
if (runagain) {
mls->global_run++;
runagain = 0;
goto repeate;
}
multilatbuf_to_file(mls, "dropped", droppedbuf);
buf_free(droppedbuf);
mls->global_run = 1000;
{
buf_t *firstpass = buf_new();
char thefile[4096];
memset(thefile, 0, 4096);
sprintf(thefile, "last");
result_list_print(mls, firstpass);
multilatbuf_to_file(mls, thefile, firstpass);
buf_free(firstpass);
}
analyse_result(mls);
return;
}
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
#else //!RTHETA
void reset_matrix(ml_state_t *mls, int num_rows)
{
if (mls->row_info) free(mls->row_info);
mls->row_info = g_new0(struct row_info, num_rows);
mls->row_count = 0;
}
int addrow(ml_state_t *mls, int type, multilat_range_t *new_range)
{
mls->row_info[mls->row_count].c_type = type;
mls->row_info[mls->row_count].range = new_range;
mls->row_count++;
return mls->row_count;
}
int get_row_type(ml_state_t *mls, int row)
{
return mls->row_info[row].c_type;
}
multilat_range_t *get_row_range(ml_state_t *mls, int row)
{
return mls->row_info[row].range;
}
/* compare the forward and reverse paths and compute
* estimates of the common delta for each node.
* the theory is that there may be a fixed delay for each node
* that accounts for the fwd/rev differences
*
* (Rij, i is the receiver)
* Rij = T + Di - Dj
* Rji = T + Dj - Di
* Rij - Di + Dj = Rji - Dj + Di
* Rij - Rji = 2Di - 2Dj
*/
void compute_deltas(ml_state_t *mls)
{
multilat_result_t *node;
multilat_range_t *mtr;
int rows = 0;
/* clear delta_in flags */
for (node = result_list_top(mls->multilat_lists);
node; node = result_list_next(node)) {
node->delta_in = 0;
node->col = -1;
}
/* count number of rows */
for (mtr = range_list_top(mls->multilat_lists);
mtr; mtr = range_list_next(mtr)) {
mtr->last_row = -1;
/* sort out ranges */
if (!mtr->drop_r && !mtr->peer->drop_r) {
if (fabs(mtr->peer->distance - mtr->distance) > MAX_BIDIR_DIFF ||
(mtr->peer->distance > MAX_BIDIR_DIFF &&
fabs(mtr->peer->distance - mtr->distance) > (mtr->distance * 0.1))) {
elog(LOG_WARNING, "dropping bogus range and angles..");
if (mtr->peer->distance > mtr->distance) {
mtr->peer->drop_r = mtr->peer->drop_p = mtr->peer->drop_t = 1;
}
else {
mtr->drop_r = mtr->drop_p = mtr->drop_t = 1;
}
}
}
if (!mtr->drop_r && !mtr->peer->drop_r) {
if (fabs(mtr->peer->distance - mtr->distance) < 20) {
mtr->result->delta_in++;
mtr->peer->result->delta_in++;
rows++;
}
}
}
rows /= 2;
#ifdef COMPUTE_DELTAS
/* count cols */
int cols = 0;
for (node = result_list_top(mls->multilat_lists);
node; node = result_list_next(node)) {
if (node->delta_in) cols++;
}
/* set up matrices */
gsl_matrix *A;
gsl_vector *b;
elog(LOG_WARNING, "allocing %d by %d matrix", rows, cols);
A = gsl_matrix_calloc(rows, cols);
b = gsl_vector_calloc(rows);
int curr_row = 0;
int curr_col = 0;
for (mtr = range_list_top(mls->multilat_lists);
mtr; mtr = range_list_next(mtr)) {
/* skip if... */
if (mtr->last_row >= 0) continue;
if (mtr->drop_r || mtr->peer->drop_r) continue;
if (fabs(mtr->peer->distance - mtr->distance) >= 20) continue;
/* ok, add it */
if (mtr->result->col < 0)
mtr->result->col = curr_col++;
if (mtr->peer->result->col < 0)
mtr->peer->result->col = curr_col++;
mtr->last_row = curr_row;
mtr->peer->last_row = curr_row;
/* b = Rij - Rji */
gsl_vector_set(b, curr_row, mtr->distance - mtr->peer->distance);
/* A = 2Di - 2Dj */
gsl_matrix_set(A, curr_row, mtr->result->col, +2);
gsl_matrix_set(A, curr_row, mtr->peer->result->col, -2);
curr_row++;
}
gsl_matrix *new_A;
gsl_matrix *V;
gsl_vector *s;
gsl_vector *x_res;
new_A = gsl_matrix_calloc(rows, cols);
V = gsl_matrix_calloc(cols, cols);
s = gsl_vector_calloc(cols);
x_res = gsl_vector_calloc(cols);
buf_t *bn = buf_new();
print_matrix(bn, A);
multilatbuf_to_file(mls, "DeltatAis", bn);
buf_free(bn);
int res = gsl_matrix_memcpy(new_A, A);
if (res != GSL_SUCCESS) {
elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
exit(1);
}
elog(LOG_WARNING, "Starting svdecomp");
res = gsl_linalg_SV_decomp_jacobi(new_A, V, s);
if (res != GSL_SUCCESS) {
elog(LOG_CRIT, "Error doing svdecomp: %s\n", gsl_strerror(res));
goto mlat_failed;
}
elog(LOG_WARNING, "Starting SV solve");
res = gsl_linalg_SV_solve(new_A, V, s, b, x_res);
if (res != GSL_SUCCESS) {
elog(LOG_CRIT, "Error doing svdecomp Solve: %s\n", gsl_strerror(res));
goto mlat_failed;
}
/* ok, should now have the answers! */
for (node = result_list_top(mls->multilat_lists);
node; node = result_list_next(node)) {
if (node->col >= 0) {
node->delta = gsl_vector_get(x_res, node->col);
elog(LOG_WARNING, "node %d, delta=%.2f", node->node, node->delta);
}
}
#endif
}
int multilat_solver(ml_state_t *mls) {
buf_t *studbuf = NULL;
mls->failed_convergence = 3;
int total_iterations = 0;
int total_passes = 0;
/*
* configure peer relationships
*/
multilat_range_t *mtr;
LOG_ENTRY("****starting multilat");
/* clear peers */
for (mtr = range_list_top(mls->multilat_lists);
mtr; mtr = range_list_next(mtr)) {
mtr->peer = NULL;
if (mtr->distance > 0)
mtr->drop_r = mtr->drop_t = mtr->drop_p = 0;
}
/* set peers and results */
for (mtr = range_list_top(mls->multilat_lists);
mtr; mtr = range_list_next(mtr)) {
if (mtr->peer == NULL) {
multilat_range_t *mtr2;
for (mtr2 = range_list_top(mls->multilat_lists);
mtr2; mtr2 = range_list_next(mtr2)) {
if (mtr != mtr2 &&
mtr->chirp_from == mtr2->data_from &&
mtr->data_from == mtr2->chirp_from &&
mtr2->peer == NULL) {
mtr->peer = mtr2;
mtr2->peer = mtr;
break;
}
}
mtr->result = result_list_get(mls, mtr->data_from);
if (mtr->peer == NULL) {
mtr2 = g_new0(multilat_range_t, 1);
range_list_push(mls->multilat_lists, mtr2);
mtr->peer = mtr2;
mtr2->peer = mtr;
mtr2->chirp_from = mtr->data_from;
mtr2->data_from = mtr->chirp_from;
mtr2->drop_r = mtr2->drop_t = mtr2->drop_p = 1;
}
mtr->peer->result = result_list_get(mls, mtr->peer->data_from);
}
}
/* compute the deltas.. */
compute_deltas(mls);
for (mtr = range_list_top(mls->multilat_lists);
mtr; mtr = range_list_next(mtr)) {
if (!mtr->drop_r && !mtr->peer->drop_r) {
if (mtr->peer->distance > mtr->distance) {
mtr->peer->userange = mtr->userange = mtr->distance;
}
else {
mtr->peer->userange = mtr->userange = mtr->peer->distance;
}
/* 8cm*sin(phi) accounts for excess length when coming in off plane */
mtr->userange += sin(mtr->phi) * PHI_DIST_VARIATION;
mtr->peer->userange += sin(mtr->peer->phi) * PHI_DIST_VARIATION;
}
else {
if (!mtr->drop_r)
mtr->userange = mtr->distance + sin(mtr->phi) * PHI_DIST_VARIATION;
if (!mtr->peer->drop_r)
mtr->peer->userange = mtr->peer->distance + sin(mtr->peer->phi) * PHI_DIST_VARIATION;
}
}
/* clear column assignments */
multilat_result_t *node;
for (node = result_list_top(mls->multilat_lists);
node; node = result_list_next(node)) {
node->col = -1;
}
/* compute number of roots */
int num_roots = 0;
multilat_result_t *mlr;
for (mlr = result_list_top(mls->multilat_lists);
mlr; mlr = result_list_next(mlr)) {
if (mlr->state == RESULT_COORD_ROOT)
num_roots++;
}
int runagain = 0;
int num_rows = 0;
int num_cols = 0;
buf_t *droppedbuf = buf_new();
int useVV = 0;
#ifdef USEZ
int axes = 3;
#else
int axes = 2;
#endif
/******************/
/* get dimensions */
/******************/
repeate:
if (mls->global_run != 0) {
result_load(mls);
}
mreal VV = 1;
/***********/
/* iterate */
/***********/
int k = 0;
int orient_count = 0;
for (k = 0; k < 100; ++k) {
elog(LOG_WARNING, "starting iterations %d", k);
total_iterations++;
/* reset usage bits and columns */
for (mtr = range_list_top(mls->multilat_lists);
mtr; mtr = range_list_next(mtr)) {
mtr->use_r = mtr->use_t = mtr->use_p = mtr->use_set = 0;
mtr->result->col = -1;
}
/* for each pair, decide which to use */
for (mtr = range_list_top(mls->multilat_lists);
mtr; mtr = range_list_next(mtr)) {
/* continue if already processed */
if (mtr->use_set) continue;
/* set bit */
mtr->use_set = 1;
if (mtr->peer) mtr->peer->use_set = 1;
/* continue if between two roots */
if (mtr->result->state == RESULT_COORD_ROOT &&
mtr->peer && mtr->peer->result->state == RESULT_COORD_ROOT)
continue;
/* if range is not dropped, use it */
if (!mtr->drop_r)
mtr->use_r = 1;
else if (!mtr->peer->drop_r)
mtr->peer->use_r = 1;
if (mls->useangles) {
/* use this theta angle if not dropped */
if (!mtr->drop_t && mtr->peer && !mtr->peer->drop_t) {
if (mtr->angle_conf > mtr->peer->angle_conf)
mtr->use_t = 1;
else
mtr->peer->use_t = 1;
}
else if (!mtr->drop_t)
mtr->use_t = 1;
else if (mtr->peer && !mtr->peer->drop_t)
mtr->peer->use_t = 1;
#ifndef HACK_OUT_PHI
if (axes > 2) {
/* use this phi angle if not dropped */
if (!mtr->drop_p && mtr->peer && !mtr->peer->drop_p) {
if (mtr->angle_conf > mtr->peer->angle_conf)
mtr->use_p = 1;
else
mtr->peer->use_p = 1;
}
else if (!mtr->drop_p)
mtr->use_p = 1;
else if (mtr->peer && !mtr->peer->drop_p)
mtr->peer->use_p = 1;
}
#endif
}
}
/* count rows and columns */
num_rows = 0;
num_cols = 0;
for (mtr = range_list_top(mls->multilat_lists);
mtr; mtr = range_list_next(mtr)) {
int eqns = (mtr->use_r + mtr->use_p + mtr->use_t);
num_rows += eqns;
if (mtr->peer)
eqns += (mtr->peer->use_r + mtr->peer->use_p + mtr->peer->use_t);
if (eqns > 0 && mtr->result->col == -1 && mtr->result->state != RESULT_COORD_ROOT) {
mtr->result->col = num_cols;
num_cols += axes;
}
}
elog(LOG_CRIT, "total rows %d, num cols %d", num_rows, num_cols);
if (num_cols > num_rows) {
elog(LOG_CRIT, "Can not continue with SVD only have %i by %i matrix", num_rows, num_cols);
goto mlat_failed;
}
if (num_cols == 0 || num_rows == 0) {
elog(LOG_CRIT, "must have rows or cols");
goto mlat_failed;
}
/* clear row map */
reset_matrix(mls, num_rows);
/* setup the matrix */
gsl_matrix *A;
gsl_vector *b;
int Vcol;
elog(LOG_DEBUG(0), "allocing %d,%d matrix", num_rows, num_cols+useVV);
A = gsl_matrix_calloc(num_rows, num_cols+useVV);
b = gsl_vector_calloc(num_rows);
Vcol = num_cols;
int curr_row = 0;
for (mtr = range_list_top(mls->multilat_lists);
mtr; mtr = range_list_next(mtr)) {
if (mtr->peer == NULL) {
elog(LOG_WARNING, "no peer??");
continue;
}
multilat_result_t *node1 = mtr->result;
multilat_result_t *node2 = mtr->peer->result;
mreal DX = node2->x - node1->x;
mreal DY = node2->y - node1->y;
mreal DZ = node2->z - node1->z;
mreal D = sqrtf(sqrf(DX) + sqrf(DY) + sqrf(DZ));
if (mtr->use_r) {
/* protect against explosion */
if (D < 1.0) {
D = 1.0;
}
mreal w = 1.0; //(mls->include_conf ? mtr->conf / 10.0 : 1.0);
if (node1->state != RESULT_COORD_ROOT) {
gsl_matrix_set(A, curr_row, node1->col + 0, -VV/D*DX*w);
gsl_matrix_set(A, curr_row, node1->col + 1, -VV/D*DY*w);
if (axes > 2)
gsl_matrix_set(A, curr_row, node1->col + 2, -VV/D*DZ*w);
}
if (node2->state != RESULT_COORD_ROOT) {
gsl_matrix_set(A, curr_row, node2->col + 0, VV/D*DX*w);
gsl_matrix_set(A, curr_row, node2->col + 1, VV/D*DY*w);
if (axes > 2)
gsl_matrix_set(A, curr_row, node2->col + 2, VV/D*DZ*w);
}
if (useVV)
gsl_matrix_set(A, curr_row, Vcol, D*w);
gsl_vector_set(b, curr_row, (mtr->userange - VV*D)*w);
curr_row = addrow(mls, ML_CONSTRAINT_RANGE, mtr);
}
/************/
/* angle */
/************/
// node 2 chirps
// node 1 send data
if (mls->useangles) {
if (mtr->use_t) {
if (fabsf(DX) < 1.0 || fabsf(DY) < 1.0) {
elog(LOG_WARNING, "skipping angle for %d->%d", node1->node, node2->node);
if (k>10) {
elog(LOG_CRIT, "DROPPING angle for %d->%d", node1->node, node2->node);
mtr->drop_t = 1;
}
goto skipangle;
}
double T = 1.0/(1.0+sqrf(DY/DX));
double mply = 226 * mtr->angle_conf;
if (node1->state != RESULT_COORD_ROOT) {
gsl_matrix_set(A, curr_row, node1->col + 0, mply * +T * (DY / sqrf(DX)));
gsl_matrix_set(A, curr_row, node1->col + 1, mply * -T * (1 / DX));
}
if (node2->state != RESULT_COORD_ROOT) {
gsl_matrix_set(A, curr_row, node2->col + 0, mply * -T * (DY / sqrf(DX)));
gsl_matrix_set(A, curr_row, node2->col + 1, mply * +T * (1 / DX));
}
gsl_vector_set(b, curr_row, mply* misc_normalize_angle_rad_d
((mtr->theta - node1->yaw) - (atan2f(DY, DX))));
skipangle:
curr_row = addrow(mls, ML_CONSTRAINT_THETA, mtr);
}
if (axes > 2 && mtr->use_p) {
/*
* weighing scheme:
* sigma for range 3.8 cm
* sigma for AZI 0.96 deg = 226x
* sigma for ZEN 45-45 3.6 deg = 60x
* sigma for ZEN 45-90 4.5 deg = 48x
*/
mreal use_phi = misc_normalize_angle_rad_d(mtr->phi);
#ifdef CORRECT_PHI
mreal corr = misc_normalize_angle_rad_d
(sin(mtr->theta)*node1->pitch + cos(mtr->theta)*node1->roll);
elog(LOG_DEBUG(11), "phi was %f, roll correction %f, now is %f",
r2d(use_phi), r2d(corr), r2d(corr+use_phi));
use_phi += corr;
#endif
double w = ((fabs(use_phi) > (45/180.0*M_PI)) ? 48.0 : 60.0) *
3.0 * mtr->angle_conf; //3.0
float D_sqr = sqrf(DX) + sqrf(DY);
if (D_sqr < 1.0) D_sqr = 1.0;
float D_prime = sqrtf(D_sqr);
float T_prime = 1.0/(1.0 + sqrf(DZ/D_prime));
float K = DZ * powf(D_sqr, -3.0/2.0);
if (node1->state != RESULT_COORD_ROOT) {
gsl_matrix_set(A, curr_row, node1->col + 0, w*T_prime*K*DX);
gsl_matrix_set(A, curr_row, node1->col + 1, w*T_prime*K*DY);
gsl_matrix_set(A, curr_row, node1->col + 2, -w*T_prime/D_prime);
}
if (node2->state != RESULT_COORD_ROOT) {
gsl_matrix_set(A, curr_row, node2->col + 0, -w*T_prime*K*DX);
gsl_matrix_set(A, curr_row, node2->col + 1, -w*T_prime*K*DY);
gsl_matrix_set(A, curr_row, node2->col + 2, +w*T_prime/D_prime);
}
gsl_vector_set(b, curr_row,
w*misc_normalize_angle_rad_d
(use_phi - misc_normalize_angle_rad_d(atan2f(DZ, D_prime))));
curr_row = addrow(mls, ML_CONSTRAINT_PHI, mtr);
}
}
}
elog(LOG_CRIT, "Just added %i rows", curr_row);
/************/
/* SVDecomp */
/************/
gsl_matrix *new_A;
gsl_matrix *V;
gsl_vector *s;
gsl_vector *x_res;
new_A = gsl_matrix_calloc(num_rows, num_cols + useVV);
V = gsl_matrix_calloc(num_cols + useVV, num_cols + useVV);
s = gsl_vector_calloc(num_cols + useVV);
x_res = gsl_vector_calloc(num_cols + useVV);
buf_t *bn = buf_new();
print_matrix(bn, A);
multilatbuf_to_file(mls, "Ais", bn);
buf_free(bn);
int res = gsl_matrix_memcpy(new_A, A);
if (res != GSL_SUCCESS) {
elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
exit(1);
}
elog(LOG_WARNING, "Starting svdecomp");
res = gsl_linalg_SV_decomp_jacobi(new_A, V, s);
if (res != GSL_SUCCESS) {
elog(LOG_CRIT, "Error doing svdecomp: %s\n", gsl_strerror(res));
goto mlat_failed;
}
elog(LOG_WARNING, "Starting SV solve");
res = gsl_linalg_SV_solve(new_A, V, s, b, x_res);
if (res != GSL_SUCCESS) {
elog(LOG_CRIT, "Error doing svdecomp Solve: %s\n", gsl_strerror(res));
goto mlat_failed;
}
elog(LOG_WARNING, "Printing state");
print_multitlat_state(mls, A, new_A, V, s, b, x_res, k);
elog(LOG_WARNING, "Done");
/*********************************/
/* update the results with x_res */
/*********************************/
int dobreak = 1;
if (useVV) {
VV += gsl_vector_get(x_res, Vcol);
elog(LOG_WARNING, "V is now %f", VV);
}
multilat_result_t *node;
for (node = result_list_top(mls->multilat_lists);
node; node = result_list_next(node)) {
if (node->state != RESULT_COORD_ROOT && node->col >= 0) {
node->x += (gsl_vector_get(x_res, node->col + 0));
node->y += (gsl_vector_get(x_res, node->col + 1));
if (axes > 2)
node->z += (gsl_vector_get(x_res, node->col + 2));
if (fabs(gsl_vector_get(x_res, node->col + 0)) > 0.01 ||
fabs(gsl_vector_get(x_res, node->col + 1)) > 0.01 ||
((axes > 2) && fabs(gsl_vector_get(x_res, node->col + 2)) > 0.01)) {
dobreak = 0;
}
}
}
#ifdef STANDALONE_MULTILAT
/* allow outlier rejection even if convergence failed */
if (k > 50) dobreak = 1;
#endif
/* only update orienation for first 10 iterations */
if (orient_count<10) {
orient_count++;
/*************************/
/* update yaw/pitch/roll */
/*************************/
mreal totx=0;
mreal toty=0;
int totsum=0;
//mreal max_err=0;
//int reset_orient_count = 0;
/* for angle-check-mode */
multilat_range_t *worst_angle = NULL;
mreal worst_angle_value = 0;
for (node = result_list_top(mls->multilat_lists);
node; node = result_list_next(node)) {
/* compute orientation for roots if there is more than one of them.
* otherwise, lock in specified orientation */
//if (num_roots > 1 || (result_view[i]->state != RESULT_COORD_ROOT))
{
mreal yaw[2] = {0,0};
mreal pitch[2] = {0,0};
mreal roll[2] = {0,0};
for (mtr = range_list_top(mls->multilat_lists);
mtr != NULL; mtr = range_list_next(mtr)) {
if (mtr->result == node && mtr->peer) {
multilat_result_t *chirp_node = mtr->peer->result;
mreal c[3];
mreal ctheta,cphi;
mreal dtheta,dphi;
/* unit vector of computed angle */
c[0] = chirp_node->x - node->x;
c[1] = chirp_node->y - node->y;
c[2] = chirp_node->z - node->z;
mreal D = sqrt(sqrf(c[0]) + sqrf(c[1]) + sqrf(c[2]));
c[0] /= D;
c[1] /= D;
c[2] /= D;
/* zenith and azimuth angles */
ctheta = atan2(c[1], c[0]);
cphi = misc_normalize_angle_rad_d(atan2(c[2], sqrt(sqrf(c[0])+sqrf(c[1]))));
/* diffs */
dtheta = ctheta - mtr->theta;
dphi = cphi - misc_normalize_angle_rad_d(mtr->phi);
/* if orient_count == 10, then we are in angle-check mode. */
if (orient_count == 10 && !mtr->drop_t) {
mreal diff = misc_normalize_angle_rad_d
(fabs(misc_normalize_angle_rad_d(dtheta) +
misc_normalize_angle_rad_d(node->yaw)));
if (diff > worst_angle_value) {
worst_angle_value = diff;
worst_angle = mtr;
}
}
if (!mtr->drop_t) {
/* sum (could weight by confidence..) */
yaw[0] += cos(dtheta)*cos(cphi);
yaw[1] += sin(dtheta)*cos(cphi);
}
if (!mtr->drop_p) { // && fabs(dphi) < (6/180.0*M_PI)) {
elog(LOG_DEBUG(20),
"dphi %f ctheta %f p += %f %f ",
r2d(dphi), r2d(ctheta),
fabs(cos(ctheta))*cos(dphi),
cos(ctheta)*sin(dphi));
elog(LOG_DEBUG(20),
"dphi %f ctheta %f r += %f %f ",
r2d(dphi), r2d(ctheta),
fabs(sin(ctheta))*cos(dphi),
sin(ctheta)*sin(dphi));
pitch[0] += fabs(cos(ctheta))*cos(dphi);
pitch[1] += cos(ctheta)*sin(dphi);
roll[0] += fabs(sin(ctheta))*cos(dphi);
roll[1] += sin(ctheta)*sin(dphi);
elog(LOG_DEBUG(20),
" == %f,%f (%f)",
pitch[0],pitch[1],
r2d(atan2(pitch[1],pitch[0])));
elog(LOG_DEBUG(20),
" == %f,%f (%f)",
roll[0],roll[1],
r2d(atan2(roll[1],roll[0])));
}
else {
elog(LOG_DEBUG(11), "skipping dphi=%f", dphi*180.0/M_PI);
}
}
}
node->yaw = -atan2(yaw[1],yaw[0]);
node->pitch = -atan2(pitch[1],pitch[0]);
node->roll = -atan2(roll[1],roll[0]);
elog(LOG_DEBUG(11), "****node %d: avg yaw=%f pitch=%f roll=%f [%f,%f,%f] ",
node->node, r2d(node->yaw),
r2d(node->pitch),
r2d(node->roll),
sqrt(sqrf(yaw[0])+sqrf(yaw[1])),
sqrt(sqrf(pitch[0])+sqrf(pitch[1])),
sqrt(sqrf(roll[0])+sqrf(roll[1])));
totx += cos(node->yaw);
toty += sin(node->yaw);
totsum++;
}
}
/* if orient_count == 10, then we are in angle-check mode. */
if (orient_count == 10) {
if (!mls->noreject && worst_angle_value > d2r(20)) {
elog(LOG_CRIT, "***DROPPING RANGE %d->%d because angle is off by %f!!!!",
worst_angle->chirp_from, worst_angle->data_from, r2d(worst_angle_value));
worst_angle->drop_r = worst_angle->drop_t = worst_angle->drop_p = 1;
//reset_orient_count = 1;
/* keep doing orientation if we dropped a range */
// if (reset_orient_count)
orient_count = 7;
}
}
mreal avgor = atan2(toty,totx);
elog(LOG_WARNING, "average orientation is %f", avgor);
#ifdef ORIENT_BY_ROOT
if (num_roots == 1) {
for (node = result_list_top(mls->multilat_lists);
node; node = result_list_next(node)) {
if (node->state == RESULT_COORD_ROOT) {
avgor = node->yaw;
elog(LOG_WARNING, "root node orientation is %f", avgor);
}
}
}
#endif
mreal max_diff = -1;
/* rotate all points and unorient */
for (node = result_list_top(mls->multilat_lists);
node; node = result_list_next(node)) {
if (node->col < 0 && node->state != RESULT_COORD_ROOT) continue;
node->yaw -= avgor;
mreal tmpx = node->x;
mreal tmpy = node->y;
node->x = cos(avgor)*tmpx - sin(avgor)*tmpy;
node->y = sin(avgor)*tmpx + cos(avgor)*tmpy;
if (node->state != RESULT_COORD_ROOT && node->col >= 0) {
mreal diff;
diff = fabs(gsl_vector_get(x_res, node->col + 0) +
(node->x - tmpx));
if (diff > max_diff) max_diff = diff;
diff = fabs(gsl_vector_get(x_res, node->col + 1) +
(node->y - tmpy));
if (diff > max_diff) max_diff = diff;
}
}
elog(LOG_WARNING, "MAX DIFF = %f", max_diff);
if (max_diff >= 0 && max_diff < 0.01) dobreak = 1;
/* don't break until we've completed orientation stuff */
dobreak = 0;
}
#if 0
/******************/
/* out put errors */
/******************/
/*
* This can be done in the mess above, but to keep it from getting
* too messy, we just repeat down here
*/
buf_t *resbuf = buf_new();
bufprintf(resbuf,
"#h chirper listener residual realrange realtheta realphi "
"calcrange calctheta calcphi real-calcrange real-calctheta real-calcphi "
"introdisterr introthetaerr introphierr\n");
buf_t *ares = buf_new();
bufprintf(ares, "#h c l angle res type dz\n");
for (i = 0; i < num_rows; ++i) {
mtr = get_row_range(mls, i);
if (mtr == NULL) {
elog(LOG_WARNING, "can't find range for row %d!!", i);
exit(1);
}
multilat_result_t *node1 = mtr->result;
multilat_result_t *node2 = mtr->peer->result;
/******************/
switch (get_row_type(mls, i)) {
case ML_CONSTRAINT_RANGE:
bufprintf(resbuf, "%i %i %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
mtr->chirp_from,
mtr->data_from,
gsl_vector_get(b, i),
mtr->real_distance,
mtr->real_theta,
mtr->real_phi,
sqrtf(m_pow_2(node1->x - node2->x) + m_pow_2(node1->y - node2->y)),
atan2f(node2->y - node1->y, node2->x - node1->x),
0.0,
mtr->real_distance -
sqrtf(m_pow_2(node1->x - node2->x) +
m_pow_2(node1->y - node2->y)),
mtr->real_theta -
atan2f(node2->y - node1->y, node2->x - node1->x),
0.0,
mtr->introerr_distance,
mtr->introerr_theta,
mtr->introerr_phi);
break;
case ML_CONSTRAINT_THETA:
bufprintf(ares, "%i %i %f %f theta 0\n",
mtr->chirp_from,
mtr->data_from,
r2d(mtr->theta),
gsl_vector_get(b, i));
break;
case ML_CONSTRAINT_PHI:
bufprintf(ares, "%i %i %f %f phi %f\n",
mtr->chirp_from,
mtr->data_from,
r2d(mtr->phi),
gsl_vector_get(b, i),
mtr->peer->result->z - mtr->result->z);
break;
default:
break;
}
}
char angleres[4096];
sprintf(angleres, "angleres%i", k);
multilatbuf_to_file(mls, angleres, ares);
buf_free(ares);
char errors[4096];
sprintf(errors, "errors%i", k);
multilatbuf_to_file(mls, errors, resbuf);
buf_free(resbuf);
#endif
/************************/
/* studentized residual */
/************************/
// H = X ( XT X )-1 XT.
{
studbuf = buf_new();
#if 0 /* use martin's weird but working version.. */
bufprintf(studbuf, "#h res stud real chirpfrom datafrom\n");
int l = 0;
gsl_matrix *X = gsl_matrix_calloc(num_rows, num_cols);
gsl_matrix *X2 = gsl_matrix_calloc(num_rows, num_cols);
gsl_matrix *XTrans = gsl_matrix_calloc(num_cols, num_rows);
gsl_matrix *XTrans2 = gsl_matrix_calloc(num_cols, num_rows);
gsl_matrix *XXTrans = gsl_matrix_calloc(num_cols, num_cols);
gsl_matrix *H = gsl_matrix_calloc(num_rows, num_rows);
gsl_vector *stud = gsl_vector_calloc(num_rows);
gsl_vector *eps = gsl_vector_calloc(num_rows);
double sum = 0.0;
int res = gsl_matrix_memcpy(X, A);
if (res != GSL_SUCCESS) {
elog(LOG_CRIT, "Error copying new matrix: %s\n", gsl_strerror(res));
exit(1);
}
res = gsl_matrix_transpose_memcpy(XTrans, X);
if (res != GSL_SUCCESS) {
elog(LOG_CRIT, "Error transposing new matrix: %s\n", gsl_strerror(res));
exit(1);
}
gsl_matrix_memcpy(XTrans2, XTrans);
gsl_matrix_memcpy(X2, X);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, XTrans, X, 1.0, XXTrans);
gsl_blas_dtrsm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, X2, XTrans2, 1.0, H);
/* compute residuals and the sum of squares */
sum = 0;
for (l = 0; l < num_rows; ++l) {
mreal eps_sum = 0;
int k;
for (k = 0; k < num_cols; ++k) {
eps_sum += gsl_matrix_get(A, l, k) * gsl_vector_get(x_res, k);
}
//elog(LOG_WARNING, "epssum=%f, b=%f", eps_sum, gsl_vector_get(b, l));
eps_sum -= gsl_vector_get(b, l);
gsl_vector_set(eps, l, eps_sum);
sum += sqrf(eps_sum);
}
/* compute the weighted residuals */
mreal max_stud = 0;
for (l = 0; l < num_rows; ++l) {
mreal sigma = sqrt((sum - sqrf(gsl_vector_get(eps, l))) / (mreal)(num_rows - 3));
mreal hat = sqrt(1.0 - gsl_matrix_get(H, l, l));
mreal a = fabs(gsl_vector_get(eps, l) / (sigma * hat));
gsl_vector_set(stud, l, a);
if (a > max_stud) max_stud = a;
mtr = get_row_range(mls, l);
multilat_result_t *mrr2 = mtr->result;
multilat_result_t *mrr1 = mtr->peer->result;
bufprintf(studbuf, "%f %f %f %i %i\n",
gsl_vector_get(eps,l),
gsl_vector_get(stud,l),
/* $$$ customize based on type!! */
mtr->real_distance - sqrtf(m_pow_2(mrr1->x - mrr2->x) + m_pow_2(mrr1->y - mrr2->y)),
mtr->chirp_from,
mtr->data_from
);
}
bufprintf(studbuf, "\n");
gsl_vector_free(eps);
#else
bufprintf(studbuf, "#h res stud measured chirpfrom datafrom\n");
int l = 0;
gsl_matrix *X = gsl_matrix_calloc(num_rows, 2);
gsl_matrix *X2 = gsl_matrix_calloc(num_rows, 2);
gsl_matrix *XTrans = gsl_matrix_calloc(2, num_rows);
gsl_matrix *XTrans2 = gsl_matrix_calloc(2, num_rows);
gsl_matrix *XXTrans = gsl_matrix_calloc(2, 2);
gsl_matrix *H = gsl_matrix_calloc(num_rows, num_rows);
gsl_vector *stud = gsl_vector_calloc(num_rows);
double mean = 0.0;
double var = 0.0;
for (l = 0; l < num_rows; ++l) {
mreal resid = gsl_vector_get(b, l);
mean += resid;
gsl_matrix_set(X, l, 0, 1);
gsl_matrix_set(X, l, 1, resid);
gsl_matrix_set(X2, l, 0, 1);
gsl_matrix_set(X2, l, 1, resid);
gsl_matrix_set(XTrans, 0, l, 1);
gsl_matrix_set(XTrans, 1, l, resid);
gsl_matrix_set(XTrans2, 0, l, 1);
gsl_matrix_set(XTrans2, 1, l, resid);
}
mean = mean / (num_rows + 0.0);
elog(LOG_WARNING, "mean is %f", mean);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, XTrans, X, 1.0, XXTrans);
gsl_blas_dtrsm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
// upper or lower does not matter // gsl_blas_dtrsm(CblasRight, CblasLower, CblasNoTrans, CblasNonUnit, 1.0, XXTrans, X2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, X2, XTrans2, 1.0, H);
/* calculate the variance */
for (l = 0; l < num_rows; ++l) {
var += m_pow_2(gsl_vector_get(b,l) - mean);
// elog(LOG_WARNING, "%f", gsl_matrix_get(H,l,l));
}
var = var / (num_rows + 0.0);
elog(LOG_WARNING, "var is %f", var);
mreal max_stud = 0;
for (l = 0; l < num_rows; ++l) {
double a = gsl_vector_get(b, l) / (sqrtf(var) * sqrtf(1.0 - gsl_matrix_get(H,l,l)));
// gsl_vector_set(stud, l, a);
gsl_vector_set(stud, l, a >= 0 ? a : -a);
if (gsl_vector_get(stud, l) > max_stud)
max_stud = gsl_vector_get(stud, l);
mtr = get_row_range(mls, l);
//multilat_result_t *mrr2 = mtr->result;
//multilat_result_t *mrr1 = mtr->peer->result;
if (get_row_type(mls, l) == ML_CONSTRAINT_RANGE)
bufprintf(studbuf, "%f %f %f %i %i\n",
gsl_vector_get(b,l),
gsl_vector_get(stud,l),
mtr->distance,
mtr->chirp_from,
mtr->data_from
);
}
bufprintf(studbuf, "\n");
#endif
if (dobreak && mls->studthresh != 0) {
if (runagain == 0 && max_stud > mls->studthresh) {
for (l = 0; l < num_rows; ++l) {
if (gsl_vector_get(stud, l) == max_stud) {
last_drop_value = max_stud;
/* important! */
runagain = 1;
mtr = get_row_range(mls, l);
char *type = NULL;
switch (get_row_type(mls, l)) {
case ML_CONSTRAINT_RANGE:
type = "range";
mtr->drop_r = 1; mtr->drop_t = 1; mtr->drop_p = 1;
break;
case ML_CONSTRAINT_THETA:
type = "theta";
mtr->drop_t = 1; break;
case ML_CONSTRAINT_PHI:
type = "phi";
mtr->drop_p = 1; break;
}
elog(LOG_WARNING, "dropping %s %d->%d (val is %f at row %i)",
type, mtr->chirp_from, mtr->data_from, gsl_vector_get(stud, l), l);
bufprintf(droppedbuf, "%i %i %s %f %f %i\n", mtr->chirp_from, mtr->data_from, type,
gsl_vector_get(b,l),
gsl_vector_get(stud,l),
mls->global_run
);
multilatbuf_to_file(mls, "dropped", droppedbuf);
}
}
}
gsl_matrix_free(X);
gsl_matrix_free(X2);
gsl_matrix_free(XTrans);
gsl_matrix_free(XTrans2);
gsl_matrix_free(XXTrans);
gsl_matrix_free(H);
gsl_vector_free(stud);
}
multilatbuf_to_file(mls, "compare", studbuf);
elog(LOG_WARNING, " ----- All done!");
}
mreal avg_range_resid = 0;
mreal avg_angle_resid = 0;
mreal avg_phi_resid = 0;
mreal rms_range_resid = 0;
mreal rms_angle_resid = 0;
mreal rms_phi_resid = 0;
int range_count = 0;
int theta_count = 0;
int phi_count = 0;
int l;
for (l = 0; l < num_rows; ++l) {
switch (get_row_type(mls, l)) {
case ML_CONSTRAINT_RANGE:
avg_range_resid += fabs(gsl_vector_get(b,l));
rms_range_resid += sqrf(gsl_vector_get(b,l));
range_count++;
break;
case ML_CONSTRAINT_THETA:
avg_angle_resid += fabs(gsl_vector_get(b,l));
rms_angle_resid += sqrf(gsl_vector_get(b,l));
theta_count++;
break;
case ML_CONSTRAINT_PHI:
avg_phi_resid += fabs(gsl_vector_get(b,l));
rms_phi_resid += sqrf(gsl_vector_get(b,l));
phi_count++;
break;
}
}
if (range_count > 0) {
avg_range_resid /= (float)range_count;
rms_range_resid = sqrt(rms_range_resid/(float)range_count);
}
if (theta_count > 0) {
avg_angle_resid /= (float)theta_count;
rms_angle_resid = sqrt(rms_angle_resid/(float)theta_count);
}
if (phi_count > 0) {
avg_phi_resid /= (float)phi_count;
rms_phi_resid = sqrt(rms_phi_resid/(float)phi_count);
}
elog(LOG_CRIT, "**** average residuals:");
elog(LOG_CRIT, " range: %f angle: %f phi %f",
avg_range_resid, avg_angle_resid, avg_phi_resid);
elog(LOG_CRIT, "RMS range: %f angle: %f phi %f",
rms_range_resid, rms_angle_resid, rms_phi_resid);
/**************/
/* free stuff */
/**************/
gsl_matrix_free(A);
gsl_vector_free(b);
gsl_matrix_free(new_A);
gsl_matrix_free(V);
gsl_vector_free(s);
// gsl_vector_free(work);
gsl_vector_free(x_res);
// gsl_vector_free(check);
buf_t *firstpass = buf_new();
char thefile[4096];
memset(thefile, 0, 4096);
sprintf(thefile, "pass%i", k);
result_list_print(mls, firstpass);
multilatbuf_to_file(mls, thefile, firstpass);
buf_free(firstpass);
#ifndef STANDALONE_MULTILAT
/*
* check for explosion and update the status!
*/
/* pack up the answers and push them to the main thread */
buf_t *result = buf_new();
buf_t *answer = buf_new();
multilat_result_t *node1;
for (node1 = result_list_top(mls->multilat_lists);
node1; node1 = result_list_next(node1)) {
if (node1->col < 0 && node1->state != RESULT_COORD_ROOT) {
elog(LOG_WARNING, "skipping node %d that isn't in the matrix..", node1->node);
continue;
}
if (fabs(node1->x) > 100000 || fabs(node1->y) > 100000 ||
fabs(node1->z) > 100000 || fabs(node1->yaw) > 100000 ||
!isnormal(node1->x) || !isnormal(node1->y) ||
!isnormal(node1->z) || !isnormal(node1->yaw)) {
elog(LOG_CRIT, "oops.. it blew up!");
mls->failed_convergence = 1;
buf_free(answer);
buf_free(result);
goto mlat_failed;
}
coord_entry_t ce = {
node: node1->node,
coord: { node1->x*10, node1->y*10, node1->z*10 },
rpy: { r2d(-node1->roll)*10, r2d(-node1->pitch)*10, r2d(-node1->yaw)*10 },
valid: 1
};
bufcpy(answer, &ce, sizeof(ce));
}
buf_t *errors = mlat_errors_to_buf(mls, NULL);
bufcpy(result, &answer, sizeof(answer));
bufcpy(result, &errors, sizeof(errors));
bufcpy(result, &studbuf, sizeof(studbuf));
studbuf = NULL;
g_msg_queue_push(mls->results_queue, result);
#endif
if (dobreak) {
dobreak = 0;
break;
}
//#ifdef STANDALONE_MULTILAT
dump_errors(mls, k);
//#endif
}
/* print out the final error stuff */
buf_t *ce = buf_new();
bufprintf(ce, "#h node coorderror\n");
mreal rms = 0;
int count = 0;
for (mlr = result_list_top(mls->multilat_lists);
mlr; mlr = result_list_next(mlr)) {
if (mlr->state != RESULT_COORD_ROOT) {
bufprintf(ce, "%i %f\n", mlr->node,
sqrtf(m_pow_2(mlr->x - mlr->real_x) +
m_pow_2(mlr->y - mlr->real_y) +
m_pow_2(mlr->z - mlr->real_z))
);
rms += m_pow_2(mlr->x - mlr->real_x) +
m_pow_2(mlr->y - mlr->real_y) +
m_pow_2(mlr->z - mlr->real_z);
count++;
}
}
bufprintf(ce, "# rms: %f\n", sqrtf(rms / (count + 0.0)));
bufprintf(ce, "# Grms: %f\n", mls->guess_rms);
multilatbuf_to_file(mls, "coorderr", ce);
buf_free(ce);
iterations = k;
#ifdef STANDALONE_MULTILAT
analyse_result(mls);
#endif
/*************/
/* increment */
/*************/
if (runagain) {
total_passes++;
mls->global_run++;
runagain = 0;
goto repeate;
}
multilatbuf_to_file(mls, "dropped", droppedbuf);
buf_free(droppedbuf);
mls->global_run = 1000;
{
buf_t *firstpass = buf_new();
char thefile[4096];
memset(thefile, 0, 4096);
sprintf(thefile, "last");
result_list_print(mls, firstpass);
multilatbuf_to_file(mls, thefile, firstpass);
buf_free(firstpass);
}
#ifdef STANDALONE_MULTILAT
analyse_result(mls);
#endif
mls->failed_convergence = 2;
LOG_ENTRY("****multilat success, %d, %d", total_iterations, total_passes);
return 0;
mlat_failed:
LOG_ENTRY("****multilat failed, %d, %d", total_iterations, total_passes);
#ifdef STANDALONE_MULTILAT
exit(1);
#else
return -1;
#endif
}
#endif
void *multilat_thread_main(void *arg) {
ml_state_t *mls = (ml_state_t *) arg;
/* nice down */
nice(10);
do_multilat:
#ifndef STANDALONE_MULTILAT
/* wait for new data to appear */
pthread_mutex_lock(&(mls->range_snapshot_mutex));
while (!mls->master_mode || mls->latest_table == NULL) {
pthread_cond_wait(&(mls->range_snapshot_cond),
&(mls->range_snapshot_mutex));
}
int i;
multilat_range_t *mlr;
/* clear the last range set */
while ((mlr = range_list_pop(mls->multilat_lists))) {
free(mlr);
}
coord_guess_t *cgt;
while ((cgt = guess_list_pop(mls->multilat_lists))) {
free(cgt);
}
multilat_result_t *mrt;
while ((mrt = result_list_pop(mls->multilat_lists))) {
free(mrt);
}
mls->global_run = 0;
/* fill the new range set */
for (i = 0; i < mls->latest_table_count; ++i) {
mlr = g_new0(multilat_range_t, 1);
mlr->chirp_from = mls->latest_table[i].range_entry.source;
mlr->data_from = mls->latest_table[i].header.flow_id.src;
if (mlr->chirp_from == mlr->data_from ||
mlr->chirp_from == 0 || mlr->data_from == 0)
goto free_it;
/* from tenths of degrees to radians */
mlr->theta = (M_PI * (float) mls->latest_table[i].range_entry.theta) / 1800.0;
mlr->phi = (M_PI * (float) mls->latest_table[i].range_entry.phi) / 1800.0;
mlr->distance = mls->latest_table[i].range_entry.distance / 10.0;
if (mlr->distance == 0)
goto free_it;
#ifdef EMBEDDED_USECONF
mlr->angle_conf = mls->latest_table[i].range_entry.a_conf / 10.0;
mlr->conf = mls->latest_table[i].range_entry.conf / 10.0;
#else
mlr->angle_conf = 1;
mlr->conf = 10;
#endif
elog(LOG_WARNING, "Processing %d->%d, r=%f (cm) t=%f (%f) p=%f",
mlr->chirp_from, mlr->data_from, mlr->distance, mlr->theta, r2d(mlr->theta), mlr->phi);
range_list_push(mls->multilat_lists, mlr);
continue;
free_it:
free(mlr);
}
free(mls->latest_table);
mls->latest_table = NULL;
mls->latest_table_count = 0;
pthread_mutex_unlock(&(mls->range_snapshot_mutex));
#endif
elog(LOG_WARNING, "Other thread got new data, %d entries", range_list_qlen(mls->multilat_lists));
update_result_list(mls); /* add new nodes to the result list */
/* dump the current range table */
multilat_range_t *tmp;
elog(LOG_WARNING, "DUMPING RANGE TABLE:");
elog(LOG_WARNING, "src dst distance theta phi aconf conf state ur dr dt dp ur ut up us:");
for (tmp = range_list_top(mls->multilat_lists); tmp; tmp=range_list_next(tmp)) {
elog(LOG_WARNING, "%d %d %.1f %.2f %.2f %.1f %.1f %d %.1f %d %d %d %d %d %d %d",
tmp->chirp_from, tmp->data_from, tmp->distance, tmp->theta, tmp->phi,
tmp->angle_conf, tmp->conf, tmp->state, tmp->userange,
tmp->drop_r, tmp->drop_t, tmp->drop_p,
tmp->use_r, tmp->use_t, tmp->use_p, tmp->use_set);
}
initialize_guess_stage(mls);
if (!guess_coords(mls)) {
goto do_multilat;
}
buf_t *guessx = buf_new();
result_list_print(mls, guessx);
multilatbuf_to_file(mls, "step1", guessx);
buf_free(guessx);
#ifdef STANDALONE_MULTILAT
if (mls->addfuzz) {
add_fuzz_to_results(mls);
}
#endif
guessx = buf_new();
result_list_print(mls, guessx);
multilatbuf_to_file(mls, "step2", guessx);
buf_free(guessx);
#ifdef STANDALONE_MULTILAT
multilat_result_t *res = result_list_top(mls->multilat_lists);
mreal rms = 0;
int count = 0;
for ( ; res != NULL; res = result_list_next(res) ) {
if (res->state != RESULT_COORD_ROOT) {
rms += m_pow_2(res->x - res->real_x) +
m_pow_2(res->y - res->real_y) +
m_pow_2(res->z - res->real_z);
count++;
}
}
mls->guess_rms = sqrtf(rms / (count + 0.0));
#endif
/* note counts of nodes and ranges */
mls->result_element_count = result_list_qlen(mls->multilat_lists);
mls->range_element_count = range_list_qlen(mls->multilat_lists);
result_save(mls);
multilat_solver(mls);
#ifdef STANDALONE_MULTILAT
exit(1);
#endif
goto do_multilat;
}
void multilat_thread_init(ml_state_t *mls) {
/* setup mutex and cond */
pthread_mutex_init(&(mls->range_snapshot_mutex), NULL);
pthread_cond_init(&(mls->range_snapshot_cond), NULL);
/* spawn thread */
pthread_attr_t thread_attr;
pthread_attr_init(&thread_attr);
if (pthread_create(&(mls->multilat_thread), &thread_attr, multilat_thread_main, mls) < 0) {
elog(LOG_CRIT, "Failed to spawn compute thread: %m");
exit(1);
}
}
#ifdef STANDALONE_MULTILAT
int killpairs[3][20];
int killpairs_count = 0;
char *killpairs_file = NULL;
void read_killpairs()
{
FILE *f = fopen(killpairs_file, "r");
while (2 == fscanf(f, "%d %d", &(killpairs[0][killpairs_count]),
&(killpairs[1][killpairs_count]))) {
printf("read killpair %d %d\n",
killpairs[0][killpairs_count],
killpairs[1][killpairs_count]);
killpairs[2][killpairs_count] = (random()%100 < killpairs_prob*100);
killpairs_count++;
}
fclose(f);
}
/*
* for median finding
*/
struct sorter {
int index;
float value;
};
int sorter_cmp(const void *x, const void *y) {
return ((struct sorter *)x)->value - ((struct sorter *)y)->value;
}
int read_in_file(ml_state_t *mls)
{
if ((mls->fakedata = fopen(mls->filename, "r")) == NULL) {
elog(LOG_CRIT, "Unable to open file %s", mls->filename);
exit(1);
}
int res = 0;
int chirper = 0;
int listener = 0;
float distance = 0;
float theta = 0;
float phi = 0;
float temp = 0;
float real_distance = 0;
float real_theta = 0;
float real_phi = 0;
float real_x = 0;
float real_y = 0;
float real_z = 0;
float real_yaw = 0;
float conf = 10;
float aconf = 1;
char input[8192];
memset(&input, 0, 8192);
while (1) {
if (fgets(input,8192,mls->fakedata) == NULL) {
break;
}
if (input[0] == '#') {
continue;
}
if (mls->include_conf) {
res = sscanf(input, "%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
&chirper, &listener, &distance, &theta, &phi,
&real_x, &real_y, &real_z, &temp, &temp, &temp, &temp,
&temp, &real_distance, &real_theta, &real_phi,
&conf, &aconf);
} else {
res = sscanf(input, "%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
&chirper, &listener, &distance, &theta, &phi,
&real_x, &real_y, &real_z, &temp, &temp, &temp, &temp,
&temp, &real_distance, &real_theta, &real_phi);
}
if (res == EOF) {
elog(LOG_WARNING, "End of input file %s", mls->filename);
break;
}
if (res != (mls->include_conf ? 18:16)) {
elog(LOG_WARNING, "error in file %s; status was %d", mls->filename, res);
continue;
}
multilat_range_t *mlr = g_new0(multilat_range_t, 1);
mlr->chirp_from = chirper;
mlr->data_from = listener;
if (mls->input_in_degrees) {
mlr->theta = theta/180.0*M_PI;
mlr->phi = phi/180.0*M_PI;
}
else {
mlr->theta = theta;
mlr->phi = phi;
}
mlr->distance = distance;
mlr->real_distance = real_distance;
mlr->real_theta = real_theta;
mlr->real_phi = real_phi;
mlr->introerr_distance = mlr->real_distance - mlr->distance;
mlr->introerr_theta = mlr->real_theta - mlr->theta;
mlr->introerr_phi = mlr->real_phi - mlr->phi;
mlr->conf = conf;
mlr->angle_conf = aconf;
int ll;
for (ll=0; ll<killpairs_count; ll++) {
if ((chirper == killpairs[0][ll] &&
listener == killpairs[1][ll]) ||
(chirper == killpairs[1][ll] &&
listener == killpairs[0][ll])) {
if (killpairs[2][ll]) {
/*
* used for simulating reflections.\
* you can set a list of pairs to remove in a file you
* pass in. then you can also specify specific
* ranges to cause to reflect.
* reflections add 10m and may or may not add a 45 degree
* angular error.
*/
int disterr = 1000;
int angleerr = 0 * (chirper == killpairs[1][ll] ? +1 : -1);
printf("offsetting %d,%d by %d cm, %d deg\n",
chirper, listener, disterr, angleerr);
mlr->distance += disterr;
mlr->theta += angleerr*M_PI/180.0;
}
else {
printf("nuking %d,%d\n",
chirper, listener);
mlr->conf = 0;
}
break;
}
}
if (mlr->conf == 0 || mlr->distance == 0) {
elog(LOG_WARNING, "skipping 0 range %d->%d",
mlr->chirp_from, mlr->data_from);
free(mlr);
goto skip;
}
range_list_push(mls->multilat_lists, mlr);
result_list_initnode(mls, listener);
result_list_initnode(mls, chirper);
multilat_result_t *mtr = result_list_get(mls, chirper);
mtr->real_x = real_x;
mtr->real_y = real_y;
mtr->real_z = real_z;
//mtr->nogt = 0; // get it from the map file!!
// update_result_list(mls); /* add new nodes to the result list */
skip:
elog(LOG_DEBUG(0), "read entry: %d %d %f %f %f", chirper, listener, theta, phi, distance);
}
fclose(mls->fakedata);
if (mls->gt_filename) {
FILE *f = fopen(mls->gt_filename, "r");
if (f == NULL) {
elog(LOG_WARNING, "unable to read gt from file %s", mls->gt_filename);
exit(1);
}
while (1) {
if (fgets(input,8192,f) == NULL) {
break;
}
if (input[0] == '#') {
continue;
}
int node;
res = sscanf(input, "%d %f %f %f %f",
&node, &real_x, &real_y, &real_z, &real_yaw);
if (res == EOF) {
elog(LOG_WARNING, "End of input file %s", mls->gt_filename);
break;
}
if (res != 5) {
elog(LOG_WARNING, "error in file %s", mls->filename);
continue;
}
result_list_initnode(mls, node);
multilat_result_t *mtr = result_list_get(mls, node);
mtr->real_x = real_x*100.0;
mtr->real_y = real_y*100.0;
mtr->real_z = real_z*100.0;
mtr->real_yaw = real_yaw;
mtr->nogt = 0;
elog(LOG_WARNING, "read %d %f %f %f",
node, real_x, real_y, real_z);
}
fclose(f);
}
/* prune duplicates, keep median phi angle */
multilat_range_t *ptr;
multilat_range_t *tmp;
for (ptr = range_list_top(mls->multilat_lists); ptr; ptr = tmp) {
multilat_range_t *ptr2;
multilat_range_t *tmp2;
multilat_stages_t lists;
memset(&lists, 0, sizeof(lists));
/* move dups into list */
for (ptr2 = range_list_next(ptr); ptr2; ptr2 = tmp2) {
tmp2 = range_list_next(ptr2);
if (ptr->chirp_from == ptr2->chirp_from &&
ptr->data_from == ptr2->data_from) {
range_list_remove(mls->multilat_lists, ptr2);
range_list_push(&lists, ptr2);
}
}
/* inc and move this one into list */
tmp = range_list_next(ptr);
range_list_remove(mls->multilat_lists, ptr);
range_list_push(&lists, ptr);
int r_count = range_list_qlen(&lists);
struct sorter s[r_count];
/* now process list */
int i;
int q[5] = {0,0,0,0,0};
for (i=0, ptr2 = range_list_top(&lists); ptr2; i++, ptr2 = range_list_next(ptr2)) {
s[i].index = i;
s[i].value = ptr2->theta;
if (s[i].value < 0)
s[i].value += (M_PI*2.0);
q[(int)(s[i].value/(M_PI/2.0))]++;
}
q[3] += q[4];
q[4] = 0;
int maxq;
int maxind=-1;
for (i=0; i<4; i++) {
int thisq = q[i] + q[(i+1)%4];
if (maxind < 0 || thisq > maxq) {
maxind = i;
maxq = thisq;
}
}
/* flip? */
if (maxind == 3) {
for (i=0; i<r_count; i++)
if (s[i].value > M_PI/2.0 && s[i].value < 3.0*M_PI/2.0)
s[i].index = -1;
else
if (s[i].value > M_PI)
s[i].value -= (2.0*M_PI);
}
else {
for (i=0; i<r_count; i++) {
int q_num = ((int)(s[i].value / (M_PI/2.0)));
if (q_num == 4) q_num = 3;
if (q_num != maxind && q_num != maxind+1)
s[i].index = -1;
}
}
/* filter stuff out of 2 quadrants */
int drops=0;
for (i=0; i<r_count; i++) {
if (s[i].index < 0) {
elog(LOG_WARNING, "dropping outlier value %f, for pair %d->%d",
s[i].value, ptr->chirp_from, ptr->data_from);
drops++;
}
else if (drops > 0) {
s[i-drops] = s[i];
}
}
r_count -= drops;
/* sort and keep median */
if (r_count > 0) {
qsort(&s, r_count, sizeof(struct sorter), sorter_cmp);
/* keep median */
int med_idx = s[r_count/2].index;
for (i=0, ptr2=range_list_top(&lists); i<med_idx;
i++, ptr2=range_list_next(ptr2));
range_list_remove(&lists, ptr2);
range_list_push_front(mls->multilat_lists, ptr2);
elog(LOG_WARNING, "keeping median: %d->%d %f %f %f",
ptr2->chirp_from, ptr2->data_from, ptr2->theta,
ptr2->phi, ptr2->distance);
}
/* free dups */
while ((ptr2 = range_list_pop(&lists))) free(ptr2);
}
return 0;
}
#define OUTPUTPREFIX "/tmp/"
int main(int argc, char **argv) {
ml_state_t mls = {
master_mode: 1
};
misc_init(&argc, argv, CVSTAG);
char *tempstr1;
char *tempstr2;
float studthresh = 0;
mls.filename = misc_parse_out_option(&argc, argv, "file", 'f');
if (mls.filename == NULL || strcmp(mls.filename, "") == 0) {
elog(LOG_CRIT, "Please input valid filename!");
exit(1);
}
char *kp;
if ((kp = misc_parse_out_option(&argc, argv, "kp", 0))) {
sscanf(kp, "%d,%d", &killpairs[0][0], &killpairs[1][0]);
killpairs[2][0] = 1;
killpairs_count = 1;
}
misc_parse_option_as_float(&argc, argv, "kprob", 0, &killpairs_prob);
killpairs_file = misc_parse_out_option(&argc, argv, "killpairs", 0);
if (killpairs_file)
read_killpairs(killpairs_file);
mls.gt_filename = misc_parse_out_option(&argc, argv, "gt", 0);
mls.outputprefix = misc_parse_out_option(&argc, argv, "outputprefix", 'p');
if (mls.outputprefix == NULL || strcmp(mls.outputprefix, "") == 0) {
elog (LOG_CRIT, "no output prefix set, setting to /tmp");
mls.outputprefix = OUTPUTPREFIX;
}
mls.noreject = misc_parse_out_switch(&argc, argv, "no_reject", 0);
mls.addfuzz = misc_parse_out_switch(&argc, argv, "fuzz", 'z');
mls.useangles = misc_parse_out_switch(&argc, argv, "useangle", 'a');
mls.input_in_degrees = misc_parse_out_switch(&argc, argv, "degrees", 0);
mls.include_conf = misc_parse_out_switch(&argc, argv, "useconf", 0);
if (misc_parse_option_as_float(&argc, argv, "studres", 's', &studthresh) != 0) {
studthresh = 0;
}
mls.studthresh = studthresh;
tempstr1 = misc_parse_out_option(&argc, argv, "one", '1');
if (tempstr1 != NULL) {
sscanf(tempstr1, "%i:%lf,%lf,%lf,%lf",
&mls.anode1, &mls.x1, &mls.y1, &mls.z1, &mls.a1);
mls.a1 = mls.a1/180.0*M_PI;
elog(LOG_WARNING, "fixed node %i at %f %f %f, angle %f",
mls.anode1, mls.x1, mls.y1, mls.z1, mls.a1);
}
tempstr2 = misc_parse_out_option(&argc, argv, "two", '2');
if (tempstr2 != NULL) {
sscanf(tempstr2, "%i:%lf,%lf,%lf,%lf",
&mls.anode2, &mls.x2, &mls.y2, &mls.z2,&mls.a2);
mls.a2 = mls.a2/180.0*M_PI;
elog(LOG_WARNING, "fixed node %i at %f %f %f angle %f",
mls.anode2, mls.x2, mls.y2, mls.z2, mls.a2);
}
mls.multilat_lists = g_new0(multilat_stages_t, 1);
range_list_init(mls.multilat_lists);
guess_list_init(mls.multilat_lists);
result_list_init(mls.multilat_lists);
read_in_file(&mls);
multilat_thread_main(&mls);
exit(1);
}
#endif
See more files for this project here