Adding log likelihood computation.

src/yafl.c

@@ -34,6 +34,8 @@
 #define _UR  (self->Ur)
 #define _DR  (self->Dr)
 
+#define _L   (self->l)
+
 #define _NX  (self->Nx)
 #define _NZ  (self->Nz)
 
@@ -251,12 +253,18 @@ yaflStatusEn yafl_ekf_base_update(yaflKalmanBaseSt * self, yaflFloat * z, yaflKa
         YAFL_TRY(status_q, yafl_math_set_vxn(_NX, _DQ, _DQ, 1.0 - _QFF));
     }
 
+    /*Start log likelihood computation*/
+    *self->l = _NZ * YAFL_L2PI;
+
     /* Do scalar updates */
     for (j = 0; j < _NZ; j++)
     {
         YAFL_TRY(status, scalar_update(self, j)); /*Q is updated here if needed!*/
     }
 
+    /*Finalize log likelihood value*/
+    *self->l *= -0.5;
+
     if (self->rff > 0.0)
     {
         /*Compute residual if needed*/
@@ -271,7 +279,7 @@ static inline yaflStatusEn \
     _bierman_update_body(yaflInt    nx, yaflFloat * x, yaflFloat * u, \
                         yaflFloat * d, yaflFloat * f, yaflFloat * v, \
                         yaflFloat   r, yaflFloat  nu, yaflFloat  ac, \
-                        yaflFloat   gdot)
+                        yaflFloat   gdot, yaflFloat * l)
 {
     yaflStatusEn status = YAFL_ST_OK;
     yaflInt j;
@@ -313,6 +321,10 @@ static inline yaflStatusEn \
 #undef  p /*Don't need p any more...*/
         r = a;
     }
+
+    /*Compute log likelihood*/
+    *l += nu * (nu / r) + YAFL_LOG(r);
+
     /*
     Now we must do:
     x += K * nu
@@ -388,7 +400,7 @@ yaflStatusEn yafl_ekf_bierman_update_scalar(yaflKalmanBaseSt * self, yaflInt i)
     YAFL_TRY(status, YAFL_MATH_SET_DV(_NX, v, _DP, f));
     YAFL_TRY(status, \
              _bierman_update_body(_NX, _X, _UP, _DP, f, v, _DR[i], _Y[i], \
-                                  1.0, 1.0));
+                                  1.0, 1.0, _L));
     /*Update Q if needed!*/
     _Q_SCALAR_UPDATE(_QFF, v);
 #   undef v /*Don't nee v any more*/
@@ -405,7 +417,7 @@ static inline yaflStatusEn \
                         yaflFloat * d, yaflFloat * f, yaflFloat * v, \
                         yaflFloat * k, yaflFloat * w, yaflFloat  nu, \
                         yaflFloat  r, yaflFloat   s, yaflFloat  ac, \
-                        yaflFloat  gdot)
+                        yaflFloat  gdot, yaflFloat * l)
 {
     yaflStatusEn status = YAFL_ST_OK;
 
@@ -446,6 +458,9 @@ static inline yaflStatusEn \
     /* Up, Dp = MWGSU(W, D)*/
     YAFL_TRY(status, yafl_math_mwgsu(nx, nx1, u, d, w, D));
 
+    /*Compute log likelihood*/
+    *l += nu * (nu / s) + YAFL_LOG(s);
+
     /* x += k * nu */
 #   define j nx1
     for (j=0; j < nx; j++)
@@ -501,7 +516,7 @@ yaflStatusEn yafl_ekf_joseph_update_scalar(yaflKalmanBaseSt * self, yaflInt i)
 #   define K h /*Don't need h any more, use it to store K*/
     YAFL_TRY(status, \
              _joseph_update_body(_NX, _X, _UP, _DP, f, v, K, _W, _Y[i], r, \
-                                 s, 1.0, 1.0));
+                                 s, 1.0, 1.0, _L));
 
     /*Update Q if needed!*/
     _Q_SCALAR_UPDATE(_QFF, K);
@@ -592,7 +607,7 @@ yaflStatusEn yafl_ekf_adaptive_bierman_update_scalar(yaflKalmanBaseSt * self, \
                                   ((yaflEKFAdaptiveSt *)self)->chi2));
 
     YAFL_TRY(status, \
-             _bierman_update_body(_NX, _X, _UP, _DP, f, v, r, nu, ac, 1.0));
+             _bierman_update_body(_NX, _X, _UP, _DP, f, v, r, nu, ac, 1.0, _L));
 
     /*Update Q if needed!*/
     _Q_SCALAR_UPDATE(_QFF, v);
