laser-nav.c

/*
 * @(#)lasernav.c       07/03/21
 * 
 * License      : GNU GPL v.2
 * Description  : Robot navigation using 3 passive laser reflectors.
 *                (pln - Passive Laser Navigaton)
 * Author       : Tran Duy Khanh (www.tran.cz)
 */

#include <math.h>
#include <robomath.h>
#include "laser-nav.h"
        
// /**
//  * Gaussian probability density function.
//  *
//  * @param  val               value we are looking for
//  * @param  sigma     standard deviation
//  */
// double evaluate_gaussian(double val, double sigma)
// {
//      return ((double)1.0/(sqrt((double)2.0*(double)M_PI) * sigma) *
//              exp((double)(-0.5) * (val*val / (sigma*sigma))));
// }

void time2angle(struct pln_time_state ts, struct pln_aangle_state *aas)
{
        aas->theta[0] = TIME2ANGLE(ts.period, ts.t_theta[0]);
        aas->theta[1] = TIME2ANGLE(ts.period, ts.t_theta[1]);
        aas->theta[2] = TIME2ANGLE(ts.period, ts.t_theta[2]);
}

void aangle2angle(struct pln_aangle_state aas, struct pln_angle_state *as)
{
        as->beta = aas.theta[1] - aas.theta[0];
        as->gama = aas.theta[2] - aas.theta[1];
        as->alfa = 2*M_PI - as->beta - as->gama;
}

void pln_get_line(struct pln_point a, struct pln_point b,
                        struct pln_line *line)
{
        line->k = (b.y - a.y)/(b.x - a.x);
        line->q = a.y - line->k * a.x;
}

void pln_get_oline(struct pln_line line, struct pln_point c,
                        struct pln_line *oline)
{
        oline->k = -1 / line.k;
        oline->q = c.y - oline->k * c.x;
}

void pln_solve_quad(struct pln_fquad quad, 
                        struct pln_compl *sol1, struct pln_compl *sol2)
{
        double tmp;

        /* normalize */
        /*tmp = 1/quad.a;
        quad.a *= tmp;
        quad.b *= tmp;
        quad.c *= tmp;*/

        tmp = POWER(quad.b) - 4*quad.a*quad.c;
        /* solution is a complex variable */
        if (tmp < 0) {
                tmp = fabs(tmp);
                sol1->real = (-quad.b)/(2*quad.a);
                sol1->imag = -(sqrt(tmp)/(2*quad.a));
                sol2->real = (-quad.b)/(2*quad.a);
                sol2->imag = sqrt(tmp)/(2*quad.a);
        /* solution is a real variable */
        } else {
                sol1->real = (-quad.b-sqrt(tmp))/(2*quad.a);
                sol1->imag = 0;
                sol2->real = (-quad.b+sqrt(tmp))/(2*quad.a);
                sol2->imag = 0;
        }

#if DEBUG>=2
        printf("pln_solve_quad(): x1.real=%f x1.imag=%f x2.real=%f x2.imag=%f\n", 
                sol1->real, sol1->imag, sol2->real, sol2->imag);
#endif
}

void pln_solve_2quad(struct pln_fquad_2var form1, 
                        struct pln_fquad_2var form2, 
                        struct pln_point *sol1, struct pln_point *sol2)
{
        double tmp;
        struct pln_fquad_2var form;
        struct pln_fquad quad;
        struct pln_compl y1, y2;

        /* 1) merge equations */
        tmp = (-form1.ax)/form2.ax;
        form.ax = form1.ax + tmp*form2.ax;
        form.bx = form1.bx + tmp*form2.bx;
        form.ay = form1.ay + tmp*form2.ay;
        form.by = form1.by + tmp*form2.by;
        form.rs = form1.rs + tmp*form2.rs;

#if DEBUG>=3
        printf("pln_solve_2quad(): %f*X2 %f*X %f*Y2 %f*Y  = %f\n",
                        form.ax, form.bx,
                        form.ay, form.by, form.rs);
#endif

