--- /dev/null
+/*
+ * Calculates analytic solution to hyperbolic intersection problem
+ * -- part of ULoPoS localization system
+ * (much worse results compared to dynamical estimation, but can
+ * provide initial estimates/guess)
+ */
+
+#include <math.h>
+
+#include "wt.h"
+
+int hypintsec(real_t *x, real_t *y, real_t d1, real_t d2) {
+ real_t
+ xa, xb, xc, ya, yb, yc, d12, d22, xa2, xb2, xc2, ya2, yb2, yc2,
+ xamxb, yamyb, xapxb, xamxb2, xmxpymy2, d12mxamxb2, d12txapxb,
+ xamxbtxmxpymy2, c1, c2, c3, c12, c22, k1, k2, k3, B, D, sqrtD,
+ tmp;
+ real_t ds[3], E[2];
+ int i, j;
+
+ xa = beacon_xy[0][0]; xb = beacon_xy[1][0]; xc = beacon_xy[2][0];
+ ya = beacon_xy[0][1]; yb = beacon_xy[1][1]; yc = beacon_xy[2][1];
+
+ d12 = sqr(d1);
+ d22 = sqr(d2);
+ xa2 = sqr(xa); xb2 = sqr(xb); xc2 = sqr(xc);
+ ya2 = sqr(ya); yb2 = sqr(yb); yc2 = sqr(yc);
+ xamxb = xa-xb; yamyb = ya-yb;
+ xapxb = xa+xb;
+ xamxb2 = sqr(xamxb);
+ xmxpymy2 = xa2-xb2+ya2-yb2;
+ d12mxamxb2 = d12-xamxb2;
+ d12txapxb = d12*xapxb;
+ xamxbtxmxpymy2 = xamxb*xmxpymy2;
+ c3 = (d1-d2)*xa + d2*xb - d1*xc;
+ c1 = ((d2-d1)*ya - d2*yb + d1*yc)/c3;
+ c2 = (d2*(-d12 - xmxpymy2) + d1*(d22+xa2-xc2+ya2-yc2))/(2*c3);
+ c12 = sqr(c1); c22 = sqr(c2);
+ k1 = 4*(d12*(ya+yb) - yamyb*(-2*c2*xamxb + xmxpymy2)
+ + c1*(d12txapxb - 2*c2*d12mxamxb2 - xamxbtxmxpymy2));
+ k2 = sqr(d12) + sqr(xmxpymy2) - 2*d12*(xa2+xb2+ya2+yb2)
+ + 4*(c2*(d12txapxb - xamxbtxmxpymy2) - c22*d12mxamxb2);
+ k3 = 4*(-d12*(1+c12) + sqr(c1*xamxb + yamyb));
+ B = k1/(2*k3);
+ D = sqr(B)-k2/k3;
+
+ if (D > 0.0)
+ sqrtD = sqrt(D);
+ else
+ sqrtD = 0.0;
+ y[0] = -B - sqrtD; y[1] = -B + sqrtD;
+ x[0] = c1*y[0] + c2; x[1] = c1*y[1] + c2;
+
+ /* better solution first */
+ for (i = 0; i < 2; i++) {
+ for (j = 0; j < 3; j++)
+ ds[j] = sqrt(sqr(x[i]-beacon_xy[j][0]) + sqr(y[i]-beacon_xy[j][1]));
+ E[i] = fabs(ds[0]-ds[1] - d1) + fabs(ds[0]-ds[2] - d2);
+ }
+ if (E[1] < E[0]) {
+ tmp = x[0]; x[0] = x[1]; x[1] = tmp;
+ tmp = y[0]; y[0] = y[1]; y[1] = tmp;
+ }
+
+ /* tell, whether there was a solution, or D has been forced to 0 */
+ return (D > 0.0);
+}
--- /dev/null
+/*
+ * ULoPoS EKF visualisation using X11 (Xlib)
+ * reads position triplets (output from [can]corrintp) from stdin
+ * draws trajectory, velocity, covariance, etc.
+ */
+
+#include <inttypes.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "ekf.h"
+#include "wt.h"
+
+real_t beacon_xy[3][2] = {
+ {-0.04, 2.14},
+ {3.04, 1.05},
+ {-0.04, -0.04},
+ /* {0, 0}, {0.4, 0}, {0, 0.4}, */
+};
+
+plot_data_t data = { .n = MAX_POINTS, };
+ekf6_t ekf6;
+
+int hyperb_xy(real_t *hx, real_t *hy, real_t *t) {
+ real_t x[2], y[2];
+ int i1, i2, i3, ok;
+
+ /* unwrap */
+ i1 = (t[0]<t[1]) ? ((t[0]<t[2])?0:2) : ((t[1]<t[2])?1:2);
+ i3 = (t[0]>t[1]) ? ((t[0]>t[2])?0:2) : ((t[1]>t[2])?1:2);
+ i2 = 6 - i1 - i3;
+ if (t[i3] - t[i1] > 0.5*D_MAX)
+ t[i1] += D_MAX;
+ if (t[i3] - t[i2] > 0.5*D_MAX)
+ t[i2] += D_MAX;
+
+ ok = hypintsec(x, y, t[0]-t[1], t[0]-t[2]);
+
+ *hx = x[0]; *hy = y[0];
+ return ok;
+}
+
+int main(int argc, char *argv[]) {
+ real_t t[3], x[6], P[6*6];
+ unsigned u[3];
+ int i, virgo = 1;
+
+ xw_init();
+
+ while (!feof(stdin)) {
+ scanf("%u%u%u", u, u+1, u+2);
+
+ for (i = 0; i < 3; i++)
+ t[i] = (XCORR2METER/32.0)*u[i];
+
+#if 1
+ if (virgo) {
+ real_t xy0[] = {2.63, 0.35};
+ ekf6_init(&ekf6, (real_t*)beacon_xy, D_MAX, 30.0, xy0, t);
+ virgo = 0;
+ }
+
+ ekf6_step(&ekf6, x, P, data.err, t);
+
+ for (i = data.n-1; i > 0; i--) {
+ data.x1[i] = data.x1[i-1];
+ data.x2[i] = data.x2[i-1];
+ }
+ data.x1[0] = x[0]; data.x2[0] = x[1];
+ data.v1 = x[4]; data.v2 = x[5];
+ data.s11 = P[0]; data.s12 = P[1]; data.s22 = P[7];
+#else
+ /* alternative: no EKF estimation, use analytic calculation */
+
+ real_t xx[2], yy[2];
+ hypintsec(xx, yy, t[0]-t[1], t[0]-t[2]);
+ data.x1[0] = xx[0];
+ data.x2[0] = yy[0];
+#endif
+ for (i = 0; i < 6; i++)
+ printf("%f\t", x[i]);
+ printf("%d %d %d\n", data.err[0], data.err[1], data.err[2]);
+
+ xw_draw(&data);
+ }
+
+ return 0;
+}
--- /dev/null
+#include <X11/Xlib.h>
+#include <X11/extensions/Xdbe.h>
+#include <inttypes.h>
+#include <limits.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <stdio.h>
+
+#include "wt.h"
+
+#define WIDTH 700
+#define HEIGHT 500
+#define BORDER 20
+
+#define PT_SIZE 1
+#define VELO_SCALE 10.0
+#define VELO_DX -5
+#define VELO_DY -3
+#define COV_SCALE 1000.0
+#define COV_N 12
+
+Display *dpy;
+Window w, dw;
+int dpy_dbe;
+XdbeBackBuffer w_bb;
+XdbeSwapInfo bb_swap;
+GC gc_path, gc_velo, gc_cov, gc_fld1, gc_fld2;
+
+real_t xy_scale, cov_c[COV_N], cov_s[COV_N];
+int x0x, y0y;
+
+void set_coord() {
+ real_t tmp, min_x, max_x, min_y, max_y;
+ int i, m;
+
+ min_x = max_x = beacon_xy[0][0];
+ min_y = max_y = beacon_xy[0][1];
+ for (m = 0; m < 3; m++) {
+ if ((tmp = beacon_xy[m][0] - 0.04) < min_x) min_x = tmp;
+ if ((tmp = beacon_xy[m][0] + 0.04) > max_x) max_x = tmp;
+ if ((tmp = beacon_xy[m][1] - 0.04) < min_y) min_y = tmp;
+ if ((tmp = beacon_xy[m][1] + 0.04) > max_y) max_y = tmp;
+ }
+ xy_scale = (WIDTH - 2*BORDER)/(max_x - min_x);
+ if ((tmp = (HEIGHT - 2*BORDER)/(max_y - min_y)) < xy_scale)
+ xy_scale = tmp;
+ x0x = 0.5*(WIDTH - xy_scale*(min_x + max_x) + 1.0);
+ y0y = 0.5*(HEIGHT + xy_scale*(min_y + max_y) + 1.0);
+
+ for (i = 0; i < COV_N; i++) {
+ tmp = (2*M_PI*i)/(real_t)COV_N;
+ cov_c[i] = cos(tmp); cov_s[i] = sin(tmp);
+ }
+}
+
+void init_gfx() {
+ GC *p, *gc[] = {&gc_path, &gc_velo, &gc_cov, &gc_fld1, &gc_fld2};
+ char *color[] = {"#FFFFFF", "#FF0000", "#FF00FF", "#00FFFF", "#0000FF"};
+ Colormap cmap;
+ XColor col;
+ int i;
+
+ cmap = DefaultColormap(dpy, 0);
+
+ for (i = 0; i < 5; i++) {
+ p = gc[i];
+ *p = XCreateGC(dpy, w, 0, NULL);
+ XParseColor(dpy, cmap, color[i], &col);
+ XAllocColor(dpy, cmap, &col);
+ XSetForeground(dpy, *p, col.pixel);
+ XSetLineAttributes(dpy, *p, 0, LineSolid, CapButt, JoinBevel);
+ }
+}
+
+void xw_init() {
+ XEvent e;
+ int v_maj, v_min;
+
+ dpy = XOpenDisplay(NULL);
+ w = XCreateSimpleWindow(dpy, DefaultRootWindow(dpy), 0, 0, WIDTH, HEIGHT, 0,
+ WhitePixel(dpy, 0), BlackPixel(dpy, 0));
+
+ if ((dpy_dbe = XdbeQueryExtension(dpy, &v_maj, &v_min))) {
+ w_bb = XdbeAllocateBackBufferName(dpy, w, XdbeBackground);
+ bb_swap.swap_window = w;
+ bb_swap.swap_action = XdbeBackground;
+ dw = w_bb;
+ }
+ else
+ dw = w;
+
+ init_gfx();
+ set_coord();
+
+ XSelectInput(dpy, w, ExposureMask);
+ XMapWindow(dpy, w);
+
+ for (;;) {
+ XNextEvent(dpy, &e);
+ if (e.type == Expose)
+ break;
+ }
+}
+
+void xw_flush_events() {
+ XEvent e;
+ while (XCheckWindowEvent(dpy, w, -1, &e));
+}
+
+inline int x2x(real_t x) {
+ real_t tmp = xy_scale*x + 0.5;
+ if (tmp < INT_MIN)
+ return INT_MIN;
+ if (tmp > INT_MAX)
+ return INT_MAX;
+ return x0x + (int)tmp;
+}
+
+inline int y2y(real_t y) {
+ real_t tmp = - xy_scale*y + 0.5;
+ if (tmp < INT_MIN)
+ return INT_MIN;
+ if (tmp > INT_MAX)
+ return INT_MAX;
+ return y0y + (int)tmp;
+}
+
+void draw_field(plot_data_t *data) {
+ XPoint pt[5];
+ int m, x, y, a = (int)(xy_scale*0.04 + 0.5), b = 1+a/2;
+
+ for (m = 0; m < 3; m++) {
+ x = x2x(beacon_xy[m][0]); y = y2y(beacon_xy[m][1]);
+ pt[0].x = x-a; pt[0].y = y-a;
+ pt[1].x = x-a; pt[1].y = y+a;
+ pt[2].x = x+a; pt[2].y = y+a;
+ pt[3].x = x+a; pt[3].y = y-a;
+ pt[4] = pt[0];
+ XDrawLines(dpy, dw, gc_fld1, pt, 5, CoordModeOrigin);
+
+ if (data->err[m])
+ XFillArc(dpy, dw, gc_velo, x-b, y-b, b, b, 0, 360*64);
+ }
+
+ /***/
+}
+
+void plot_path(plot_data_t *data) {
+ XPoint pt[MAX_POINTS];
+ int i;
+
+ for (i = 0; i < data->n; i++) {
+ pt[i].x = x2x(data->x1[i]);
+ pt[i].y = y2y(data->x2[i]);
+ }
+ XDrawLines(dpy, dw, gc_path, pt, data->n, CoordModeOrigin);
+ for (i = 0; i < data->n; i++)
+ XFillArc(dpy, dw, gc_path, pt[i].x - PT_SIZE, pt[i].y - PT_SIZE,
+ 2*PT_SIZE, 2*PT_SIZE, 0, 360*64);
+}
+
+void plot_cov(plot_data_t *data) {
+ XPoint pt[COV_N+1];
+ int i;
+
+ for (i = 0; i < COV_N; i++) {
+ pt[i].x = x2x(data->x1[0] +
+ COV_SCALE*(data->s11*cov_c[i] + data->s12*cov_s[i]));
+ pt[i].y = y2y(data->x2[0] +
+ COV_SCALE*(data->s12*cov_c[i] + data->s22*cov_s[i]));
+ }
+ pt[COV_N] = pt[0];
+ XDrawLines(dpy, dw, gc_cov, pt, COV_N+1, CoordModeOrigin);
+}
+
+void plot_velo(plot_data_t *data) {
+ int dx, dy,
+ x1 = x2x(data->x1[0]), y1 = y2y(data->x2[0]),
+ x2 = x1 + (int)(VELO_SCALE*xy_scale*data->v1 + 0.5),
+ y2 = y1 - (int)(VELO_SCALE*xy_scale*data->v2 + 0.5);
+ real_t c;
+
+ XDrawLine(dpy, dw, gc_velo, x1, y1, x2, y2);
+ c = 1.0/sqrt(sqr(data->v1) + sqr(data->v2) + 1e-12);
+ dx = (int)(c*(data->v1*VELO_DX - data->v2*VELO_DY) + 0.5);
+ dy = (int)(c*(data->v2*VELO_DX + data->v1*VELO_DY) + 0.5);
+ XDrawLine(dpy, dw, gc_velo, x2, y2, x2 + dx, y2 - dy);
+ dx = (int)(c*(data->v1*VELO_DX + data->v2*VELO_DY) + 0.5);
+ dy = (int)(c*(data->v2*VELO_DX - data->v1*VELO_DY) + 0.5);
+ XDrawLine(dpy, dw, gc_velo, x2, y2, x2 + dx, y2 - dy);
+}
+
+void xw_draw(plot_data_t *data) {
+ xw_flush_events();
+
+ if (!dpy_dbe)
+ XClearWindow(dpy, dw);
+
+ draw_field(data);
+ plot_path(data);
+ plot_cov(data);
+ plot_velo(data);
+
+ if (dpy_dbe)
+ XdbeSwapBuffers(dpy, &bb_swap, 1);
+
+ XFlush(dpy);
+}
projects / eurobot / public.git / commitdiff