@@ -641,7 +656,7 @@ yaflStatusEn \
 #   define K h /*Don't need h any more, use it to store K*/
     YAFL_TRY(status, \
              _joseph_update_body(_NX, _X, _UP, _DP, f, v, K, _W, nu, r, s, \
-                                 ac, 1.0));
+                                 ac, 1.0, _L));
 
     /*Update Q if needed!*/
     _Q_SCALAR_UPDATE(_QFF, K);
@@ -748,6 +763,9 @@ yaflStatusEn \
     /* Up, Dp = MWGSU(W, D)*/
     YAFL_TRY(status, yafl_math_mwgsu(nx, nx1, u, d, w, D));
 
+    /*Compute log likelihood*/
+    *self->l += nu * (nu / s) + YAFL_LOG(s);
+
     /* self.x += K * nu */
     YAFL_TRY(status, yafl_math_add_vxn(nx, _X, K, nu));
 #undef D /*Don't nee D any more*/
@@ -836,7 +854,7 @@ yaflStatusEn \
 
     YAFL_TRY(status, \
              _bierman_update_body(_NX, _X, _UP, _DP, f, v, r, nu, \
-                                  1.0, gdot));
+                                  1.0, gdot, _L));
 
     /*Update Q if needed!*/
     _Q_SCALAR_UPDATE(_QFF, v);
@@ -886,7 +904,7 @@ yaflStatusEn \
 #   define K h /*Don't need h any more, use it to store K*/
     YAFL_TRY(status, \
              _joseph_update_body(_NX, _X, _UP, _DP, f, v, K, _W, nu, r, s, \
-                                 1.0, gdot));
+                                 1.0, gdot, _L));
 
     /*Update Q if needed!*/
     _Q_SCALAR_UPDATE(_QFF, K);
@@ -932,7 +950,7 @@ yaflStatusEn \
                                   ((yaflEKFAdaptiveRobustSt *)self)->chi2));
 
     YAFL_TRY(status, _bierman_update_body(_NX, _X, _UP, _DP, f, v, r, nu, \
-                                          ac, gdot));
+                                          ac, gdot, _L));
 
     /*Update Q if needed!*/
     _Q_SCALAR_UPDATE(_QFF, v);
@@ -984,7 +1002,7 @@ yaflStatusEn \
 #   define K h /*Don't need h any more, use it to store K*/
     YAFL_TRY(status, \
              _joseph_update_body(_NX, _X, _UP, _DP, f, v, K, _W, nu, r, s, \
-                                 ac, gdot));
+                                 ac, gdot, _L));
 
     /*Update Q if needed!*/
     _Q_SCALAR_UPDATE(_QFF, K);
@@ -1028,6 +1046,8 @@ yaflStatusEn \
 #define _UUR  (_KALMAN_SELF->Ur)
 #define _UDR  (_KALMAN_SELF->Dr)
 
+#define _UL   (_KALMAN_SELF->l)
+
 #define _UNX  (_KALMAN_SELF->Nx)
 #define _UNZ  (_KALMAN_SELF->Nz)
 
@@ -1475,12 +1495,18 @@ yaflStatusEn yafl_ukf_base_update(yaflUKFBaseSt * self, yaflFloat * z, \
     YAFL_TRY(status, yafl_math_ruv(nz,      _UY, _UUR));
     YAFL_TRY(status, yafl_math_rum(nz, nx, _PZX, _UUR));
 
+    /*Start log likelihood computation*/
+    *_KALMAN_SELF->l = nz * YAFL_L2PI;
+
     /*Now we can do scalar updates*/
     for (i = 0; i < nz; i++)
     {
         YAFL_TRY(status, scalar_update(_KALMAN_SELF, i));
     }
 
+    /*Finalize log likelihood value*/
+    *_KALMAN_SELF->l *= -0.5;
+
     YAFL_TRY(status, _yafl_ukf_compute_residual(self, z));
     return status;
 }
@@ -1527,7 +1553,7 @@ yaflStatusEn yafl_ukf_bierman_update_scalar(yaflKalmanBaseSt * self, yaflInt i)
 
     YAFL_TRY(status, \
              _bierman_update_body(nx, _X, _UP, _DP, f, v, _DR[i], _Y[i], \
-                                  1.0, 1.0));
+                                  1.0, 1.0, _UL));
 #   undef f
     return status;
 }