        /* 2) normalize */
        tmp = 1/form.bx;
        form.bx *= tmp;
        form.by *= tmp;
        form.rs *= tmp;

#if DEBUG>=3
        printf("pln_solve_2quad(): %f*X %f*Y = %f\n", 
                        form.bx, form.by, form.rs);
#endif
        
        /* 3) substitution for X - we get a quadratic equation */
        quad.a = POWER(-form.by)*form1.ax + form1.ay;
        quad.b = 2*(-form.by)*form.rs + form1.bx*(-form.by) + form1.by;
        quad.c = POWER(form.rs) + form1.bx*form.rs - form1.rs;

#if DEBUG>=3
        printf("pln_solve_2quad(): a=%f b=%f c=%f\n", 
                        quad.a, quad.b, quad.c);
#endif

        /* 4) solve the quadratic equation - find Y */
        pln_solve_quad(quad, &y1, &y2);
        
        /* 5) find X, the final results */
        sol1->y = y1.real;
        sol1->x = (-form.by)*y1.real + form.rs;
        sol2->y = y2.real;
        sol2->x = (-form.by)*y2.real + form.rs;
}

void pln_circ_line_cross(struct pln_circle circle, struct pln_line line,
                        struct pln_point *sol1, struct pln_point *sol2)
{
        struct pln_compl x1, x2;
        struct pln_fquad quad;

        /* solve quadratic equation - find X */
        quad.a = POWER(line.k) + 1.0;
        quad.b = 0;
        quad.c = -POWER(circle.radius);
        pln_solve_quad(quad, &x1, &x2);
        /* count Y */
        sol1->x = x1.real;
        sol1->y = x1.real * line.k;
        sol2->x = x2.real;
        sol2->y = x2.real * line.k;

#if DEBUG>=3
        printf("pln_circ_line_cross(): [x1=%f,y1=%f] [x2=%f,y2=%f]\n", 
                sol1->x, sol1->y, sol2->x, sol2->y);
#endif
}

void pln_get_circle2(struct pln_circ c, struct pln_circle *circle)
{
        double opposite;
        int sgn = -1;

        /* get opposite side to the known angle */
        opposite = sqrt(POWER(c.pl.x-c.pr.x) + 
                        POWER(c.pl.y-c.pr.y)) / 2;
        /* if the angle is greater than PI/2, we have to use double angle, 
         * because the center of the circle will be outside */
        if (c.angle > M_PI/2) {
                c.angle = (2*M_PI-c.angle*2)/2;
                sgn = 1;
        }

        /* get circle center */
        if (opposite == FWIDTH/2) {
                circle->x0 = opposite;
                circle->y0 = FHEIGHT + 
                        sgn*(opposite/tan(c.angle));
        } else {
                circle->y0 = opposite;
                circle->x0 = FWIDTH + 
                        sgn*(opposite/tan(c.angle));
        }
        /* circle radius */
        circle->radius = opposite/cos(M_PI/2-c.angle);
}

