ULoPoS: swapped prediction/correction step in EKF

it helped a bit, now the measurement is smoother, but delayed
greetings to Havlena ;-)

diff --git a/src/uzv/ekf/ekf.c b/src/uzv/ekf/ekf.c
index 862a726478530cb6a2124e9463460a8dd7ce1fdd..70ca8499e31b5ca976e3a70be69a7a491e846334 100644 (file)
--- a/src/uzv/ekf/ekf.c
+++ b/src/uzv/ekf/ekf.c
@@ -43,6 +43,16 @@ void ekf_step(ekf_state_t *ekf, real_t *y) {
     *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;
 
+  /*
+   * P=A*P*A' + Q
+   * x=A*x
+   */
+  mtx_mul_tr(n, n, n, P, A, T1);
+  mtx_mul(n, n, n, A, T1, P);
+  mtx_add(n, n, P, Q, P);
+  memcpy(T1, x, n*sizeof(real_t));
+  mtx_mul(n, n, 1, A, T1, x);
+
   /*
    * M=C*P*C' + R
    * L=P*C'*inv(M)
@@ -78,22 +88,13 @@ void ekf_step(ekf_state_t *ekf, real_t *y) {
 
   /*
    * P=P - L*M*L'
-   * P=A*P*A' + Q
+   * x=x + L*dy
    */
   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);
+  mtx_add(n, 1, x, T1, x);
 
   /* correct P=P' */
   for (i = 1, in = n; i < n; i++, in += n)