@@ -1565,7 +1591,7 @@ yaflStatusEn yafl_ukf_adaptive_bierman_update_scalar(yaflKalmanBaseSt * self, ya
                                   ((yaflUKFAdaptivedSt *)self)->chi2));
 
     YAFL_TRY(status, \
-             _bierman_update_body(nx, _X, _UP, _DP, f, v, r, _Y[i], ac, 1.0));
+             _bierman_update_body(nx, _X, _UP, _DP, f, v, r, _Y[i], ac, 1.0, _UL));
 #   undef f
     return status;
 }
@@ -1610,7 +1636,7 @@ yaflStatusEn yafl_ukf_robust_bierman_update_scalar(yaflKalmanBaseSt * self, \
 
     YAFL_TRY(status, \
              _bierman_update_body(nx, _X, _UP, _DP, f, v, r, nu, \
-                                  1.0, gdot));
+                                  1.0, gdot, _UL));
 #   undef f
     return status;
 }
@@ -1655,7 +1681,7 @@ yaflStatusEn \
                                   ((yaflUKFAdaptiveRobustSt *)self)->chi2));
 
     YAFL_TRY(status, \
-             _bierman_update_body(nx, _X, _UP, _DP, f, v, r, nu, ac, gdot));
+             _bierman_update_body(nx, _X, _UP, _DP, f, v, r, nu, ac, gdot, _UL));
 #   undef f
     return status;
 }
