ULoPoS EKF/matrix.c: crude protection agains numeric faults

Two operations are protected in a very basic way against numerical fault,
which occured very often during tests. The operations are float / and sqrt.
If supplied with wrong data, the resulting matrices diverged to either NaN of Inf
and never returned to normal values.

diff --git a/src/uzv/ekf/matrix.c b/src/uzv/ekf/matrix.c
index 384c1273f3074aef496de2cb5118c24f0e725df4..b0a5ea2ecabc3b418e06ded237cf8385c4fdb2f5 100644 (file)
--- a/src/uzv/ekf/matrix.c
+++ b/src/uzv/ekf/matrix.c
@@ -15,6 +15,8 @@
 #include <math.h>
 #include <stdlib.h>
 
+#define MAX_VAL 1.0e6
+
 /* C=A*B */
 void mtx_mul(int l, int m, int n, double *a, double *b, double *c) {
   int i, jl, jm, k, kl;
@@ -60,8 +62,9 @@ void mtx_sub(int n, int m, double *a, double *b, double *c) {
  * M-->V, that M=V*V'
  * works only for M=M' & M>0
  */
-void chol_decomp(int n, double *m, double *v) {
+int chol_decomp(int n, double *m, double *v) {
   int i, in, j, jn, kn;
+  int rc, fail = 0;
 
   for (i = 0, in = 0; i < n; i++, in += n) {
     for (j = 0, jn = 0; j < i; j++, jn += n) {
@@ -69,26 +72,48 @@ void chol_decomp(int n, double *m, double *v) {
       for (kn = 0; kn < j*n; kn += n)
        v[i+jn] -= v[i+kn]*v[j+kn];
       v[i+jn] /= v[j+jn];
+
+      if (!finite(v[i+jn])) {
+       fail = 1;
+       if ((rc = isinf(v[i+jn])))
+         v[i+jn] = rc*MAX_VAL;
+       else if (isnan(v[i+jn]))
+         v[i+jn] = 1.0;
+      }
     }
     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]);
+
+    if (!finite(v[i+jn])) {
+      fail = 1;
+      v[i+in] = 0.0;
+    }
   }
+
+  return fail;
 }
 
 /*
  * 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 chol_inv(int n, double *m, double *w) {
   int i, in, j, jn, k, kn;
+  int fail;
 
-  chol_decomp(n, m, w);
+  fail = 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];
+
+    if (!finite(w[i+in])) {
+      fail = 1;
+      w[i+in] = isinf(w[i+in])*MAX_VAL;
+    }
+
     for (j = 0; j < i; j++) {
       w[j+in] = 0.0;
       for (kn = j*n; kn < i*n; kn += n)
@@ -117,6 +142,8 @@ void chol_inv(int n, double *m, double *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];
+
+  return fail;
 }
 
 /* KOHEII, */