ULoPoS: EKF error recovery and reinitialization added

- detection of "get lost" state
-- if robot found too far out of playground
-- if state contains NaN or Inf (should not happen)
-- if >=2 measurement errors occur in >=N_LOST consecutive periods
- reinitialization using analytic solution

diff --git a/src/uzv/ekf/ekf.h b/src/uzv/ekf/ekf.h
index f486fd05947794f66ffd768bae6d87f2fe2c29ec..fce3e76a8e836b70ce859615d4a59671e21f6485 100644 (file)
--- a/src/uzv/ekf/ekf.h
+++ b/src/uzv/ekf/ekf.h
@@ -27,6 +27,7 @@ typedef struct ekf6_t {
   ekf_state_t ekf;
   real_t beacon_xy[3][2];
   real_t Dmax;
+  int err_cnt, lost;
 } ekf6_t;
 
 void ekf_step(ekf_state_t *ekf, real_t *y);

diff --git a/src/uzv/ekf/ekf6.c b/src/uzv/ekf/ekf6.c
index 69d2d8caa1d062b80afdd455680e95c803e63ca3..080452ad11f29b1ae35bec724b2960183e940d49 100644 (file)
--- a/src/uzv/ekf/ekf6.c
+++ b/src/uzv/ekf/ekf6.c
@@ -7,9 +7,19 @@
 #include <string.h>
 #include <math.h>
 
+#include "geom.h"
 #include "ekf.h"
 
+#define N_LOST  12
 #define Q_DRIFT 8e-9
+#define BOOT_DIST_ERR_THR 0.2
+#define MAX_OUT_TOL 0.1
+
+void sub_init_state(ekf6_t *ekf6, real_t *xy, real_t t1);
+
+inline real_t beacon_dist(real_t *bxy, real_t rx, real_t ry) {
+  return sqrt(sqr(rx - bxy[0]) + sqr(ry - bxy[1]));
+}
 
 void ekf6_output(ekf6_t *ekf6) {
   int i;
@@ -25,10 +35,69 @@ void ekf6_output(ekf6_t *ekf6) {
   }
 }
 
