--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+SUBDIRS = ekf
--- /dev/null
+ULoPoS -- ultrasonic localization system
--- /dev/null
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+ @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+
--- /dev/null
+# -*- makefile -*-
+
+lib_LIBRARIES = uzvekf
+uzvekf_SOURCES = ekf6.c ekf.c matrix.c
+uzvekf_LIBS = m
+
+include_HEADERS = ekf.h
+
+# -ffast-math is the most important optimization here
+#CFLAGS := $(filter-out -fno-strict-aliasing,$(CFLAGS)) --std=c99 -D_GNU_SOURCE -ffast-math -O3 -funroll-loops -ftree-vectorize
--- /dev/null
+/*
+ * EKF (Extended Kalman Filter) for ULoPoS localization system
+ *
+ * Special case of EKF with linear dynamics, linear noise dynamics,
+ * and *nonlinear* output function (C matrix)
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include "ekf.h"
+#include "matrix.h"
+
+#if 0
+#include <stdio.h>
+void dbg_print_mtx(int sym, int n, double *m) {
+ int i, j;
+
+ for (i = 0; i < n; i++) {
+ printf((i==0)?"[":" ");
+ for (j = 0; j < n; j++)
+ switch (sym) {
+ case 1:
+ printf("%f ", (j<i) ? m[i+j*n] : m[j+i*n]);
+ break;
+ case 2:
+ printf("%f ", (j>i) ? m[i+j*n] : m[j+i*n]);
+ break;
+ default:
+ printf("%f ", m[i+j*n]);
+ }
+ printf("%c\n", (i==n-1)?']':';');
+ }
+ printf("\n");
+}
+#endif
+
+void ekf_step(ekf_state_t *ekf, real_t *y) {
+ int i, in, j, jn, n = ekf->n, m = ekf->m;
+ real_t *x = ekf->x, *ym = ekf->ym, *dy = ekf->dy, *exc = ekf->exc,
+ *P = ekf->P, *A = ekf->A, *Q = ekf->Q, *R = ekf->R, *C = ekf->C,
+ *M = ekf->M, *L = ekf->L, *F = ekf->F, *T1 = ekf->T1, *T2 = ekf->T2;
+ real_t e, exc_thr_sqr = ekf->exc_thr_sqr;
+
+ /*
+ * M=C*P*C' + R
+ * L=P*C'*inv(M)
+ */
+ mtx_mul_tr(n, n, m, P, C, T1);
+ mtx_mul(m, n, m, C, T1, M);
+ mtx_add(m, m, M, R, M);
+ //dbg_print_mtx(0, m, M);
+ chol_inv(m, M, T2);
+ //dbg_print_mtx(0, m, T2);
+ mtx_mul(n, m, m, T1, T2, L);
+
+ /*
+ * dy=y-ym
+ * i=1..m: exc(i)=dy(i)^2*(L(:,i)'*inv(P)*L(:,i))
+ */
+ //dbg_print_mtx(0, n, P);
+ chol_inv(n, P, F);
+ //dbg_print_mtx(0, n, F);
+ for (i = 0, in = 0; i < m; i++, in += n) {
+ dy[i] = y[i] - ym[i];
+ mtx_mul(n, n, 1, F, &L[in], T1);
+ e = 0.0;
+ for (j = 0; j < n; j++)
+ e += L[j+in]*T1[j];
+ exc[i] = e*sqr(dy[i]);
+
+ if (exc[i] > exc_thr_sqr)
+ /* zrus mereni, ktera povazujeme za chybna */
+ for (j = 0; j < n; j++)
+ L[j+in] = 0.0;
+ }
+
+ /*
+ * P=P - L*M*L'
+ * P=A*P*A' + Q
+ */
+ mtx_mul_tr(m, m, n, M, L, T1);
+ mtx_mul(n, m, n, L, T1, T2);
+ mtx_sub(n, n, P, T2, P);
+ mtx_mul_tr(n, n, n, P, A, T1);
+ mtx_mul(n, n, n, A, T1, P);
+ mtx_add(n, n, P, Q, P);
+
+ /*
+ * x=x + L*dy
+ * x=A*x
+ */
+ mtx_mul(n, m, 1, L, dy, T1);
+ mtx_add(n, 1, x, T1, T1);
+ mtx_mul(n, n, 1, A, T1, x);
+
+ /* correct P=P' */
+ for (i = 1, in = n; i < n; i++, in += n)
+ for (j = 0, jn = 0; j < i; j++, jn += n)
+ P[i+jn] = P[j+in] = 0.5*(P[i+jn] + P[j+in]);
+}
+
+int ekf_init(ekf_state_t *ekf, int n, int m, real_t exc_thr) {
+ int i;
+
+ ekf->n = n; ekf->m = m;
+ ekf->exc_thr_sqr = sqr(exc_thr);
+
+ if (!((ekf->x = (real_t*)malloc(n*sizeof(real_t))) &&
+ (ekf->ym = (real_t*)malloc(m*sizeof(real_t))) &&
+ (ekf->dy = (real_t*)malloc(m*sizeof(real_t))) &&
+ (ekf->exc = (real_t*)malloc(m*sizeof(real_t))) &&
+ (ekf->P = (real_t*)malloc(n*n*sizeof(real_t))) &&
+ (ekf->A = (real_t*)malloc(n*n*sizeof(real_t))) &&
+ (ekf->Q = (real_t*)malloc(n*n*sizeof(real_t))) &&
+ (ekf->F = (real_t*)malloc(n*n*sizeof(real_t))) &&
+ (ekf->T1 = (real_t*)malloc(n*n*sizeof(real_t))) &&
+ (ekf->T2 = (real_t*)malloc(n*n*sizeof(real_t))) &&
+ (ekf->R = (real_t*)malloc(m*m*sizeof(real_t))) &&
+ (ekf->M = (real_t*)malloc(m*m*sizeof(real_t))) &&
+ (ekf->C = (real_t*)malloc(n*m*sizeof(real_t))) &&
+ (ekf->L = (real_t*)malloc(n*m*sizeof(real_t)))))
+ return -1;
+
+ for (i = 0; i < n; i++)
+ ekf->x[i] = 0.0;
+ for (i = 0; i < n*n; i++)
+ ekf->P[i] = ekf->A[i] = ekf->Q[i] = ekf->F[i] = ekf->T1[i] =
+ ekf->T2[i] = 0.0;
+ for (i = 0; i < m*m; i++)
+ ekf->R[i] = ekf->M[i] = 0.0;
+ for (i = 0; i < n*m; i++)
+ ekf->C[i] = ekf->L[i] = 0.0;
+
+ return 0;
+}
+
+/* KOHEII, */
--- /dev/null
+#define DOUBLE_REAL
+#ifndef REAL_T
+#define REAL_T
+#ifdef DOUBLE_REAL
+typedef double real_t;
+#else
+typedef float real_t;
+#endif
+#endif
+
+#ifndef SQR_FN
+#define SQR_FN
+static inline real_t sqr(real_t x) {
+ return x*x;
+}
+#endif
+
+typedef struct ekf_state_t {
+ int n, m;
+ real_t *x, *ym, *dy, *exc;
+ real_t *P, *A, *Q, *R, *C, *M, *L, *F;
+ real_t *T1, *T2;
+ real_t exc_thr_sqr;
+} ekf_state_t;
+
+typedef struct ekf6_t {
+ ekf_state_t ekf;
+ real_t beacon_xy[3][2];
+ real_t Dmax;
+} ekf6_t;
+
+void ekf_step(ekf_state_t *ekf, real_t *y);
+int ekf_init(ekf_state_t *ekf, int n, int m, real_t exc_thr);
+
+void ekf6_step(ekf6_t *ekf6, real_t *x, real_t *P, int *err, real_t *y);
+int ekf6_init(ekf6_t *ekf6, real_t *beacon_xy, real_t Dmax, real_t exc_thr,
+ real_t *xy, real_t *y0);
--- /dev/null
+/*
+ * 6th order EKF for ULoPoS
+ * state: [x1, x2, D, D', v1, v2]
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include "ekf.h"
+
+void ekf6_output(ekf6_t *ekf6) {
+ int i;
+ real_t *x = ekf6->ekf.x, *ym = ekf6->ekf.ym, *C = ekf6->ekf.C;
+ real_t d;
+
+ for (i = 0; i < 3; i++) {
+ C[i] = x[0] - ekf6->beacon_xy[i][0];
+ C[i+3] = x[1] - ekf6->beacon_xy[i][1];
+ d = sqrt(sqr(C[i]) + sqr(C[i+3]));
+ ym[i] = x[2] + d;
+ C[i] /= d; C[i+3] /= d;
+ }
+}
+
+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 = ekf->n;
+
+ /* rozbal mereni (podle soucasneho odhadu D) */
+ for (i = 0; i < 3; i++)
+ if ((yw[i] = y[i]) < ekf->x[2])
+ yw[i] += ekf6->Dmax;
+
+ /* x --> nelinearni vystupni fce C */
+ ekf6_output(ekf6);
+
+ /* datovy + casovy krok */
+ ekf_step(ekf, yw);
+
+ /* orizni odhad D */
+ ekf->x[2] = fmod(ekf->x[2], ekf6->Dmax);
+
+ memcpy(x, ekf->x, n*sizeof(real_t));
+ memcpy(P, ekf->P, n*n*sizeof(real_t));
+ for (i = 0; i < 3; i++)
+ err[i] = (ekf->exc[i] > ekf->exc_thr_sqr);
+}
+
+int ekf6_init(ekf6_t *ekf6, real_t *beacon_xy, real_t Dmax, real_t exc_thr,
+ real_t *xy, real_t *y0)
+{
+ ekf_state_t *ekf = &ekf6->ekf;
+ 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;
+
+ memcpy(ekf6->beacon_xy, beacon_xy, 2*3*sizeof(real_t));
+ ekf6->Dmax = Dmax;
+
+ if (ekf_init(ekf, n, m, exc_thr))
+ return -1;
+ x = ekf->x; P = ekf->P; A = ekf->A; Q = ekf->Q; R = ekf->R; C = ekf->C;
+
+ 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 = 8e-9;
+ 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);
+
+ return 0;
+}
--- /dev/null
+/*
+ * Basic matrix library, aimed to support EKF, part of ULoPoS
+ * offers +, -, * and Cholesky factorization and inversion
+ *
+ * Works on "one-dimensional" C arrays, organized the same way
+ * as data in MATLAB mxArray: as a string of column vectors;
+ * No memory saving for symetric or sparse matices is employed.
+ */
+
+/*
+ * convention: a_{i,j} = a[i+j*n], a ~ (n,m)
+ * (ie. column vectors concatenated)
+ */
+
+#include <math.h>
+#include <stdlib.h>
+
+/* C=A*B */
+void mtx_mul(int l, int m, int n, double *a, double *b, double *c) {
+ int i, jl, jm, k, kl;
+
+ for (i = 0; i < l; i++)
+ for (jl = 0, jm = 0; jl < l*n; jl += l, jm += m) {
+ c[i+jl] = 0.0;
+ for (k = 0, kl = 0; k < m; k++, kl += l)
+ c[i+jl] += a[i+kl]*b[k+jm];
+ }
+}
+
+/* C=A*B' */
+void mtx_mul_tr(int l, int m, int n, double *a, double *b, double *c) {
+ int i, j, jm, jl, kn, kl;
+
+ for (i = 0; i < l; i++)
+ for (j = 0, jm = 0, jl = 0; j < n; j++, jm += m, jl += l) {
+ c[i+jl] = 0.0;
+ for (kn = 0, kl = 0; kn < m*n; kn += n, kl += l)
+ c[i+jl] += a[i+kl]*b[j+kn];
+ }
+}
+
+void mtx_add(int n, int m, double *a, double *b, double *c) {
+ int i, jn;
+
+ for (i = 0; i < n; i++)
+ for (jn = 0; jn < n*n; jn += n)
+ c[i+jn] = a[i+jn] + b[i+jn];
+}
+
+void mtx_sub(int n, int m, double *a, double *b, double *c) {
+ int i, jn;
+
+ for (i = 0; i < n; i++)
+ for (jn = 0; jn < n*n; jn += n)
+ c[i+jn] = a[i+jn] - b[i+jn];
+}
+
+/*
+ * performs Cholesky-Banachiewicz matrix decomposition
+ * M-->V, that M=V*V'
+ * works only for M=M' & M>0
+ */
+void chol_decomp(int n, double *m, double *v) {
+ int i, in, j, jn, kn;
+
+ for (i = 0, in = 0; i < n; i++, in += n) {
+ for (j = 0, jn = 0; j < i; j++, jn += n) {
+ v[i+jn] = m[i+jn];
+ for (kn = 0; kn < j*n; kn += n)
+ v[i+jn] -= v[i+kn]*v[j+kn];
+ v[i+jn] /= v[j+jn];
+ }
+ v[i+in] = m[i+in];
+ for (kn = 0; kn < i*n; kn += n)
+ v[i+in] -= v[i+kn]*v[i+kn];
+ v[i+in] = sqrt(v[i+in]);
+ }
+}
+
+/*
+ * inverts real, symmetric, positive definite matrix M (ie. M>0 & M=M')
+ * M-->W, that M*W=I
+ */
+void chol_inv(int n, double *m, double *w) {
+ int i, in, j, jn, k, kn;
+
+ chol_decomp(n, m, w);
+
+ /* W=inv(W)' (fill upper triangle) */
+ for (i = 0, in = 0; i < n; i++, in += n) {
+ w[i+in] = 1.0/w[i+in];
+ for (j = 0; j < i; j++) {
+ w[j+in] = 0.0;
+ for (kn = j*n; kn < i*n; kn += n)
+ w[j+in] += w[i+kn]*w[j+kn];
+ w[j+in] *= -w[i+in];
+ }
+ }
+
+ /* W=W*W' (fill only the lower triangle) */
+ for (i = 0; i < n; i++) {
+ for (j = 0, jn = 0; j <= i; j++, jn += n) {
+ if (i == j) {
+ w[i+jn] *= w[i+jn];
+ k = i+1;
+ }
+ else {
+ w[i+jn] = 0.0;
+ k = i;
+ }
+ for (kn = k*n; k < n; k++, kn += n)
+ w[i+jn] += w[i+kn]*w[j+kn];
+ }
+ }
+
+ /* copy to upper triangle of W */
+ for (i = 0, in = 0; i < n-1; i++, in += n)
+ for (j = i+1, jn = j*n; j < n; j++, jn += n)
+ w[i+jn] = w[j+in];
+}
+
+/* KOHEII, */
--- /dev/null
+void mtx_mul(int l, int m, int n, double *a, double *b, double *c);
+void mtx_mul_tr(int l, int m, int n, double *a, double *b, double *c);
+void mtx_add(int n, int m, double *a, double *b, double *c);
+void mtx_sub(int n, int m, double *a, double *b, double *c);
+void chol_decomp(int n, double *m, double *v);
+void chol_inv(int n, double *m, double *w);
projects / eurobot / public.git / commitdiff