void pln_get_circle(struct pln_circ c, struct pln_circle *circle)
{
        struct pln_line line, oline;
        struct pln_circle centercirc;
        struct pln_point center1, center2;
        double opposite, cdist, angle;
        int sgnx=1, sgny=1;

        /* opposite side of the known angle */
        opposite = sqrt(POWER(c.pl.x-c.pr.x) + 
                        POWER(c.pl.y-c.pr.y)) / 2;
        /* if the angle is greater than PI/2, we have to use double angle, 
         * because the center of the circle will be outside */
        angle = c.angle;
        if (c.angle > M_PI/2)
                c.angle = (2*M_PI-c.angle*2)/2;

        /* distance from center point to center of the circle */
        cdist = opposite/tan(c.angle);
        /* get line parameters from 2 points */
        pln_get_line(c.pl, c.pr, &line);
        /* get ortogonal line to a line if we know the cross point */
        pln_get_oline(line, c.pc, &oline);
        /* we put center to point c.pc, so dont care about the y-intercept */
        oline.q = 0;

        /* we know the distance from known point to center of our circle, so
         * try to find the center, which are the cross points of the line and
         * circle with radius equal to our distance */
        centercirc.x0 = c.pc.x;
        centercirc.y0 = c.pc.y;
        centercirc.radius = cdist;
        pln_circ_line_cross(centercirc, oline, &center1, &center2);

        /* ortogonal side C-L-B */
        if (isinf(line.k)) {
                if (angle < M_PI/2) {
                        sgnx = -1;
                        sgny = -1;
                } else {
                        sgnx = 1;
                        sgny = 1;
                }
        /* side K-H-C */
        } else if (oline.k < 0) {
                if ((angle>M_PI/2 && angle<M_PI) || (angle>3*M_PI/2 && angle<2*M_PI)) {
                        sgnx = -1;
                        sgny = 1;
                } else {
                        sgnx = 1;
                        sgny = -1;
                }
        /* side K-G-B */
        } else {
                if ((angle>M_PI/2 && angle<M_PI) || (angle>3*M_PI/2 && angle<2*M_PI)) {
                        sgnx = -1;
                        sgny = -1;
                }
        }
#ifdef USE_RED_REFLECTORS
        sgnx *= -1;
        sgny *= -1;
#endif
        /* center of the circle */
        circle->x0 = centercirc.x0 + sgnx*fabs(center1.x);
        circle->y0 = centercirc.y0 + sgny*fabs(center1.y);
        /* circle radius */
        circle->radius = fabs(opposite/cos(M_PI/2-c.angle));
}

void pln_circ2form(struct pln_circle circle, struct pln_fquad_2var *form)
{
        form->ax = 1;
        form->bx = 2 * -circle.x0;
        form->cx = POWER(circle.x0);
        form->ay = 1;
        form->by = 2 * -circle.y0;
        form->cy = POWER(circle.y0);
        form->rs = POWER(circle.radius) - (form->cx + form->cy);
#if DEBUG>=3
        printf("pln_circ2form(): %f*X2 %f*X (%f) %f*Y2 %f*Y (%f) = %f\n",
                        form->ax, form->bx, form->cx, 
                        form->ay, form->by, form->cy, form->rs);
#endif
}