+void ekf6_reboot(ekf6_t *ekf6, real_t *y) {
+  real_t t[3], xh[2], yh[2], xy0[2], ds[3];
+  real_t d1, d2, E, Dmax = ekf6->Dmax;
+  int i, i1, i2, i3, out1, out2, n = ekf6->ekf.n;
+
+  DBG("EKF reboot\n");
+
+  /* unwrap */
+  t[0] = y[0];  t[1] = y[1];  t[2] = y[2];
+  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 = 3 - i1 - i3;
+  if (t[i3] - t[i1] > 0.5*Dmax)
+    t[i1] += Dmax;
+  if (t[i3] - t[i2] > 0.5*Dmax)
+    t[i2] += Dmax;
+  
+  /* find analytic TDOA solution(s) */
+  d1 = t[0]-t[1];  d2 = t[0]-t[2];
+  hypintsec((real_t*)ekf6->beacon_xy, xh, yh, d1, d2);
+
+  /* choose the better one */
+  out1 = geom_out_bound(xh[0], yh[0]);
+  out2 = geom_out_bound(xh[1], yh[1]);
+
+  if (out1 && out2) {
+    i = (rand() > RAND_MAX/2);
+    geom_nearest_bound(xh[i], yh[i], xy0, xy0 + 1);
+    DBG("{(%f,%f), (%f,%f)}\n", xh[0], yh[0], xh[1], yh[1]);
+    DBG("geom_nearest_bound (%f,%f)-->(%f,%f)\n", xh[i], yh[i],
+       xy0[0], xy0[1]);
+  }
+  else if (out2) {
+    xy0[0] = xh[0];  xy0[1] = yh[0];
+  }
+  else if (out1) {
+    xy0[0] = xh[1];  xy0[1] = yh[1];
+  }
+  else {
+    for (i = 0; i < 3; i++)
+      ds[i] = beacon_dist(ekf6->beacon_xy[i], xh[1], yh[1]);
+    E = fabs(ds[0]-ds[1] - d1) + fabs(ds[0]-ds[2] - d2);
+    DBG("E=%g\n", E);
+    if (E > BOOT_DIST_ERR_THR)
+      i = 0;
+    else
+      i = (rand() > RAND_MAX/2);
+    xy0[0] = xh[i];  xy0[1] = yh[i];
+  }
+
+  /* initialize state with x, y and zero velocities */
+  for (i = 0; i < n*n; i++)
+    ekf6->ekf.P[i] = 0.0;
+  sub_init_state(ekf6, xy0, y[0]);
+
+  DBG("reinitialized x=%f, y=%f, D=%f\n",
+      ekf6->ekf.x[0], ekf6->ekf.x[1], ekf6->ekf.x[2]);
+}
+
 void ekf6_step(ekf6_t *ekf6, real_t *x, real_t *P, int *err, real_t *y) {
   ekf_state_t *ekf = &ekf6->ekf;
-  real_t yw[3];
-  int i, n_err, n = ekf->n;
+  real_t yw[3], nx, ny;
+  int i, n_err, lost_now, n = ekf->n;
 
   /* rozbal mereni (podle soucasneho odhadu D) */
   for (i = 0; i < 3; i++)
@@ -52,10 +121,80 @@ void ekf6_step(ekf6_t *ekf6, real_t *x, real_t *P, int *err, real_t *y) {
   /* pri >= 1 chybnem mereni zastav sum procesu driftu hodin */
   ekf->Q[3*7] = (n_err >= 1) ? 0.0 : Q_DRIFT;
 
+  /* detekce stavu "jsem ztracen" a znovuspusteni */
+  lost_now = 0;
+  for (i = 0; i < n; i++)
+    /* Inf, Nan --> jsem ztracen */
+    if (!finite(ekf->x[i])) {
+      lost_now = 1;
+      DBG("LOST: Inf/NaN\n");
+      break;
+    }
+
+  if (!lost_now) {
+    if (geom_out_bound(ekf->x[0], ekf->x[1])) {
+      if (geom_nearest_bound(ekf->x[0], ekf->x[1], &nx, &ny) > MAX_OUT_TOL) {
+       lost_now = 1;
+       DBG("LOST: out\n");
+      }
+      else {
+       ekf->x[0] = nx;  ekf->x[1] = ny;
+      }
+    }
+  }
+
+  if (!lost_now) {
+    if (ekf6->lost) {
+      if (n_err == 0) {
+       /* robot uz se (snad) nasel */
+       ekf6->err_cnt = 0;
+       ekf6->lost = 0;
+      }
+      else
+       lost_now = 1;
+    }
+    else {
+      if (n_err >= 2) {
+       if (++ekf6->err_cnt >= N_LOST) {
+         lost_now = 1;
+         DBG("LOST: err_cnt\n");
+       }
+      }
+      else
+       ekf6->err_cnt = 0;
+    }
+  }
+
+  if (lost_now) {
+    ekf6_reboot(ekf6, y);
+    ekf6->lost = 1;
+  }
+
   memcpy(x, ekf->x, n*sizeof(real_t));
   memcpy(P, ekf->P, n*n*sizeof(real_t));
 }
 
+void sub_init_state(ekf6_t *ekf6, real_t *xy, real_t t1) {
+  ekf_state_t *ekf = &ekf6->ekf;
+  real_t *x = ekf->x, *P = ekf->P;
+  real_t Dmax = ekf6->Dmax;
+  real_t d_xy, dr_max, eps;
+
+  d_xy = 2.0;
+  dr_max = (2*1.5e-3)*Dmax;
+  eps = 1e-12;
+
+  P[0*7] = P[1*7] = d_xy;
+  P[3*7] = dr_max;
+  P[2*7] = P[4*7] = P[5*7] = eps;
+
+  x[0] = xy[0];  x[1] = xy[1];
+  x[2] = fmod(Dmax + t1 - beacon_dist(ekf6->beacon_xy[0], x[0], x[1]), Dmax);
+  x[3] = x[4] = x[5] = 0.0;
+}
+
+#include <stdio.h>
+
 int ekf6_init(ekf6_t *ekf6, real_t *beacon_xy, real_t Dmax, real_t exc_thr,
              real_t *xy, real_t *y0)
 {
@@ -63,7 +202,7 @@ int ekf6_init(ekf6_t *ekf6, real_t *beacon_xy, real_t Dmax, real_t exc_thr,
   int n = 6, m = 3;
   real_t *x = ekf->x,
     *P = ekf->P, *A = ekf->A, *Q = ekf->Q, *R = ekf->R, *C = ekf->C;
-  real_t d_xy, dr_max, acx, acd, eps;
+  real_t acx, acd;
 
   memcpy(ekf6->beacon_xy, beacon_xy, 2*3*sizeof(real_t));
   ekf6->Dmax = Dmax;
@@ -75,28 +214,20 @@ int ekf6_init(ekf6_t *ekf6, real_t *beacon_xy, real_t Dmax, real_t exc_thr,
   A[0*7] = A[1*7] = A[2*7] = A[3*7] = A[4*7] = A[5*7] = 1.0;
   A[2+3*6] = A[0+4*6] = A[1+5*6] = 1.0;
 
-  d_xy = 2.0;
-  dr_max = (2*1.5e-3)*Dmax;
   acx = 2e-7;
   acd = Q_DRIFT;
-  eps = 1e-12;
   Q[3*7] = acd;
   Q[4*7] = Q[5*7] = acx;
 
   R[0*4] = R[1*4] = R[2*4] = 5e-5;
 
-  P[0*7] = P[1*7] = d_xy;
-  P[3*7] = dr_max;
-  P[2*7] = P[4*7] = P[5*7] = eps;
-
   C[6] = C[7] = C[8] = 1.0;
 
   /* pocatecni stav */
-  x[0] = xy[0];  x[1] = xy[1];
-  x[2] = fmod(Dmax + y0[0]
-             - sqrt(sqr(x[0] - ekf6->beacon_xy[0][0])
-                    + sqr(x[1] - ekf6->beacon_xy[0][1])),
-             Dmax);
+  sub_init_state(ekf6, xy, y0[0]);
+
+  /* vynuluj chybove stavy */
+  ekf6->err_cnt = ekf6->lost = 0;
 
   return 0;
 }

diff --git a/src/uzv/geom/Makefile.omk b/src/uzv/geom/Makefile.omk
index ab05f430d2cc3d50b215377e0a0da4f5b146338b..d27b1b89c54e0615302dc26616f6e4df023995eb 100644 (file)
--- a/src/uzv/geom/Makefile.omk
+++ b/src/uzv/geom/Makefile.omk
@@ -1,7 +1,7 @@
 # -*- makefile -*-
 
 lib_LIBRARIES = uzvgeom
-uzvgeom_SOURCES = hypintsec.c
+uzvgeom_SOURCES = hypintsec.c geom_out.c
 uzvgeom_LIBS = m
 
 include_HEADERS = geom.h

diff --git a/src/uzv/geom/geom.h b/src/uzv/geom/geom.h
index 7f7a8cc9321e417752d51ee2e231b6dfeb2efdce..a2baa046e06a54f93b6f17a6b4cf6ec91fc988d3 100644 (file)
--- a/src/uzv/geom/geom.h
+++ b/src/uzv/geom/geom.h
@@ -1,6 +1,18 @@
 #ifndef GEOM_H
 #define GEOM_H
 
+#include "robodim.h"
+
+#define min(a,b)      (((a) < (b)) ? (a) : (b))
+#define MIN_DISTANCE  min(ROBOT_WIDTH_M/2.0, \
+                         (ROBOT_AXIS_TO_BACK_M+ROBOT_AXIS_TO_FRONT_M)/2.0)
+
+#define MIN_POS_X     MIN_DISTANCE
+#define MIN_POS_Y     MIN_DISTANCE
+#define MAX_POS_X     (PLAYGROUND_WIDTH_M - MIN_DISTANCE)
+#define MAX_POS_Y     (PLAYGROUND_HEIGHT_M - MIN_DISTANCE)
+
+#define DOUBLE_REAL
 #ifndef REAL_T
 #define REAL_T
 #ifdef DOUBLE_REAL
@@ -17,6 +29,8 @@ static inline real_t sqr(real_t x) {
 }
 #endif
 
+int geom_out_bound(real_t x, real_t y);
+real_t geom_nearest_bound(real_t x, real_t y, real_t *xn, real_t *yn);
 int hypintsec(real_t *beacon_xy, real_t *x, real_t *y, real_t d1, real_t d2);
 
 #endif /* GEOM_H */

diff --git a/src/uzv/geom/geom_out.c b/src/uzv/geom/geom_out.c
new file mode 100644 (file)
index 0000000..448145f
--- /dev/null
+++ b/src/uzv/geom/geom_out.c
@@ -0,0 +1,38 @@
+#include <math.h>
+#include <float.h>
+
+#include "geom.h"
+
+int geom_out_bound(real_t x, real_t y) {
+  return
+    (x < MIN_POS_X) ||
+    (x > MAX_POS_X) ||
+    (y < MIN_POS_Y) ||
+    (y > MAX_POS_Y);
+}
+
+real_t geom_nearest_bound(real_t x, real_t y, real_t *xn, real_t *yn) {
+  static real_t
+    x1[] = {MIN_POS_X, MIN_POS_X, MIN_POS_X, MAX_POS_X},
+    y1[] = {MIN_POS_Y, MAX_POS_Y, MIN_POS_Y, MIN_POS_Y},
+    dx[] = {MAX_POS_X-MIN_POS_X, MAX_POS_X-MIN_POS_X, 0.0, 0.0},
+    dy[] = {0.0, 0.0, MAX_POS_Y-MIN_POS_Y, MAX_POS_Y-MIN_POS_Y};
+  real_t t, xt, yt, d, d_min = FLT_MAX;
+  int i;
+
+  for (i = 0; i < 4; i++) {
+    t = (dx[i]*(x - x1[i]) + dy[i]*(y - y1[i]))/(sqr(dx[i]) + sqr(dy[i]));
+    if (t > 1.0)
+      t = 1.0;
+    else if (t < 0.0)
+      t = 0.0;
+
+    xt = x1[i] + t*dx[i];  yt = y1[i] + t*dy[i];
+    if ((d = sqr(x - xt) + sqr(y - yt)) < d_min) {
+      d_min = d;
+      *xn = xt;  *yn = yt;
+    }
+  }
+
+  return d_min;
+}