@@ -1791,6 +1817,9 @@ static inline yaflStatusEn yafl_ukf_update_epilogue(yaflUKFBaseSt * self, yaflFl
     /* Decorrelate measurements part 2*/
     YAFL_TRY(status, yafl_math_rum(nz, nx, _PZX, _UUS));
 
+    /*Start log likelihood computation*/
+    *_KALMAN_SELF->l = nz * YAFL_L2PI;
+
     /*Now we can do scalar updates*/
     for (i = 0; i < nz; i++)
     {
@@ -1813,8 +1842,14 @@ static inline yaflStatusEn yafl_ukf_update_epilogue(yaflUKFBaseSt * self, yaflFl
         Up, Dp = udu(P)
         */
         YAFL_TRY(status, yafl_math_udu_down(nx, _UUP, _UDP, 1.0 / ds[i], pzxi));
+
+        /*Compute log likelihood*/
+        *_KALMAN_SELF->l += y[i] * (y[i] / ds[i]) + YAFL_LOG(ds[i]);
     }
 
+    /*Finalize log likelihood value*/
+    *_KALMAN_SELF->l *= -0.5;
+
     YAFL_TRY(status, _yafl_ukf_compute_residual(self, z));
     return status;
 }
@@ -2204,6 +2239,8 @@ const yaflUKFSigmaMethodsSt yafl_ukf_julier_spm =
 #undef _UUR
 #undef _UDR
 
+#undef _UL
+
 #undef _UNX
 #undef _UNZ
 
@@ -2238,5 +2275,7 @@ const yaflUKFSigmaMethodsSt yafl_ukf_julier_spm =
 #undef _UR
 #undef _DR
 
+#undef _L
+
 #undef _NX
 #undef _NZ

src/yafl.h

@@ -64,6 +64,8 @@ struct _yaflKalmanBaseSt {
     yaflFloat * Ur; /*Upper triangular part of R*/
     yaflFloat * Dr; /*Diagonal part of R*/
 
+    yaflFloat * l;  /*likelihood*/
+
     yaflFloat rff;  /*R forgetting factor*/
 
     yaflInt   Nx;   /*State vector size*/
@@ -82,7 +84,8 @@ struct _yaflKalmanBaseSt {
     yaflFloat Dq[nx];                         \
                                               \
     yaflFloat Ur[((nz - 1) * nz)/2];          \
-    yaflFloat Dr[nz]
+    yaflFloat Dr[nz];                         \
+    yaflFloat l
 
 /*---------------------------------------------------------------------------*/
 /*TODO: make qff and rff parameters*/
@@ -106,6 +109,8 @@ struct _yaflKalmanBaseSt {
                                                                          \
     .rff = _rff,                                                         \
                                                                          \
+    .l   = &_mem.l,                                                      \
+                                                                         \
     .Nx  = _nx,                                                          \
     .Nz  = _nz                                                           \
 }

src/yafl_math.h

@@ -25,9 +25,9 @@
 do {                                                                       \
     if (YAFL_UNLIKELY(!(cond)))                                            \
     {                                                                      \
-        YAFL_LOG("YAFL:The expression (%s) is false in \n function: %s",   \
+        YAFL_DBG("YAFL:The expression (%s) is false in \n function: %s",   \
                  #cond, func);                                             \
-        YAFL_LOG("\n file: %s\n line: %d\n will return: %s\n",             \
+        YAFL_DBG("\n file: %s\n line: %d\n will return: %s\n",             \
                  file, line, #err);                                        \
         return err;                                                        \
     }                                                                      \
@@ -35,6 +35,9 @@ do {                                                                       \
 
 #define YAFL_CHECK(cond, err) _YAFL_CHECK(cond, err, __FILE__, __func__, __LINE__)
 
+/*np.log(2. * np.pi)*/
+#define YAFL_L2PI (1.8378770664093453)
+
 typedef enum {
     /*Warning flag masks*/
     YAFL_ST_MSK_REGULARIZED  = 0x01,
@@ -81,9 +84,9 @@ do {                                                                          \
     (status) |= (exp);                                                        \
     if (YAFL_UNLIKELY(YAFL_ST_ERR_THR <= (status)))                           \
     {                                                                         \
-        YAFL_LOG("YAFL:The expression (%s) gave an error in \n function: %s", \
+        YAFL_DBG("YAFL:The expression (%s) gave an error in \n function: %s", \
                  #exp, func);                                                 \
-        YAFL_LOG("\n file: %s\n line: %d\n will return: %d\n",                \
+        YAFL_DBG("\n file: %s\n line: %d\n will return: %d\n",                \
                  file, line, status);                                         \
         return status;                                                        \
     }                                                                         \

src/yaflpy/yafl_config.h

@@ -22,7 +22,7 @@
 #include <stdint.h>
 #include <stdio.h>
 
-#define YAFL_LOG(...) fprintf(stderr, __VA_ARGS__)
+#define YAFL_DBG(...) fprintf(stderr, __VA_ARGS__)
 
 typedef int32_t yaflInt;
 
@@ -35,11 +35,16 @@ typedef int32_t yaflInt;
 #   define YAFL_EPS  (1.0e-15)
 #   define YAFL_SQRT sqrt
 #   define YAFL_ABS  fabs
+#   define YAFL_ABS  fabs
+#   define YAFL_EXP  exp
+#   define YAFL_LOG  log
 #else/*YAFL_USE_64_BIT*/
     typedef float  yaflFloat;
 #   define YAFL_EPS  (1.0e-6)
 #   define YAFL_SQRT sqrtf
 #   define YAFL_ABS  fabsf
+#   define YAFL_EXP  expf
+#   define YAFL_LOG  logf
 #endif/*YAFL_USE_64_BIT*/
 
 #ifdef __GNUC__

src/yaflpy/yaflpy.pyx

@@ -211,6 +211,8 @@ cdef extern from "yafl.c":
         yaflFloat * Ur   #
         yaflFloat * Dr   #
 
+        yaflFloat * l    #
+
         yaflFloat rff    #
 
         yaflInt   Nx     #
@@ -2256,6 +2258,8 @@ cdef class yaflKalmanBase:
     # Kalman filter C-self
     cdef yaflPyKalmanBaseSt c_self
 
+    cdef yaflFloat _l
+
     # Kalman filter memory views
     cdef yaflFloat [::1]    v_x
     cdef yaflFloat [::1]    v_y
@@ -2301,6 +2305,9 @@ cdef class yaflKalmanBase:
         self.c_self.base.base.Nx = dim_x
         self.c_self.base.base.Nz = dim_z
 
+        #Set likelihood storage
+        self.c_self.base.base.l = &self._l
+
         #Set forgetting factors
         self.c_self.base.base.rff = 0.0
 
@@ -2432,6 +2439,15 @@ cdef class yaflKalmanBase:
     def Dr(self, value):
         self._Dr[:] = value
 
+    #--------------------------------------------------------------------------
+    @property
+    def l(self):
+        return self._l
+
+    @l.setter
+    def l(self, value):
+        raise AttributeError('yaflKalmanBase does not support this!')
+
     #--------------------------------------------------------------------------
     @property
     def P(self):