void pln_get_position(struct pln_angle_state as, struct pln_pos_state *pos)
{
        struct pln_circ cir1, cir2;
        struct pln_circle circle1, circle2;
        struct pln_fquad_2var form1, form2;
        struct pln_point sol1, sol2, cp;
        double dist1, dist2;

        /* get circle from 1 known angles and 3 known points */
        cir1.pl = R3;
        cir1.pc = L;
        cir1.pr = R2;
        cir1.angle = as.alfa;

        /*cir1.pl = K;
        cir1.pc = H;
        cir1.pr = C;
        cir1.angle = as.beta;*/

        /* if the angle is close to PI, there will be many uncertainties so
         * use the second side to correct this error */
#ifdef USE_RED_REFLECTORS
        if (evaluate_gaussian(as.gama-M_PI, 0.4) < 0.8) {
#else
        if (evaluate_gaussian(as.beta-M_PI, 0.4) < 0.8) {
#endif
                cir2.pl = R1;
                cir2.pc = H;
                cir2.pr = R3;
                cp = R3;
#ifdef USE_RED_REFLECTORS
                cir2.angle = as.gama;
#else
                cir2.angle = as.beta;
#endif
        } else {
                cir2.pl = R1;
                cir2.pc = G;
                cir2.pr = R2;
                cp = R2;
#ifdef USE_RED_REFLECTORS
                cir2.angle = as.beta;
#else
                cir2.angle = as.gama;
#endif
        }

        /* collective point */
        /*cp = C;
        cp = B;
        cp = K;*/

        /* get circles, those cross points intersect the position we are
         * looking for */
        pln_get_circle(cir1, &circle1);
        pln_get_circle(cir2, &circle2);

#if DEBUG>=3
        printf("pln_get_position(): circle 1: angle=%f "
                        "center[%f,%f] radius=%f\n",
                        DEGREES(cir1.angle), circle1.x0, 
                        circle1.y0, circle1.radius);
        printf("pln_get_position(): circle 2: angle=%f "
                        "center[%f,%f] radius=%f\n",
                        DEGREES(cir2.angle), circle2.x0, 
                        circle2.y0, circle2.radius);
#endif

        /* get quadratic formular from the circle parameters */
        pln_circ2form(circle1, &form1);
        pln_circ2form(circle2, &form2);
        /* solve the equation */
        pln_solve_2quad(form1, form2, &sol1, &sol2);

#if DEBUG>=3
        printf("pln_get_position(): [x1=%f;y1=%f] [x2=%f;y2=%f]\n", 
                        sol1.x, sol1.y, sol2.x, sol2.y);
#endif

        /* evaluate possible cross points and eliminate the known point from
         * two solutions */
        dist1 = fabs(sqrt(POWER(sol1.x-cp.x) + POWER(sol1.y-cp.y)));
        dist2 = fabs(sqrt(POWER(sol2.x-cp.x) + POWER(sol2.y-cp.y)));

        if (evaluate_gaussian(dist1, 0.4) > 0.5) {
                pos->x = lround(sol2.x);
                pos->y = lround(sol2.y);
        } else {
                pos->x = lround(sol1.x);
                pos->y = lround(sol1.y);
        }
}

void pln_set_points()
{
        /* fixed known points */
        SET_POINT_A(A);
        SET_POINT_B(B);
        SET_POINT_C(C);
        SET_POINT_E(E);
        SET_POINT_F(F);
        SET_POINT_G(G);
        SET_POINT_H(H);
        SET_POINT_K(K);
        SET_POINT_L(L);
        SET_POINT_R1(R1);
        SET_POINT_R2(R2);
        SET_POINT_R3(R3);

#if DEBUG>=3
        printf("pln_set_points():\n");
        printf("A.x = %f; A.y = %f\n", A.x, A.y);
        printf("B.x = %f; B.y = %f\n", B.x, B.y);
        printf("C.x = %f; C.y = %f\n", C.x, C.y);
        printf("E.x = %f; E.y = %f\n", E.x, E.y);
        printf("F.x = %f; F.y = %f\n", F.x, F.y);
        printf("G.x = %f; G.y = %f\n", G.x, G.y);
        printf("H.x = %f; H.y = %f\n", H.x, H.y);
        printf("K.x = %f; K.y = %f\n", K.x, K.y);
        printf("L.x = %f; L.y = %f\n", L.x, L.y);
        printf("R1.x = %f; R1.y = %f\n", R1.x, R1.y);
        printf("R2.x = %f; R2.y = %f\n", R2.x, R2.y);
        printf("R3.x = %f; R3.y = %f\n", R3.x, R3.y);
#endif
}

int cmpi(const void *a, const void *b)
{
        return ((int)*(int*)a > (int)*(int*)b);
}

int cmpd(const void *a, const void *b)
{
        return ((double)*(double*)a > (double)*(double*)b);
}

int cmpang(const void *a, const void *b)
{
        return ((double)(((struct pln_angle_eval *)a)->eval) <
                        (double)(((struct pln_angle_eval *)b)->eval));
}

void pln_pos2ang(struct pln_pos_state ps, struct pln_aangle_state *aas)
{
        double a1, a2, a3, angles[3];

#ifdef USE_RED_REFLECTORS
        ps.head += M_PI;
#endif
        /* angles between 0 and reflectors (with center at robot position) */
        a1 = atan2(R1.y - ps.y, R1.x - ps.x);
        a2 = atan2(R2.y - ps.y, R2.x - ps.x);
        a3 = atan2(R3.y - ps.y, R3.x - ps.x);

        a1 = (a1 < 0) ? 2*M_PI + a1 : a1;
        a2 = (a2 < 0) ? 2*M_PI + a2 : a2;
        a3 = (a3 < 0) ? 2*M_PI + a3 : a3;
        ps.head = (ps.head < 0) ? 2*M_PI + ps.head : ps.head;

        /* offset to the head angle */
        angles[0] = (a1-ps.head < 0) ? 2*M_PI+a1-ps.head : a1-ps.head;
        angles[1] = (a2-ps.head < 0) ? 2*M_PI+a2-ps.head : a2-ps.head;
        angles[2] = (a3-ps.head < 0) ? 2*M_PI+a3-ps.head : a3-ps.head;

        /* sort and store results */
        qsort(angles, 3, sizeof(double), cmpd);
        aas->theta[0] = angles[0];
        aas->theta[1] = angles[1];
        aas->theta[2] = angles[2];
        aas->head = ps.head;
}

void test_angles(struct pln_point *positions, int *poscount)
{
        struct pln_pos_state ps, cpos;
        struct pln_aangle_state aas;
        struct pln_angle_state as;
        int i, j;

        /* set fixed points */
        pln_set_points();

        /* range of position */
        for (j=1; j<FHEIGHT-1; j+=10) {
                for (i=1; i<FWIDTH-1; i+=10) {
                        /* convert the position to angles */
                        ps.x = i;
                        ps.y = j;

                        ps.head = DEG2RAD(0);
                        pln_pos2ang(ps, &aas);

                        /*printf("theta[0]=%f theta[1]=%f theta[2]=%f\n", 
                                DEGREES(aas.theta[0]), DEGREES(aas.theta[1]), 
                                DEGREES(aas.theta[2]));*/

                        as.beta = aas.theta[1] - aas.theta[0];
                        as.gama = aas.theta[2] - aas.theta[1];
                        as.alfa = 2*M_PI - as.beta - as.gama;
                        /*printf("alfa=%f; beta=%f; gama=%f\n", 
                                DEGREES(as.alfa), DEGREES(as.beta), 
                                DEGREES(as.gama));*/

                        /* get position from measured angles */
                        pln_get_position(as, &cpos);
                        /*printf("robot=[x=%d;y=%d]\n", 
                                        (int)cpos.x, (int)cpos.y);*/

                        /*if (cpos.x != ps.x || cpos.y != ps.y) {*/
                        if (cpos.x == ps.x && cpos.y == ps.y) {
                                /*printf("[ %f ; %f ] => [ %f ; %f ]\n\n", 
                                ps.x, ps.y, cpos.x, cpos.y);*/
                                positions[(*poscount)].x = ps.x;
                                positions[(*poscount)].y = ps.y;
                                (*poscount)++;
                        }
                }
        }
        printf("\n");
}

void pln_coordinate(struct pln_pos_state estpos, struct pln_aangle_state aas, 
                        struct pln_angle_state *as)
{
        struct pln_aangle_state aas0;
        struct pln_angle_state _as0, _as;
        double head;
        double _angles[3], __angles[3];;
        struct pln_angle_eval angeval[3];
        int i, j;

        /* convert the estimated position to angles with head=0. Angles are 
         * absolute from robot`s head */
        head = estpos.head;
        estpos.head = DEG2RAD(0);
        pln_pos2ang(estpos, &aas0);
        estpos.head = head;

        /* count angles between reflectors */
        aangle2angle(aas0, &_as0);
        aangle2angle(aas, &_as);

        /* temporary variables */
        _angles[0] = _as0.alfa;
        _angles[1] = _as0.beta;
        _angles[2] = _as0.gama;
        angeval[0].angle = _as.alfa;
        angeval[1].angle = _as.beta;
        angeval[2].angle = _as.gama;

        /* evaluation, select angles depending to it`s probability. Decrease
         * sigma value of gaussian evaluation function to decrease the 
         * accepted range */
        for (j=0; j<3; j++) {
                for (i=0; i<3; i++) {
                        angeval[i].eval = evaluate_gaussian(
                                angeval[i].angle - _angles[j], 0.1);
                }
                /* sort evaluated angles */
                qsort(angeval, 3, sizeof(struct pln_angle_eval), cmpang);
                /* if the first one has a specific probability, use it. If 
                 * it differentiates too much, use rather the estimated 
                 * value */
                __angles[j] = (angeval[0].eval<0.5) ? 
                                _angles[j] : angeval[0].angle;
        }
        /* the results */
        as->alfa = __angles[0];
        as->beta = __angles[1];
        as->gama = __angles[2];
}

void test_coordination()
{
        struct pln_pos_state estpos;
        struct pln_aangle_state aas;
        struct pln_angle_state as;
        int i, j;

        /* set fixed points */
        pln_set_points();

        /* estimated position */
        estpos.x = 500;
        estpos.y = 1600;
        estpos.head = DEG2RAD(180);
        /* measured times (after selections) */
        /*aas.theta[0] = DEG2RAD(47.726);
        aas.theta[1] = DEG2RAD(147.387);
        aas.theta[2] = DEG2RAD(191.302);*/
        /* used to test if we use red reflectors */
        /*aas.theta[0] = DEG2RAD(72.645);
        aas.theta[1] = DEG2RAD(167.592);
        aas.theta[2] = DEG2RAD(315.000);*/

        /*  */
        for (i=0; i<360; i++) {
                estpos.head = DEG2RAD(i);
                pln_pos2ang(estpos, &aas);
                pln_coordinate(estpos, aas, &as);
                printf("head=%f alfa=%f beta=%f gama=%f\n", 
                        DEGREES(estpos.head), DEGREES(as.alfa), 
                        DEGREES(as.beta), DEGREES(as.gama));
        }
}

void pln_sel_angles(struct pln_pos_state estpos, double *angles, 
                        int angcount, struct pln_aangle_state *maas)
{
        struct pln_aangle_state aas;
        struct pln_angle_eval angeval[angcount];
        double eval1, eval2, sgn;
        int i, j;

        /* convert the estimated position to angles. Angles are absolute to 
         * the robot`s head */
        pln_pos2ang(estpos, &aas);
        
#if DEBUG>=1
        printf("pln_sel_angles(): estpos: %f %f %f %f\n",
                        DEGREES(aas.theta[0]),
                        DEGREES(aas.theta[1]),
                        DEGREES(aas.theta[2]),
                        DEGREES(aas.head));
#endif

        /* evaluation, select angles depending to it`s probability. Decrease
         * sigma value of gaussian evaluation function to decrease the 
         * accepted range */
        for (j=0; j<3; j++) {
                if ((aas.theta[j] < DEG2RAD(5)) 
                        || (2*M_PI-aas.theta[j] < DEG2RAD(5))) {
                        for (i=0; i<angcount; i++) {
                                if (angles[i] < M_PI)
                                        sgn = 1;
                                else 
                                        sgn = -1;
                                eval1 = evaluate_gaussian(
                                                aas.theta[j]-angles[i], 0.1);
                                eval2 = evaluate_gaussian(
                                                aas.theta[j] -
                                                (angles[i] + sgn*2*M_PI), 0.1);
#if DEBUG>=2
                                printf("angle=%f sgn=%f %f %f %f %f\n", 
                                        DEGREES(angles[i]), 
                                        sgn,
                                        DEGREES(aas.theta[j]-angles[i]), 
                                        DEGREES(aas.theta[j]-
                                                (angles[i] + sgn*2*M_PI)),
                                        eval1, eval2);
#endif
                                angeval[i].eval = (eval1>eval2) ? eval1:eval2;
                                angeval[i].angle = angles[i];
#if DEBUG>=2
                                printf("%f %f: ", aas.theta[j], angles[i]);
                                printf("%f %f\n", aas.theta[j]-angles[i], 
                                        evaluate_gaussian(aas.theta[j]-angles[i], 
                                                                0.1));
                                printf("gauss=%f %f\n", -0.006645, 
                                        evaluate_gaussian(-0.006645, 0.1));
#endif
                        }
                } else {
                        for (i=0; i<angcount; i++) {
                                angeval[i].angle = angles[i];
                                angeval[i].eval = 
                                        evaluate_gaussian(aas.theta[j]-angles[i], 
                                        0.1);
#if DEBUG>=2
                                printf("%f %f: ", aas.theta[j], angles[i]);
                                printf("%f %f\n", aas.theta[j]-angles[i], 
                                        evaluate_gaussian(aas.theta[j]-angles[i], 
                                        0.1));
                                printf("gauss=%f %f\n", -0.006645, 
                                        evaluate_gaussian(-0.006645, 0.1));
#endif
                        }
                }
                /* sort evaluated angles */
                qsort(angeval,angcount,sizeof(struct pln_angle_eval),cmpang);
                /* if the first one has a specific probability, use it. If 
                 * it differentiates too much, use rather the estimated 
                 * value */
                maas->theta[j] = (angeval[0].eval<2.7) ? 
                                        aas.theta[j] : angeval[0].angle;
        }
}

void test_sel_angles()
{
        struct pln_pos_state estpos;
        struct pln_aangle_state maas;
        double angles[10] = {   DEG2RAD(  5.318970),
                                DEG2RAD( 50.306396),
                                DEG2RAD(101.881714),
                                DEG2RAD(125.046387),
                                DEG2RAD(135.726311),
                                DEG2RAD(208.531494),
                                DEG2RAD(354.326782),
                                DEG2RAD(225.380757),
                                DEG2RAD(281.309932),
                                DEG2RAD(320.819946) };
        int i;

        /* set fixed points */
        pln_set_points();

        /* estimated position */
        estpos.x = 500;
        estpos.y = 1600;
        estpos.head = DEG2RAD(11);

        /*
        for (i=0; i<10; i++)
                angles[i] = TIME2ANGLE(65536, (double)(rand() % 65536));
        angles[0] = DEG2RAD(135.726311);
        angles[5] = DEG2RAD(225.380757);
        angles[9] = DEG2RAD(281.309932);*/
        qsort(angles, 10, sizeof(double), cmpd);

        printf("angles: ");
        for (i=0; i<10; i++)
                printf("%f ", DEGREES(angles[i]));
        printf("\n");

        /* select angles */
        pln_sel_angles(estpos, angles, 10, &maas);

        printf("selected angles: ");
        for(i=0; i<3; i++)
                printf("%f ", DEGREES(maas.theta[i]));
        printf("\n");
}

void pln_cal_position(unsigned int *times, unsigned int timecnt, 
                        struct pln_pos_state act_pos, 
                        struct pln_pos_state *est_pos)
{
        struct pln_aangle_state maas;
        struct pln_angle_state as;
        double angles[10];
        int i;

        /* convert times to angles */
        for (i=0; i<timecnt-1; i++)
                angles[i] = TIME2ANGLE(times[0], times[i+1]);

#if DEBUG>=1
        printf("pln_cal_position(): angles: ");
        for (i=0; i<timecnt-1; i++)
                printf("%f ", DEGREES(angles[i]));
        printf("\n");
#endif

        /* select angles */
        pln_sel_angles(act_pos, angles, timecnt-1, &maas);

#if DEBUG>=1
        printf("pln_cal_position(): selected angles: ");
        for(i=0; i<3; i++)
                printf("%f ", DEGREES(maas.theta[i]));
        printf("\n");
#endif

        /* coordinate angles */
        pln_coordinate(act_pos, maas, &as);

#if DEBUG>=1
        printf("pln_cal_position(): head=%f alfa=%f beta=%f gama=%f\n", 
                DEGREES(act_pos.head), DEGREES(as.alfa), 
                DEGREES(as.beta), DEGREES(as.gama));
#endif

        /* calculate position */
        pln_get_position(as, est_pos);
}

void test_cal_position(struct pln_point *positions, int *poscount)
{
        struct pln_pos_state act_pos;
        struct pln_pos_state est_pos;
        unsigned int times[] = { 54619, 1714, 12312, 34549, 41231, 49670};

        /* set fixed points */
        pln_set_points();

        /* actual position */
        act_pos.x = 500;
        act_pos.y = 1600;
        act_pos.head = DEG2RAD(0);

        /* calculated measured position */
        pln_cal_position(times, sizeof(times)/sizeof(unsigned int), 
                                act_pos, &est_pos);
        printf("calculated posititon: [x=%f,y=%f]\n", est_pos.x, est_pos.y);

        positions[(*poscount)].x = est_pos.x;
        positions[(*poscount)].y = est_pos.y;
        (*poscount)++;
}

---