Merge branch 'master' of https://github.com/shkolnick-kun/yafl.git

README.md

@@ -11,7 +11,10 @@ There are also libraries for python:
 * [pykalman](https://github.com/pykalman/pykalman).
 
 ## The library
-In our you can find some **Kalman filter** variants:
+Technically speaking all filters in YAFL are adaptive since all of them have at least a measurement noice covariance adjustment.
+The term **Adaptive** is used in our docs for **Kalman filter** variants with H-infinity divergence correction.
+
+In YAFL you can find these **Kalman filter** variants:
 
 | Algorithm family | Basic        | Adaptive     | Robust       | Adaptive robust |
 | :--------------- | :----------: | :----------: | :----------: | --------------: |
@@ -33,12 +36,21 @@ For sequential UD-factorized **UKF** only **Bierman** updates have been implemen
 
 And yes, we [**can actually**](./doc/UsingEKFTricksWithSPKF.pdf) use **EKF** tricks with **UKF**!
 
+## Notes on process and measurement noice covariance adjustments
+We used [this paper](https://arxiv.org/pdf/1702.00884.pdf) to implement optional Q and R adaptive adjustments.
+Here are som notes on our implementation:
+* All filters in this lib have the optional measurement noice covariance adjustment which can be enabled by setting `rff` to a small positive number e.g. `1e-4`.
+* All EKF filters in this lib have the optional process noice covariance adjustment which can be enabled by setting `qff` to a small positive number e.g. `1e-4`.
+* None of UKF filters have the optional process noice covariance adjustment as it leads to filters instability.
+
+## Notes on implementation
 The library is written in C and is intended for embedded systems usage:
 * We use static memory allocation
 * We use cache-friendly algorithms when available.
 * Regularization techniques are used when necessary. The code is numerically stable.
 * Depends only on C standard library.
 
+## Using with C
 To use the library you need to:
 * go to our [Releases page](https://github.com/shkolnick-kun/yafl/releases),
 * download and unpack the latest release source code archive,

doc/C-Manual.md

@@ -238,8 +238,8 @@ kfMemorySt kf_memory =
 };
 
 /*This is EKF structure definition*/
-/*                                           fx  jfx  hx  jhx zrf nx  nz   memory */
-yaflEKFBaseSt kf = YAFL_EKF_BASE_INITIALIZER(fx, jfx, hx, jhx, 0, NX, NZ, kf_memory);
+/*                                           fx  jfx  hx  jhx zrf nx  nz  rff  qff  memory */
+yaflEKFBaseSt kf = YAFL_EKF_BASE_INITIALIZER(fx, jfx, hx, jhx, 0, NX, NZ, 0.0, 0.0, kf_memory);
 
 /*
 Arguments of initializer macro:
@@ -250,6 +250,8 @@ jhx    - measurement Jacobian function pointer
 zrf    - measurement Residual function pointer (needed to calculate the distance between forecast and measurement vectors)
 nx     - the dimension of state vector
 nz     - the dimension of measurement vector
+rff    - measurement noice covatiance forgetting factor
+qff    - process noice covatiance forgetting factor
 memory - the name of a memory structure.
 */
 ```
@@ -300,7 +302,7 @@ Filter algorithms and data structures are declared in [src/yafl.h](./src/yafl.h)
 To power the filtering code we need some math functions which are declared in [src/yafl_math](./src/yafl_math.h).
 
 Functions declared in [src/yafl.h](./src/yafl.h) and [src/yafl_math](./src/yafl_math.h) return result code, defined in `yaflStatusEn`.
-If everything is OK then `YAFL_ST_OK = 0` is returned. If something went wrong the result code is actually a bit mask. 
+If everything is OK then `YAFL_ST_OK = 0` is returned. If something went wrong the result code is actually a bit mask.
 Here is the bit field list:
 * Warning bit fields:
   * `YAFL_ST_MSK_REGULARIZED`  - we had to do regularization to maintain the filter integrity
@@ -341,7 +343,7 @@ To disthiguish between warinings and errors `YAFL_ST_ERR_THR` is used, e.g.:
 We prefer using static memory allocation so filter structure initializers look like this:
 ```C
 /*yaflKalmanBaseSt is for internal usage only, so our code axemple is about EKF*/
-yaflEKFBaseSt kf = YAFL_EKF_BASE_INITIALIZER(fx, jfx, hx, jhx, zrf, nx, nz, memory);
+yaflEKFBaseSt kf = YAFL_EKF_BASE_INITIALIZER(fx, jfx, hx, jhx, zrf, nx, nz, rff, qff, memory);
 ```
 where:
 * `fx`     - state transition function pointer
@@ -351,6 +353,8 @@ where:
 * `zrf`    - measurement Residual function pointer (needed to calculate the distance between forecast and measurement vectors)
 * `nx`     - the dimension of state vector
 * `nz`     - the dimension of measurement vector
+* `rff`    - measurement noice covatiance forgetting factor
+* `qff`    - process noice covatiance forgetting factor
 * `memory` - the name of the memory structure.
 
 The filter control block does not store an data it has only pointers to data storage, so we must have som separate data storage called `memory`.
@@ -367,12 +371,19 @@ The function `zrf` must have the prototype which must be similar to:
 
 it is utilized to compute the error vectors as there are some cases when we can't use the Euclid distance.
 
-As you can see all functions have `yaflKalmanBaseSt * self` as their first parameter this is done to enable passing additional data to these functions. 
+As you can see all functions have `yaflKalmanBaseSt * self` as their first parameter this is done to enable passing additional data to these functions.
 So all these functions are **"methods"** of some **"class"** which has `yaflKalmanBaseSt` as its **"basic class"**. As you can see we are using **OOP** approach here.
 
 Now let's talk about memory. We use **mixins** to declare filter memory pools so users can put more fields at their filter memory pools.
 This also enables us to do nested **mixin** macros calls while keeping memory pool strustures flat.
 
+### Notes on Q and R adjustments
+We used [this paper](https://arxiv.org/pdf/1702.00884.pdf) to implement optional Q and R adaptive adjustments.
+Here are som notes on our implementation:
+* All filters in this lib have the optional measurement noice covariance adjustment which can be enabled by setting `rff` to a small positive number e.g. `1e-4`.
+* All EKF filters in this lib have the optional process noice covariance adjustment which can be enabled by setting `qff` to a small positive number e.g. `1e-4`.
+* None of UKF filters have the optional process noice covariance adjustment as it leads to filters instability.
+
 ### EKF stuff
 The basic type for all **EKF** control blocks is `yaflEKFBaseSt`. It is initialized with `YAFL_EKF_BASE_INITIALIZER` macro.
 In the example above we gave the initial explanation of its work. Now let's go deeper.
@@ -584,7 +595,7 @@ someMemorySt memory =
 #### Basic EKF variants
 Basic **EKF** control block can be initialized with:
 ```C
-yaflEKFBaseSt kf = YAFL_EKF_BASE_INITIALIZER(fx, jfx, hx, jhx, zrf, nx, nz, memory);
+yaflEKFBaseSt kf = YAFL_EKF_BASE_INITIALIZER(fx, jfx, hx, jhx, zrf, nx, nz, rff, qff, memory);
 ```
 
 Predict macros are:
@@ -628,7 +639,7 @@ The memory mixin used is `YAFL_EKF_BASE_MEMORY_MIXIN`.
 
 The initilizer macro is:
 ```C
-YAFL_EKF_ADAPTIVE_INITIALIZER(fx, jfx, hx, jhx, zrf, nx, nz, memory)
+YAFL_EKF_ADAPTIVE_INITIALIZER(fx, jfx, hx, jhx, zrf, nx, nz, rff, qff, chi2, memory)
 ```
 
 As you can see it takes the same parameter list as `YAFL_EKF_BASE_INITIALIZER`. the `chi2` field is set to `scipy.stats.chi2.ppf(0.999, 1)`
@@ -673,7 +684,7 @@ The noise model is Normal with Poisson outliers.
 See:
 
 A.V. Chernodarov, G.I. Djandjgava and A.P. Rogalev (1999).
-Monitoring and adaptive robust protection of the integrity of air data 
+Monitoring and adaptive robust protection of the integrity of air data
 inertial satellite navigation systems for maneuverable aircraft.
 
 In: Proceedings of the RTO SCI International Conference on Integrated Navigation Systems,
@@ -683,12 +694,13 @@ held at "Electropribor", St. Petersburg, pp. 2111-10, RTO-MP-43 , NeuiIly-sur-Se
 yaflFloat psi(yaflKalmanBaseSt * self, yaflFloat normalized_error)
 {
     (void)self;
-    if (3.0 >= normalized_error)
+    yaflFloat abs_ne = fabs(normalized_error);
+    if (3.0 >= abs_ne)
     {
         return normalized_error;
     }
 
-    if (6.0 >= normalized_error)
+    if (6.0 >= abs_ne)
     {
         /*Small glitch*/
         return normalized_error / 3.0;
@@ -701,12 +713,13 @@ yaflFloat psi(yaflKalmanBaseSt * self, yaflFloat normalized_error)
 yaflFloat psi_dot(yaflKalmanBaseSt * self, yaflFloat normalized_error)
 {
     (void)self;
-    if (3.0 >= normalized_error)
+    yaflFloat abs_ne = fabs(normalized_error);
+    if (3.0 >= abs_ne)
     {
         return 1.0;
     }
 
-    if (6.0 >= normalized_error)
+    if (6.0 >= abs_ne)
     {
         /*Small glitch*/
         return 1.0 / 3.0;
@@ -734,7 +747,7 @@ The memory mixin used is `YAFL_EKF_BASE_MEMORY_MIXIN`.
 
 The initilizer macro is:
 ```C
-YAFL_EKF_ROBUST_INITIALIZER(fx, jfx, hx, jhx, zrf, g, gdot,  nx, nz, memory)
+YAFL_EKF_ROBUST_INITIALIZER(fx, jfx, hx, jhx, zrf, g, gdot, nx, nz, rff, qff, memory)
 ```
 
 Note that we have two additional arguments:
@@ -743,7 +756,7 @@ Note that we have two additional arguments:
 
 The example for `YAFL_EKF_ROBUST_INITIALIZER` call is:
 ```C
-yaflEKFRobustSt kf = YAFL_EKF_ROBUST_INITIALIZER(fx, jfx, hx, jhx, zrf, psi, psi_dot,  nx, nz, memory);
+yaflEKFRobustSt kf = YAFL_EKF_ROBUST_INITIALIZER(fx, jfx, hx, jhx, zrf, psi, psi_dot, nx, nz, rff, qff, memory);
 ```
 
 Predict macros are:
@@ -786,7 +799,7 @@ The memory mixin used is `YAFL_EKF_BASE_MEMORY_MIXIN`.
 
 The initilizer macro is:
 ```C
-YAFL_EKF_ADAPTIVE_ROBUST_INITIALIZER(fx, jfx, hx, jhx, zrf, g, gdot,  nx, nz, memory)
+YAFL_EKF_ADAPTIVE_ROBUST_INITIALIZER(fx, jfx, hx, jhx, zrf, g, gdot, nx, nz, rff, qff, chi2, memory)
 ```
 
 As you can see it takes the same parameter list as `YAFL_EKF_ROBUST_INITIALIZER`. the `chi2` field is set to `scipy.stats.chi2.ppf(0.997, 1)`
@@ -823,7 +836,7 @@ The basic type for all **UKF** control blocks is `yaflUKFBaseSt`.
 The memory mixin used is `YAFL_UKF_BASE_MEMORY_MIXIN` which is similar to other memory mixins in YAFL.
 
 ```C
-YAFL_UKF_BASE_INITIALIZER(points, points_methods, fx, xmf, xrf, hx, zmf, zrf, nx, nz, memory)
+YAFL_UKF_BASE_INITIALIZER(points, points_methods, fx, xmf, xrf, hx, zmf, zrf, nx, nz, rff, memory)
 ```
 It is quite similar to **EKF** base initializer but there are no Jacobian function pointers and there are som new parameters:
 * `points`         - a pointer to sigma point generator object
@@ -832,6 +845,8 @@ It is quite similar to **EKF** base initializer but there are no Jacobian functi
 * `xrf`            - a pointer to a state residual function
 * `zmf`            - a ponter to a measurement mean function
 
+**Note that we don't have `qff` parameter since `Q` adjustment leads to numerical instability of Unscented filter variants!**
+
 #### Siama points generators
 Sigma point generator is runtime extension to `yaflUKFBaseSt` data type.
 We desided to add such runtime extension to **UKF** types because user may
@@ -1115,8 +1130,8 @@ extern yaflStatusEn  hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
 extern yaflStatusEn zmf(yaflKalmanBaseSt * self, yaflFloat *result, yaflFloat *sigmas_z);
 extern yaflStatusEn zrf(yaflKalmanBaseSt * self, yaflFloat *result, yaflFloat *a, yaflFloat *b)
 
-/*                                         points   points_metods    fx  xmf  xrf  hx  zmf  zrf  nx  nz   memory */
-yaflEKFBaseSt kf = YAFL_UKF_BASE_INITIALIZER(sp, yafl_ukf_merwe_spm, fx, xmf, xrf, hx, zmf, zrf, NX, NX, ukf_memory);
+/*                                         points   points_metods    fx  xmf  xrf  hx  zmf  zrf  nx  nz  rff  memory */
+yaflEKFBaseSt kf = YAFL_UKF_BASE_INITIALIZER(sp, yafl_ukf_merwe_spm, fx, xmf, xrf, hx, zmf, zrf, NX, NX, 0.0, ukf_memory);
 
 ```
 
@@ -1201,7 +1216,7 @@ The memory mixin used is `YAFL_UKF_BASE_MEMORY_MIXIN` which is similar to other
 
 The initializer used is:
 ```C
-yaflEKFBaseSt kf = YAFL_UKF_ROBUST_INITIALIZER(points, points_methods, fx, xmf, xrf, hx, zmf, zrf, g, gdot, nx, nz, memory);
+yaflEKFBaseSt kf = YAFL_UKF_ROBUST_INITIALIZER(points, points_methods, fx, xmf, xrf, hx, zmf, zrf, g, gdot, nx, nz, rff, memory);
 ```
 
 where:
@@ -1223,8 +1238,8 @@ extern yaflFloat psi(yaflKalmanBaseSt * self, yaflFloat normalized_error);
 extern yaflFloat psi_dot(yaflKalmanBaseSt * self, yaflFloat normalized_error);
 
 /*The filter is initiated differently from basic UKF:*/
-/*                                           points   points_metods    fx  xmf  xrf  hx  zmf  zrf   g     gdot   nx  nz   memory */
-yaflEKFBaseSt kf = YAFL_UKF_ROBUST_INITIALIZER(sp, yafl_ukf_merwe_spm, fx, xmf, xrf, hx, zmf, zrf, psi, psi_dot, NX, NX, ukf_memory);
+/*                                           points   points_metods    fx  xmf  xrf  hx  zmf  zrf   g     gdot   nx  nz  rff  memory */
+yaflEKFBaseSt kf = YAFL_UKF_ROBUST_INITIALIZER(sp, yafl_ukf_merwe_spm, fx, xmf, xrf, hx, zmf, zrf, psi, psi_dot, NX, NX, 0.0, ukf_memory);
 ```
 
 #### Adaptive robust Bierman UKF
@@ -1271,8 +1286,8 @@ The memory mixin used is `YAFL_UKF_MEMORY_MIXIN` which is similar to other memor
 
 The initializer used is: `YAFL_UKF_INITIALIZER`
 ```C
-/*                                                  points   points_metods    fx  xmf  xrf  hx  zmf  zrf  nx  nz  chi2    memory */
-yaflEKFBaseSt kf = YAFL_UKF_FULL_ADAPTIVE_INITIALIZER(sp, yafl_ukf_merwe_spm, fx, xmf, xrf, hx, zmf, zrf, NX, NX, chi2, ukf_memory);
+/*                                                  points   points_metods    fx  xmf  xrf  hx  zmf  zrf  nx  nz  rff, chi2    memory */
+yaflEKFBaseSt kf = YAFL_UKF_FULL_ADAPTIVE_INITIALIZER(sp, yafl_ukf_merwe_spm, fx, xmf, xrf, hx, zmf, zrf, NX, NX, rff, chi2, ukf_memory);
 ```
 where `chi2` must be set to `scipy.stats.chi2.ppf(1.0 - some_small_number, NZ)` as we can not predict `chi2` for specific `nz` at compile time using C-preprocessor.
 

doc/Python-Manual.md

@@ -103,6 +103,7 @@ The Kalman filter has these attrbutes:
 * `P = Up.dot(Dp.dot(Up.T))` which is state covariance matrix
 * `Q = Uq.dot(Dq.dot(Uq.T))` which is process noise matrix
 * `R = Ur.dot(Dr.dot(Ur.T))` which is measurement noise matrix
+* `rff` which is `R` forgetting factor used to ajust measurement noice covariance at runtime
 
 Since YAFL implements UD-factorized filters we don't have these attributes directly, but have vectors to store UD-decomposition elements of these attributes:
 * `Up :np.array((max(1, (dim_x * (dim_x - 1))//2), ))` is vector with upper triangular elements of P
@@ -112,6 +113,13 @@ Since YAFL implements UD-factorized filters we don't have these attributes direc
 * `Ur :np.array((max(1, (dim_z * (dim_z - 1))//2), ))` is vector with upper triangular elements of R
 * `Dr :np.array((dim_z, ))` is vector with diagonal elements of R
 
+### Notes on Q and R adjustments
+We used [this paper](https://arxiv.org/pdf/1702.00884.pdf) to implement optional Q and R adaptive adjustments.
+Here are som notes on our implementation:
+* All filters in this lib have the optional measurement noice covariance adjustment which can be enabled by setting `rff` attribute to a small positive number e.g. `1e-4`.
+* All EKF filters in this lib have the optional process noice covariance adjustment which can be enabled by setting `qff` attribute to a small positive number e.g. `1e-4`.
+* None of UKF filters have the optional process noice covariance adjustment as it leads to filters instability.
+
 ### EKF stuf
 The base class for all **EKF** variants is
 ```Python
@@ -123,6 +131,8 @@ where:
 
 This class extends `yaflpy.yaflKalmanBase`. This class is not supposet to be used directly.
 
+**NOTE: In addition to `rff` all EKF variants have `qff` attribute which is `Q` forgetting factor used to ajust process noice covariance at runtime**
+
 #### Basic EKF variants
 Basic UD-factorized **EKF** variants are:
 ```Python

pyproject.toml

@@ -1,3 +1,3 @@
 [build-system]
-requires = ["setuptools", "wheel", "Cython", "numpy", "scipy"]
+requires = ["setuptools", "wheel", "Cython", "numpy", "scipy", "filterpy"]
 build-backend = "setuptools.build_meta"

requirements-dev.txt

@@ -1,5 +1,6 @@
 Cython >= 0.29.21
 numpy >= 1.17
 scipy >= 1.5
+filterpy >= 1.4.5
 setuptools >= 51.0.0
 wheel >= 0.36.2

setup.cfg

@@ -1,6 +1,6 @@
 [metadata]
 name = yaflpy
-version = 0.10.0
+version = 0.20.0
 platforms = any
 
 description = Yet Another Filtering Library

src/yafl.h

@@ -337,8 +337,9 @@ YAFL_EKF_UPDATE_IMPL(yafl_ekf_adaptive_robust_bierman_update, \
 /*-----------------------------------------------------------------------------
                            Adaptive Joseph filter
 -----------------------------------------------------------------------------*/
-#define YAFL_EKF_ADAPTIVE_ROBUST_JOSEPH_PREDICT(self) \
+#define YAFL_EKF_ADAPTIVE_ROBUST_JOSEPH_PREDICT \
     _yafl_ada_rob_predict_wrapper
+
 YAFL_EKF_UPDATE_IMPL(yafl_ekf_adaptive_robust_joseph_update, \
                      yaflEKFAdaptiveRobustSt)
 
@@ -456,10 +457,10 @@ WARNING:
     .sp_info = _p,                                                            \
     .sp_meth = _pm,                                                           \
                                                                               \
-    .xmf = (yaflUKFFuncP)_xmf,                                                \
-    .xrf = (yaflUKFResFuncP)_xrf,                                             \
+    .xmf = (yaflKalmanFuncP)_xmf,                                             \
+    .xrf = (yaflKalmanResFuncP)_xrf,                                          \
                                                                               \
-    .zmf = (yaflUKFFuncP)_zmf,                                                \
+    .zmf = (yaflKalmanFuncP)_zmf,                                             \
                                                                               \
     .zp  = _mem.zp,                                                           \
                                                                               \
@@ -570,13 +571,13 @@ typedef struct {
 } yaflUKFAdaptiveRobustSt; /*Robust EKF*/
 
 /*---------------------------------------------------------------------------*/
-#define YAFL_UKF_ADAPTIVE_ROBUST_INITIALIZER(_f, _jf, _h, _jh, _zrf,      \
-                                             _g, _gdot, _nx, _nz,         \
-                                             _rff, _chi2, _mem)           \
-{                                                                         \
-    .base = YAFL_UKF_ROBUST_INITIALIZER(_f, _jf, _h, _jh, _zrf,           \
-                                        _g, _gdot, _nx, _nz, _rff, _mem), \
-    .chi2 = _chi2                                                         \
+#define YAFL_UKF_ADAPTIVE_ROBUST_INITIALIZER(_p, _pm, _f, _xmf, _xrf, _h,        \
+                                             _zmf, _zrf, _g, _gdot, _nx,         \
+                                             _nz, _rff, _chi2, _mem)             \
+{                                                                                \
+    .base = YAFL_UKF_ROBUST_INITIALIZER(_p, _pm, _f, _xmf, _xrf, _h, _zmf, _zrf, \
+                                        _g, _gdot, _nx, _nz, _rff, _mem),        \
+    .chi2 = _chi2                                                                \
 }
 /*
 A good value of chi2 for yaflUKFAdaptiveRobustSt is:
@@ -631,13 +632,13 @@ typedef struct {
 } yaflUKFFullAdapiveSt;
 
 /*---------------------------------------------------------------------------*/
-#define YAFL_UKF_FULL_ADAPTIVE_INITIALIZER(_p, _pm, _f, _xmf, _xrf, _h,  \
-                                           _zmf, _zrf, _nx, _nz,         \
-                                           _rff, _chi2, _mem)            \
-{                                                                        \
-    .base = YAFL_UKF_BASE_INITIALIZER(_p, _pm, _f, _xmf, _xrf, _h, _zmf, \
-                                      _zrf, _nx, _nz, _rff, _mem),       \
-    .chi2 = _chi2                                                        \
+#define YAFL_UKF_FULL_ADAPTIVE_INITIALIZER(_p, _pm, _f, _xmf, _xrf, _h,   \
+                                           _zmf, _zrf, _nx, _nz,          \
+                                           _rff, _chi2, _mem)             \
+{                                                                         \
+    .base = YAFL_UKF_INITIALIZER(_p, _pm, _f, _xmf, _xrf, _h, _zmf, _zrf, \
+                                 _nx, _nz, _rff, _mem),                   \
+    .chi2 = _chi2                                                         \
 }
 
 /*---------------------------------------------------------------------------*/

tests/projects/CppCheckResults.xml

@@ -1,310 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<results version="2">
-    <cppcheck version="1.90" />
-    <errors>
-        <error id="variableScope" severity="style" msg="The scope of the variable &apos;i&apos; can be reduced." verbose="The scope of the variable &apos;i&apos; can be reduced. Warning: Be careful when fixing this message, especially when there are inner loops. Here is an example where cppcheck will write that the scope for &apos;i&apos; can be reduced:\012void f(int x)\012{\012    int i = 0;\012    if (x) {\012        // it&apos;s safe to move &apos;int i = 0;&apos; here\012        for (int n = 0; n &lt; 10; ++n) {\012            // it is possible but not safe to move &apos;int i = 0;&apos; here\012            do_something(&amp;i);\012        }\012    }\012}\012When you see this message it is always safe to reduce the variable scope 1 level." cwe="398">
-            <location file="../../src/yafl.c" line="1045" column="13" />
-            <symbol>i</symbol>
-        </error>
-        <error id="redundantAssignment" severity="style" msg="Variable &apos;resj&apos; is reassigned a value before the old one has been used." verbose="Variable &apos;resj&apos; is reassigned a value before the old one has been used." cwe="563">
-            <location file="../../src/yafl_math.c" line="223" column="1" info="resj is overwritten" />
-            <location file="../../src/yafl_math.c" line="223" column="1" info="resj is assigned" />
-            <symbol>resj</symbol>
-        </error>
-        <error id="redundantAssignment" severity="style" msg="Variable &apos;status&apos; is reassigned a value before the old one has been used." verbose="Variable &apos;status&apos; is reassigned a value before the old one has been used." cwe="563">
-            <location file="../src/hdf5utils.c" line="74" column="12" info="status is overwritten" />
-            <location file="../src/hdf5utils.c" line="73" column="12" info="status is assigned" />
-            <symbol>status</symbol>
-        </error>
-        <error id="redundantAssignment" severity="style" msg="Variable &apos;status&apos; is reassigned a value before the old one has been used." verbose="Variable &apos;status&apos; is reassigned a value before the old one has been used." cwe="563">
-            <location file="../src/hdf5utils.c" line="75" column="12" info="status is overwritten" />
-            <location file="../src/hdf5utils.c" line="74" column="12" info="status is assigned" />
-            <symbol>status</symbol>
-        </error>
-        <error id="redundantAssignment" severity="style" msg="Variable &apos;status&apos; is reassigned a value before the old one has been used." verbose="Variable &apos;status&apos; is reassigned a value before the old one has been used." cwe="563">
-            <location file="../src/hdf5utils.c" line="76" column="12" info="status is overwritten" />
-            <location file="../src/hdf5utils.c" line="75" column="12" info="status is assigned" />
-            <symbol>status</symbol>
-        </error>
-        <error id="redundantAssignment" severity="style" msg="Variable &apos;status&apos; is reassigned a value before the old one has been used." verbose="Variable &apos;status&apos; is reassigned a value before the old one has been used." cwe="563">
-            <location file="../src/hdf5utils.c" line="127" column="12" info="status is overwritten" />
-            <location file="../src/hdf5utils.c" line="126" column="12" info="status is assigned" />
-            <symbol>status</symbol>
-        </error>
-        <error id="redundantAssignment" severity="style" msg="Variable &apos;status&apos; is reassigned a value before the old one has been used." verbose="Variable &apos;status&apos; is reassigned a value before the old one has been used." cwe="563">
-            <location file="../src/hdf5utils.c" line="128" column="12" info="status is overwritten" />
-            <location file="../src/hdf5utils.c" line="127" column="12" info="status is assigned" />
-            <symbol>status</symbol>
-        </error>
-        <error id="redundantAssignment" severity="style" msg="Variable &apos;status&apos; is reassigned a value before the old one has been used." verbose="Variable &apos;status&apos; is reassigned a value before the old one has been used." cwe="563">
-            <location file="../src/hdf5utils.c" line="129" column="12" info="status is overwritten" />
-            <location file="../src/hdf5utils.c" line="128" column="12" info="status is assigned" />
-            <symbol>status</symbol>
-        </error>
-        <error id="unreadVariable" severity="style" msg="Variable &apos;status&apos; is assigned a value that is never used." verbose="Variable &apos;status&apos; is assigned a value that is never used." cwe="563">
-            <location file="../src/hdf5utils.c" line="76" column="12" />
-            <symbol>status</symbol>
-        </error>
-        <error id="unreadVariable" severity="style" msg="Variable &apos;status&apos; is assigned a value that is never used." verbose="Variable &apos;status&apos; is assigned a value that is never used." cwe="563">
-            <location file="../src/hdf5utils.c" line="129" column="12" />
-            <symbol>status</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;_yafl_ada_ekf_predict_wrapper&apos; is never used." verbose="The function &apos;_yafl_ada_ekf_predict_wrapper&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="211" column="0" />
-            <symbol>_yafl_ada_ekf_predict_wrapper</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;_yafl_ada_rob_predict_wrapper&apos; is never used." verbose="The function &apos;_yafl_ada_rob_predict_wrapper&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="308" column="0" />
-            <symbol>_yafl_ada_rob_predict_wrapper</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;_yafl_rob_ekf_predict_wrapper&apos; is never used." verbose="The function &apos;_yafl_rob_ekf_predict_wrapper&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="272" column="0" />
-            <symbol>_yafl_rob_ekf_predict_wrapper</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ekf_adaptive_bierman_update&apos; is never used." verbose="The function &apos;yafl_ekf_adaptive_bierman_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="217" column="0" />
-            <symbol>yafl_ekf_adaptive_bierman_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ekf_adaptive_joseph_update&apos; is never used." verbose="The function &apos;yafl_ekf_adaptive_joseph_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="223" column="0" />
-            <symbol>yafl_ekf_adaptive_joseph_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ekf_adaptive_robust_bierman_update&apos; is never used." verbose="The function &apos;yafl_ekf_adaptive_robust_bierman_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="314" column="0" />
-            <symbol>yafl_ekf_adaptive_robust_bierman_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ekf_adaptive_robust_joseph_update&apos; is never used." verbose="The function &apos;yafl_ekf_adaptive_robust_joseph_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="322" column="0" />
-            <symbol>yafl_ekf_adaptive_robust_joseph_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ekf_bierman_update&apos; is never used." verbose="The function &apos;yafl_ekf_bierman_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="183" column="0" />
-            <symbol>yafl_ekf_bierman_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ekf_do_not_use_this_update&apos; is never used." verbose="The function &apos;yafl_ekf_do_not_use_this_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="236" column="0" />
-            <symbol>yafl_ekf_do_not_use_this_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ekf_robust_bierman_update&apos; is never used." verbose="The function &apos;yafl_ekf_robust_bierman_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="278" column="0" />
-            <symbol>yafl_ekf_robust_bierman_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ekf_robust_joseph_update&apos; is never used." verbose="The function &apos;yafl_ekf_robust_joseph_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="284" column="0" />
-            <symbol>yafl_ekf_robust_joseph_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_add_mm&apos; is never used." verbose="The function &apos;yafl_math_add_mm&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="305" column="0" />
-            <symbol>yafl_math_add_mm</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_add_mu&apos; is never used." verbose="The function &apos;yafl_math_add_mu&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="415" column="0" />
-            <symbol>yafl_math_add_mu</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_add_mv&apos; is never used." verbose="The function &apos;yafl_math_add_mv&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="224" column="0" />
-            <symbol>yafl_math_add_mv</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_add_u&apos; is never used." verbose="The function &apos;yafl_math_add_u&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="470" column="0" />
-            <symbol>yafl_math_add_u</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_add_uv&apos; is never used." verbose="The function &apos;yafl_math_add_uv&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="372" column="0" />
-            <symbol>yafl_math_add_uv</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_add_vrn&apos; is never used." verbose="The function &apos;yafl_math_add_vrn&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="74" column="0" />
-            <symbol>yafl_math_add_vrn</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_add_vrv&apos; is never used." verbose="The function &apos;yafl_math_add_vrv&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="114" column="0" />
-            <symbol>yafl_math_add_vrv</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_add_vtm&apos; is never used." verbose="The function &apos;yafl_math_add_vtm&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="259" column="0" />
-            <symbol>yafl_math_add_vtm</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_add_vtu&apos; is never used." verbose="The function &apos;yafl_math_add_vtu&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="339" column="0" />
-            <symbol>yafl_math_add_vtu</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_add_vvt&apos; is never used." verbose="The function &apos;yafl_math_add_vvt&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="162" column="0" />
-            <symbol>yafl_math_add_vvt</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_add_vxv&apos; is never used." verbose="The function &apos;yafl_math_add_vxv&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="94" column="0" />
-            <symbol>yafl_math_add_vxv</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_badd_bu&apos; is never used." verbose="The function &apos;yafl_math_badd_bu&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="722" column="0" />
-            <symbol>yafl_math_badd_bu</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_badd_mu&apos; is never used." verbose="The function &apos;yafl_math_badd_mu&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="676" column="0" />
-            <symbol>yafl_math_badd_mu</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_badd_u&apos; is never used." verbose="The function &apos;yafl_math_badd_u&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="525" column="0" />
-            <symbol>yafl_math_badd_u</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_badd_ut&apos; is never used." verbose="The function &apos;yafl_math_badd_ut&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="582" column="0" />
-            <symbol>yafl_math_badd_ut</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_badd_v&apos; is never used." verbose="The function &apos;yafl_math_badd_v&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="601" column="0" />
-            <symbol>yafl_math_badd_v</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_badd_vvt&apos; is never used." verbose="The function &apos;yafl_math_badd_vvt&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="631" column="0" />
-            <symbol>yafl_math_badd_vvt</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_bset_mu&apos; is never used." verbose="The function &apos;yafl_math_bset_mu&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="675" column="0" />
-            <symbol>yafl_math_bset_mu</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_bsub_bu&apos; is never used." verbose="The function &apos;yafl_math_bsub_bu&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="723" column="0" />
-            <symbol>yafl_math_bsub_bu</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_bsub_mu&apos; is never used." verbose="The function &apos;yafl_math_bsub_mu&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="677" column="0" />
-            <symbol>yafl_math_bsub_mu</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_bsub_ut&apos; is never used." verbose="The function &apos;yafl_math_bsub_ut&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="583" column="0" />
-            <symbol>yafl_math_bsub_ut</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_bsub_v&apos; is never used." verbose="The function &apos;yafl_math_bsub_v&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="602" column="0" />
-            <symbol>yafl_math_bsub_v</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_bsub_vvt&apos; is never used." verbose="The function &apos;yafl_math_bsub_vvt&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="632" column="0" />
-            <symbol>yafl_math_bsub_vvt</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_set_mm&apos; is never used." verbose="The function &apos;yafl_math_set_mm&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="304" column="0" />
-            <symbol>yafl_math_set_mm</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_set_mu&apos; is never used." verbose="The function &apos;yafl_math_set_mu&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="414" column="0" />
-            <symbol>yafl_math_set_mu</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_set_mv&apos; is never used." verbose="The function &apos;yafl_math_set_mv&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="223" column="0" />
-            <symbol>yafl_math_set_mv</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_set_u&apos; is never used." verbose="The function &apos;yafl_math_set_u&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="418" column="0" />
-            <symbol>yafl_math_set_u</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_set_vvt&apos; is never used." verbose="The function &apos;yafl_math_set_vvt&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="161" column="0" />
-            <symbol>yafl_math_set_vvt</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_mm&apos; is never used." verbose="The function &apos;yafl_math_sub_mm&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="306" column="0" />
-            <symbol>yafl_math_sub_mm</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_mu&apos; is never used." verbose="The function &apos;yafl_math_sub_mu&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="416" column="0" />
-            <symbol>yafl_math_sub_mu</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_mv&apos; is never used." verbose="The function &apos;yafl_math_sub_mv&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="225" column="0" />
-            <symbol>yafl_math_sub_mv</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_u&apos; is never used." verbose="The function &apos;yafl_math_sub_u&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="471" column="0" />
-            <symbol>yafl_math_sub_u</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_uv&apos; is never used." verbose="The function &apos;yafl_math_sub_uv&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="373" column="0" />
-            <symbol>yafl_math_sub_uv</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_vrn&apos; is never used." verbose="The function &apos;yafl_math_sub_vrn&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="75" column="0" />
-            <symbol>yafl_math_sub_vrn</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_vrv&apos; is never used." verbose="The function &apos;yafl_math_sub_vrv&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="115" column="0" />
-            <symbol>yafl_math_sub_vrv</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_vtm&apos; is never used." verbose="The function &apos;yafl_math_sub_vtm&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="260" column="0" />
-            <symbol>yafl_math_sub_vtm</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_vtu&apos; is never used." verbose="The function &apos;yafl_math_sub_vtu&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="340" column="0" />
-            <symbol>yafl_math_sub_vtu</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_vvt&apos; is never used." verbose="The function &apos;yafl_math_sub_vvt&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="163" column="0" />
-            <symbol>yafl_math_sub_vvt</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_vvtxn&apos; is never used." verbose="The function &apos;yafl_math_sub_vvtxn&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="193" column="0" />
-            <symbol>yafl_math_sub_vvtxn</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_vxn&apos; is never used." verbose="The function &apos;yafl_math_sub_vxn&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="37" column="0" />
-            <symbol>yafl_math_sub_vxn</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_math_sub_vxv&apos; is never used." verbose="The function &apos;yafl_math_sub_vxv&apos; is never used." cwe="561">
-            <location file="../../src/yafl_math.c" line="95" column="0" />
-            <symbol>yafl_math_sub_vxv</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_adaptive_bierman_predict&apos; is never used." verbose="The function &apos;yafl_ukf_adaptive_bierman_predict&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="488" column="0" />
-            <symbol>yafl_ukf_adaptive_bierman_predict</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_adaptive_bierman_update&apos; is never used." verbose="The function &apos;yafl_ukf_adaptive_bierman_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="489" column="0" />
-            <symbol>yafl_ukf_adaptive_bierman_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_adaptive_predict&apos; is never used." verbose="The function &apos;yafl_ukf_adaptive_predict&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="590" column="0" />
-            <symbol>yafl_ukf_adaptive_predict</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_adaptive_robust_bierman_predict&apos; is never used." verbose="The function &apos;yafl_ukf_adaptive_robust_bierman_predict&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="534" column="0" />
-            <symbol>yafl_ukf_adaptive_robust_bierman_predict</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_adaptive_robust_bierman_update&apos; is never used." verbose="The function &apos;yafl_ukf_adaptive_robust_bierman_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="536" column="0" />
-            <symbol>yafl_ukf_adaptive_robust_bierman_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_adaptive_update&apos; is never used." verbose="The function &apos;yafl_ukf_adaptive_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.c" line="1543" column="0" />
-            <symbol>yafl_ukf_adaptive_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_bierman_predict&apos; is never used." verbose="The function &apos;yafl_ukf_bierman_predict&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="468" column="0" />
-            <symbol>yafl_ukf_bierman_predict</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_bierman_update&apos; is never used." verbose="The function &apos;yafl_ukf_bierman_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="469" column="0" />
-            <symbol>yafl_ukf_bierman_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_post_init&apos; is never used." verbose="The function &apos;yafl_ukf_post_init&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="433" column="0" />
-            <symbol>yafl_ukf_post_init</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_robust_bierman_predict&apos; is never used." verbose="The function &apos;yafl_ukf_robust_bierman_predict&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="513" column="0" />
-            <symbol>yafl_ukf_robust_bierman_predict</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_robust_bierman_update&apos; is never used." verbose="The function &apos;yafl_ukf_robust_bierman_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.h" line="514" column="0" />
-            <symbol>yafl_ukf_robust_bierman_update</symbol>
-        </error>
-        <error id="unusedFunction" severity="style" msg="The function &apos;yafl_ukf_update&apos; is never used." verbose="The function &apos;yafl_ukf_update&apos; is never used." cwe="561">
-            <location file="../../src/yafl.c" line="1395" column="0" />
-            <symbol>yafl_ukf_update</symbol>
-        </error>
-        <error id="missingIncludeSystem" severity="information" msg="Cppcheck cannot find all the include files (use --check-config for details)" verbose="Cppcheck cannot find all the include files. Cppcheck can check the code without the include files found. But the results will probably be more accurate if all the include files are found. Please check your project&apos;s include directories and add all of them as include directories for Cppcheck. To see what files Cppcheck cannot find use --check-config." />
-    </errors>
-</results>

tests/projects/ada_bierman/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="ada_bierman" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/ada_bierman.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/ada_joseph/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="ada_joseph" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/ada_joseph.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/ada_rob_bierman/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="ada_rob_bierman" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/ada_rob_bierman.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/ada_rob_joseph/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="ada_rob_joseph" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/ada_rob_joseph.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/ada_rob_ukf_bierman/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="ada_rob_ukf_bierman" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/ada_rob_ukf_bierman.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/ada_ukf/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="ada_ukf" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/ada_ukf.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/ada_ukf_bierman/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="ada_ukf_bierman" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/ada_ukf_bierman.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/bierman/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="bierman" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/bierman.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/joseph/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="joseph" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/bierman.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/rob_bierman/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="rob_bierman" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/rob_bierman.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/rob_joseph/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="rob_joseph" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/rob_joseph.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/rob_ukf_bierman/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="rob_ukf_bierman" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/rob_ukf_bierman.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/hdf5_test.cbp → tests/projects/ukf/hdf5_test.cbp

@@ -2,7 +2,7 @@
 <CodeBlocks_project_file>
 	<FileVersion major="1" minor="6" />
 	<Project>
-		<Option title="hdf5_test" />
+		<Option title="ukf" />
 		<Option pch_mode="2" />
 		<Option compiler="gcc" />
 		<Build>
@@ -27,9 +27,9 @@
 			<Add option="-fstack-usage" />
 			<Add option="-fdump-ipa-cgraph" />
 			<Add option="-DYAFL_USE_64_BIT=1" />
-			<Add directory="../src" />
 			<Add directory="../../src" />
-			<Add directory="../../src/yaflpy" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
 		</Compiler>
 		<Linker>
 			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
@@ -40,22 +40,22 @@
 			<Add library="m" />
 			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
 		</Linker>
-		<Unit filename="../../src/yafl.c">
+		<Unit filename="../../../src/yafl.c">
 			<Option compilerVar="CC" />
 		</Unit>
-		<Unit filename="../../src/yafl.h" />
-		<Unit filename="../../src/yafl_math.c">
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
 			<Option compilerVar="CC" />
 		</Unit>
-		<Unit filename="../../src/yafl_math.h" />
-		<Unit filename="../../src/yaflpy/yafl_config.h" />
-		<Unit filename="../src/hdf5utils.c">
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/ukf.c">
 			<Option compilerVar="CC" />
 		</Unit>
-		<Unit filename="../src/hdf5utils.h" />
-		<Unit filename="../src/main.c">
+		<Unit filename="../../src/hdf5utils.c">
 			<Option compilerVar="CC" />
 		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
 		<Extensions>
 			<lib_finder disable_auto="1" />
 		</Extensions>

tests/projects/ukf_bierman/hdf5_test.cbp

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="ukf_bierman" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/hdf5_test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+				</Compiler>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wcast-align" />
+			<Add option="-Wfloat-equal" />
+			<Add option="-Wunreachable-code" />
+			<Add option="-pedantic" />
+			<Add option="-Wextra" />
+			<Add option="-Wall" />
+			<Add option="-g" />
+			<Add option="-DYAFL_USE_64_BIT=1" />
+			<Add directory="../../src" />
+			<Add directory="../../../src" />
+			<Add directory="../../../src/yaflpy" />
+		</Compiler>
+		<Linker>
+			<Add library="/usr/lib/x86_64-linux-gnu/hdf5/serial/libhdf5.a" />
+			<Add library="pthread" />
+			<Add library="dl" />
+			<Add library="sz" />
+			<Add library="z" />
+			<Add library="m" />
+			<Add directory="/home/anon/Documents/Projects/hdf5_test/" />
+		</Linker>
+		<Unit filename="../../../src/yafl.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl.h" />
+		<Unit filename="../../../src/yafl_math.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../../src/yafl_math.h" />
+		<Unit filename="../../../src/yaflpy/yafl_config.h" />
+		<Unit filename="../../src/ukf_bierman.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="../../src/hdf5utils.h" />
+		<Extensions>
+			<lib_finder disable_auto="1" />
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

tests/projects/yafl_hdf_tests.workspace

@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_workspace_file>
+	<Workspace title="Workspace">
+		<Project filename="joseph/hdf5_test.cbp" />
+		<Project filename="bierman/hdf5_test.cbp" />
+		<Project filename="ada_bierman/hdf5_test.cbp" />
+		<Project filename="ada_joseph/hdf5_test.cbp" />
+		<Project filename="ada_rob_bierman/hdf5_test.cbp" />
+		<Project filename="ada_rob_joseph/hdf5_test.cbp" />
+		<Project filename="rob_bierman/hdf5_test.cbp" />
+		<Project filename="rob_joseph/hdf5_test.cbp" />
+	</Workspace>
+</CodeBlocks_workspace_file>

tests/src/ada_bierman.c

@@ -0,0 +1,191 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jfx(yaflKalmanBaseSt * self, yaflFloat * w, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nx2;
+
+    (void)x;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(w,             YAFL_ST_INV_ARG_2);
+
+    nx  = self->Nx;
+    nx2 = nx * 2;
+
+    for (i = 0; i < nx; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx2 * i;
+        for (j = 0; j < nx; j++)
+        {
+            w[nci + j] = (i != j) ? 0.0 : 1.0;
+        }
+    }
+
+    w[nx2*0 + 2] = 0.1;
+    w[nx2*1 + 3] = 0.1;
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jhx(yaflKalmanBaseSt * self, yaflFloat * h, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nz;
+
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(h,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    nx = self->Nx;
+    nz = self->Nz;
+
+    for (i = 0; i < nz; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx * i;
+        for (j = 0; j < nx; j++)
+        {
+            h[nci + j] = 0.0;
+        }
+    }
+
+    h[nx*0 + 0] = 1.0;
+    h[nx*1 + 1] = 1.0;
+    return YAFL_ST_OK;
+}
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_EKF_BASE_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflEKFAdaptiveSt kf = YAFL_EKF_ADAPTIVE_INITIALIZER(fx, jfx, hx, jhx, 0, NX, NZ, 0.0, 0.0, 10.827566170662733, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        YAFL_EKF_ADAPTIVE_BIERAMN_PREDICT(&kf);
+        yafl_ekf_adaptive_bierman_update(&kf, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/ada_joseph.c

@@ -0,0 +1,191 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jfx(yaflKalmanBaseSt * self, yaflFloat * w, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nx2;
+
+    (void)x;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(w,             YAFL_ST_INV_ARG_2);
+
+    nx  = self->Nx;
+    nx2 = nx * 2;
+
+    for (i = 0; i < nx; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx2 * i;
+        for (j = 0; j < nx; j++)
+        {
+            w[nci + j] = (i != j) ? 0.0 : 1.0;
+        }
+    }
+
+    w[nx2*0 + 2] = 0.1;
+    w[nx2*1 + 3] = 0.1;
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jhx(yaflKalmanBaseSt * self, yaflFloat * h, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nz;
+
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(h,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    nx = self->Nx;
+    nz = self->Nz;
+
+    for (i = 0; i < nz; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx * i;
+        for (j = 0; j < nx; j++)
+        {
+            h[nci + j] = 0.0;
+        }
+    }
+
+    h[nx*0 + 0] = 1.0;
+    h[nx*1 + 1] = 1.0;
+    return YAFL_ST_OK;
+}
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_EKF_BASE_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflEKFAdaptiveSt kf = YAFL_EKF_ADAPTIVE_INITIALIZER(fx, jfx, hx, jhx, 0, NX, NZ, 0.0, 0.0, 10.827566170662733, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        YAFL_EKF_ADAPTIVE_JOSEPH_PREDICT(&kf);
+        yafl_ekf_adaptive_joseph_update(&kf, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/ada_rob_bierman.c

@@ -0,0 +1,228 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jfx(yaflKalmanBaseSt * self, yaflFloat * w, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nx2;
+
+    (void)x;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(w,             YAFL_ST_INV_ARG_2);
+
+    nx  = self->Nx;
+    nx2 = nx * 2;
+
+    for (i = 0; i < nx; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx2 * i;
+        for (j = 0; j < nx; j++)
+        {
+            w[nci + j] = (i != j) ? 0.0 : 1.0;
+        }
+    }
+
+    w[nx2*0 + 2] = 0.1;
+    w[nx2*1 + 3] = 0.1;
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jhx(yaflKalmanBaseSt * self, yaflFloat * h, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nz;
+
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(h,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    nx = self->Nx;
+    nz = self->Nz;
+
+    for (i = 0; i < nz; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx * i;
+        for (j = 0; j < nx; j++)
+        {
+            h[nci + j] = 0.0;
+        }
+    }
+
+    h[nx*0 + 0] = 1.0;
+    h[nx*1 + 1] = 1.0;
+    return YAFL_ST_OK;
+}
+
+/*---------------------------------------------------------------------------*/
+yaflFloat g(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return x;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return x/3.0;
+    }
+
+    return (x > 0.0) ? 1.0 : -1.0;
+}
+
+yaflFloat gdot(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return 1.0;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return 1.0/3.0;
+    }
+
+    return 0.0;
+}
+
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_EKF_BASE_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflEKFAdaptiveRobustSt kf = YAFL_EKF_ADAPTIVE_ROBUST_INITIALIZER(fx, jfx, hx, jhx, 0, g, gdot, NX, NZ, 0.0, 0.0, 8.807468393511947, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        YAFL_EKF_ADAPTIVE_ROBUST_BIERAMN_PREDICT(&kf);
+        yafl_ekf_adaptive_robust_bierman_update(&kf, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.base.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.base.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/ada_rob_joseph.c

@@ -0,0 +1,228 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jfx(yaflKalmanBaseSt * self, yaflFloat * w, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nx2;
+
+    (void)x;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(w,             YAFL_ST_INV_ARG_2);
+
+    nx  = self->Nx;
+    nx2 = nx * 2;
+
+    for (i = 0; i < nx; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx2 * i;
+        for (j = 0; j < nx; j++)
+        {
+            w[nci + j] = (i != j) ? 0.0 : 1.0;
+        }
+    }
+
+    w[nx2*0 + 2] = 0.1;
+    w[nx2*1 + 3] = 0.1;
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jhx(yaflKalmanBaseSt * self, yaflFloat * h, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nz;
+
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(h,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    nx = self->Nx;
+    nz = self->Nz;
+
+    for (i = 0; i < nz; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx * i;
+        for (j = 0; j < nx; j++)
+        {
+            h[nci + j] = 0.0;
+        }
+    }
+
+    h[nx*0 + 0] = 1.0;
+    h[nx*1 + 1] = 1.0;
+    return YAFL_ST_OK;
+}
+
+/*---------------------------------------------------------------------------*/
+yaflFloat g(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return x;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return x/3.0;
+    }
+
+    return (x > 0.0) ? 1.0 : -1.0;
+}
+
+yaflFloat gdot(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return 1.0;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return 1.0/3.0;
+    }
+
+    return 0.0;
+}
+
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_EKF_BASE_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflEKFAdaptiveRobustSt kf = YAFL_EKF_ADAPTIVE_ROBUST_INITIALIZER(fx, jfx, hx, jhx, 0, g, gdot, NX, NZ, 0.0, 0.0, 8.807468393511947, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        YAFL_EKF_ADAPTIVE_ROBUST_JOSEPH_PREDICT(&kf);
+        yafl_ekf_adaptive_robust_joseph_update(&kf, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.base.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.base.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/ada_rob_ukf_bierman.c

@@ -0,0 +1,169 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+/*---------------------------------------------------------------------------*/
+yaflFloat g(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return x;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return x/3.0;
+    }
+
+    return (x > 0.0) ? 1.0 : -1.0;
+}
+
+yaflFloat gdot(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return 1.0;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return 1.0/3.0;
+    }
+
+    return 0.0;
+}
+
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_UKF_BASE_MEMORY_MIXIN(NX, NZ);
+    YAFL_UKF_JULIER_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflUKFJulierSt         sp = YAFL_UKF_JULIER_INITIALIZER(NX, 0, 0.0, kf_memory);
+yaflUKFAdaptiveRobustSt kf = YAFL_UKF_ADAPTIVE_ROBUST_INITIALIZER(&sp.base, &yafl_ukf_julier_spm, fx, 0, 0, hx, 0, 0, g, gdot, NX, NZ, 0.0, 8.807468393511947, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+
+    yafl_ukf_post_init(&kf.base.base);
+
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        yafl_ukf_adaptive_robust_bierman_predict(&kf);
+        yafl_ukf_adaptive_robust_bierman_update(&kf, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.base.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.base.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/ada_ukf.c

@@ -0,0 +1,134 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_UKF_MEMORY_MIXIN(NX, NZ);
+    YAFL_UKF_JULIER_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflUKFJulierSt      sp = YAFL_UKF_JULIER_INITIALIZER(NX, 0, 0.0, kf_memory);
+yaflUKFFullAdapiveSt kf = YAFL_UKF_FULL_ADAPTIVE_INITIALIZER(&sp.base, &yafl_ukf_julier_spm, fx, 0, 0, hx, 0, 0, NX, NZ, 0.0, 27.631021115871036, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+
+    yafl_ukf_post_init(&kf.base.base);
+
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        yafl_ukf_adaptive_predict(&kf);
+        yafl_ukf_adaptive_update(&kf.base.base, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.base.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.base.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/ada_ukf_bierman.c

@@ -0,0 +1,134 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_UKF_MEMORY_MIXIN(NX, NZ);
+    YAFL_UKF_JULIER_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflUKFJulierSt      sp = YAFL_UKF_JULIER_INITIALIZER(NX, 0, 0.0, kf_memory);
+yaflUKFAdaptivedSt   kf = YAFL_UKF_ADAPTIVE_INITIALIZER(&sp.base, &yafl_ukf_julier_spm, fx, 0, 0, hx, 0, 0, NX, NZ, 0.0, 10.827566170662733, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+
+    yafl_ukf_post_init(&kf.base);
+
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        yafl_ukf_adaptive_bierman_predict(&kf);
+        yafl_ukf_adaptive_bierman_update(&kf, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/bierman.c

@@ -0,0 +1,191 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jfx(yaflKalmanBaseSt * self, yaflFloat * w, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nx2;
+
+    (void)x;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(w,             YAFL_ST_INV_ARG_2);
+
+    nx  = self->Nx;
+    nx2 = nx * 2;
+
+    for (i = 0; i < nx; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx2 * i;
+        for (j = 0; j < nx; j++)
+        {
+            w[nci + j] = (i != j) ? 0.0 : 1.0;
+        }
+    }
+
+    w[nx2*0 + 2] = 0.1;
+    w[nx2*1 + 3] = 0.1;
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jhx(yaflKalmanBaseSt * self, yaflFloat * h, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nz;
+
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(h,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    nx = self->Nx;
+    nz = self->Nz;
+
+    for (i = 0; i < nz; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx * i;
+        for (j = 0; j < nx; j++)
+        {
+            h[nci + j] = 0.0;
+        }
+    }
+
+    h[nx*0 + 0] = 1.0;
+    h[nx*1 + 1] = 1.0;
+    return YAFL_ST_OK;
+}
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_EKF_BASE_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflEKFBaseSt kf = YAFL_EKF_BASE_INITIALIZER(fx, jfx, hx, jhx, 0, NX, NZ, 0.0, 0.0, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        YAFL_EKF_BIERMAN_PREDICT(&kf);
+        yafl_ekf_bierman_update(&kf, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/create_h5.py

@@ -9,9 +9,12 @@ import subprocess
 import numpy as np
 import h5py
 
+import matplotlib.pyplot as plt
 
-clear = np.zeros((100000, 2))
-noisy = np.zeros((100000, 2))
+N = 10000
+
+clear = np.zeros((N, 2))
+noisy = np.zeros((N, 2))
 for i in range(1, len(clear)):
     clear[i] = clear[i-1] + np.array([1.,1.])
     noisy[i] = clear[i]   + np.random.normal(scale=20., size=2)
@@ -21,8 +24,10 @@ with h5py.File('../data/input.h5', 'w') as h5f:
     h5f.create_dataset('noisy', data=noisy)
 
 #Run kf aplication here
-subprocess.call(['../projects/bin/Debug/hdf5_test'])
+subprocess.call(['../projects/ada_ukf/bin/Debug/hdf5_test'])
 
 with h5py.File('../data/output.h5', 'r') as h5f:
     kf_out = h5f['kf_out'][:]
 
+plt.plot(clear - kf_out)
+plt.show()

tests/src/main.c.bak

@@ -1,598 +0,0 @@
-/************************************************************
-
-  This example shows how to read and write data to a compact
-  dataset.  The program first writes integers to a compact
-  dataset with dataspace dimensions of DIM0xDIM1, then
-  closes the file.  Next, it reopens the file, reads back
-  the data, and outputs it to the screen.
-
-  This file is intended for use with HDF5 Library version 1.8
-
- ************************************************************/
-
-#include <hdf5/serial/hdf5.h>
-#include <stdio.h>
-#include <stdlib.h>
-
-#include "hdf5utils.h"
-#include "yakf.h"
-
-/*-----------------------------------------------------------------------------
-                            Kalman filter things
------------------------------------------------------------------------------*/
-#define NX 4
-#define NZ 2
-
-void fx(yakfBaseSt * self)
-{
-    yakfFloat * x;
-    YAKF_ASSERT(self);
-    YAKF_ASSERT(self->x);
-    YAKF_ASSERT(4 == self->Nx);
-
-    x = self->x;
-    x[0] += 0.1 * x[2];
-    x[1] += 0.1 * x[3];
-}
-
-void jfx(yakfBaseSt * self)
-{
-    yakfInt i;
-    yakfInt nx;
-    yakfInt nx2;
-    yakfFloat * w;
-
-    YAKF_ASSERT(self);
-    YAKF_ASSERT(self->W);
-    YAKF_ASSERT(4 == self->Nx);
-
-    w   = self->W;
-    nx  = self->Nx;
-    nx2 = nx * 2;
-
-    for (i = 0; i < nx; i++)
-    {
-        yakfInt j;
-        yakfInt nci;
-
-        nci = nx2 * i;
-        for (j = 0; j < nx; j++)
-        {
-            w[nci + j] = (i != j) ? 0.0 : 1.0;
-        }
-    }
-
-    w[nx2*0 + 2] = 0.1;
-    w[nx2*1 + 3] = 0.1;
-}
-
-void hx(yakfBaseSt * self)
-{
-    yakfFloat * x;
-    yakfFloat * y;
-    YAKF_ASSERT(self);
-    YAKF_ASSERT(self->x);
-    YAKF_ASSERT(2 == self->Nz);
-
-    x = self->x;
-    y = self->y;
-    y[0] = x[0];
-    y[1] = x[1];
-}
-
-void jhx(yakfBaseSt * self)
-{
-    yakfInt i;
-    yakfInt nx;
-    yakfInt nz;
-    yakfFloat * h;
-
-    YAKF_ASSERT(self);
-    YAKF_ASSERT(self->H);
-    YAKF_ASSERT(4 == self->Nx);
-    YAKF_ASSERT(2 == self->Nz);
-
-    nx = self->Nx;
-    nz = self->Nz;
-    h = self->H;
-
-    for (i = 0; i < nz; i++)
-    {
-        yakfInt j;
-        yakfInt nci;
-
-        nci = nx * i;
-        for (j = 0; j < nx; j++)
-        {
-            h[nci + j] = 0.0;
-        }
-    }
-
-    h[nx*0 + 0] = 1.0;
-    h[nx*1 + 1] = 1.0;
-}
-/*---------------------------------------------------------------------------*/
-typedef struct{
-    YAKF_BASE_MEMORY_MIXIN(NX, NZ);
-} kfMemorySt;
-
-#define DP (0.1)
-#define DX (1.0e-6)
-#define DZ (400)
-kfMemorySt kf_memory =
-{
-    .x = {
-        [0] = 50.0,
-        [2] = 10.0
-    },
-
-    .Up = {
-        0,
-        0,0,
-        0,0,0
-    },
-    .Dp = {DP, DP, DP, DP},
-
-    .Uq = {
-        0,
-        0,0,
-        0,0,0
-    },
-    .Dq = {DX, DX, DX, DX},
-
-    .Ur = {0},
-    .Dr = {DZ, DZ}
-};
-
-yakfBaseSt kf = YAKF_BASE_INITIALIZER(fx, jfx, hx, jhx, 0, NX, NZ, kf_memory);
-
-/*-----------------------------------------------------------------------------
-                                  Test data
------------------------------------------------------------------------------*/
-
-#define FILE            "data/input.h5"
-#define DATASET         "noisy"
-#define DIM0            5
-#define DIM1            6
-
-/*---------------------------------------------------------------------------*/
-int main (void)
-{
-    hid_t  file;    /* Handles */
-    herr_t status;
-    int    i;
-    //int    j;
-
-
-    /*Test read*/
-    file = H5Fopen(FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
-    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, DATASET);
-    status = H5Fclose(file);
-
-    YAKF_ASSERT(mat.shape.dim.y == NZ);
-    for (i=0; i<mat.shape.dim.x; i++)
-    {
-        YAKF_BIERMAN_PREDICT(&kf);
-        yakf_bierman_update(&kf, mat.data + NZ*i);
-        mat.data[NZ*i + 0] = kf.x[0];
-        mat.data[NZ*i + 1] = kf.x[1];
-    }
-
-    file = H5Fcreate("data/output.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
-    hdf5_utils_write_array(file, "kf_out", &mat);
-    status = H5Fclose(file);
-
-    free(mat.data);
-//
-//    file = H5Fopen(FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
-//    mat = hdf5_utils_read_array(file, DATASET);
-//    status = H5Fclose(file);
-//
-//    printf ("Matrix out %s:\n", DATASET);
-//    for (i=0; i<mat.shape.dim.x; i++) {
-//        printf (" [");
-//        for (j=0; j<mat.shape.dim.y; j++)
-//            printf (" %f", mat.data[mat.shape.dim.y*i+j]);
-//        printf ("]\n");
-//    }
-//    free(mat.data);
-//
-//    double a[] = {1, 2, 3};
-//    double b[] = {6, 5, 4};
-//    double c[] = {0, 0, 0};
-//
-//    printf("\nCheck *_nv\n");
-//
-//    yakfm_set_nv(3, c, a, 10);
-//    printf("10*a = [");
-//    for (i=0; i < 3; i++)
-//    {
-//        printf("%f ", c[i]);
-//    }
-//    printf("]\n");
-//
-//    yakfm_add_nv(3, c, a, 1);
-//    printf("11*a = [");
-//    for (i=0; i < 3; i++)
-//    {
-//        printf("%f ", c[i]);
-//    }
-//    printf("]\n");
-//
-//    yakfm_sub_nv(3, c, a, 1);
-//    printf("10*a = [");
-//    for (i=0; i < 3; i++)
-//    {
-//        printf("%f ", c[i]);
-//    }
-//    printf("]\n");
-//
-//
-//    printf("\nCheck *_vrn\n");
-//    yakfm_set_vrn(3, c, a, 10);
-//    printf("a/10 = [");
-//    for (i=0; i < 3; i++)
-//    {
-//        printf("%f ", c[i]);
-//    }
-//    printf("]\n");
-//
-//    yakfm_add_vrn(3, c, a, 1);
-//    printf("a*1.1 = [");
-//    for (i=0; i < 3; i++)
-//    {
-//        printf("%f ", c[i]);
-//    }
-//    printf("]\n");
-//
-//    yakfm_sub_vrn(3, c, a, 1);
-//    printf("a/10 = [");
-//    for (i=0; i < 3; i++)
-//    {
-//        printf("%f ", c[i]);
-//    }
-//    printf("]\n");
-//
-//    double m[3][3] =
-//    {
-//        {1,1,1},
-//        {0,1,2},
-//        {3,1,1}
-//    };
-//
-//    yakfm_set_mv(3, 3, c, m, a);
-//    printf("m*a = [");
-//    for (i=0; i < 3; i++)
-//    {
-//        printf("%f ", c[i]);
-//    }
-//    printf("]\n");
-//
-//    yakfm_set_vtm(3, 3, c, a, m);
-//    printf("a*m = [");
-//    for (i=0; i < 3; i++)
-//    {
-//        printf("%f ", c[i]);
-//    }
-//    printf("]\n");
-//
-//    yakfm_set_vvt(3, m, a, b);
-//
-//    printf("m = [\n");
-//    for (i=0; i<3; i++) {
-//        printf (" [");
-//        for (j=0; j<3; j++)
-//            printf (" %f", m[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    double d[3][3] =
-//    {
-//        {0.7,  3,  0.8},
-//        {0,  1.1,  2.5},
-//        {3,  1.7, 1.01}
-//    };
-//
-//    double r[3][3] =
-//    {
-//        {0,0,0},
-//        {0,0,0},
-//        {0,0,0}
-//    };
-//
-//    yakfm_set_mm(3, 3, 3, r, m, d);
-//
-//    printf("m*d = [\n");
-//    for (i=0; i<3; i++) {
-//        printf (" [");
-//        for (j=0; j<3; j++)
-//            printf (" %f", r[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    double v[5] = {1.0, 2.7, 3.0, 0.1, 0.2};
-//    double u[] =
-//        {3,
-//         0.8, 2.5,
-//         7.0, 0.3, 1.1,
-//        -1.0, 3.1, 7.7, 5.2};
-//
-//    double rvu[5] = {0};
-//
-//    yakfm_set_vtu(5, rvu, v, u);
-//    printf("v'u = [");
-//    for (i=0; i < 5; i++)
-//    {
-//        printf("%f ", rvu[i]);
-//    }
-//    printf("]\n");
-//
-//    yakfm_set_uv(5, rvu, u, v);
-//    printf("uv = [");
-//    for (i=0; i < 5; i++)
-//    {
-//        printf("%f ", rvu[i]);
-//    }
-//    printf("]\n");
-//
-//    yakfm_set_mu(3, 3, r, d, u);
-//
-//    printf("d.dot(u[:3,:3]) = [\n");
-//    for (i=0; i<3; i++) {
-//        printf (" [");
-//        for (j=0; j<3; j++)
-//            printf (" %f", r[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    yakfm_set_u(3, r, u);
-//
-//    printf("u[:3,:3] = [\n");
-//    for (i=0; i<3; i++) {
-//        printf (" [");
-//        for (j=0; j<3; j++)
-//            printf (" %f", r[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    yakfm_sub_u(3, r, u);
-//
-//    printf("u[:3,:3] = [\n");
-//    for (i=0; i<3; i++) {
-//        printf (" [");
-//        for (j=0; j<3; j++)
-//            printf (" %f", r[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    double bb[5][5] =
-//    {
-//        {0.7,  3,  0.8,  11,  12},
-//        {0,  1.1,  2.5, 0.1, 0.2},
-//        {3,  1.7, 1.01, 7,   0.5},
-//        {0,  1.1,  2.5, 0.1, 0.2},
-//        {3,  1.7, 1.01, 7,   0.5},
-//    };
-//
-//    YAKFM_BSET_U(5, 1, 1, bb, 3, u);
-//    printf("u[:3,:3] = [\n");
-//    for (i=0; i<5; i++) {
-//        printf (" [");
-//        for (j=0; j<5; j++)
-//            printf (" %f", bb[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    YAKFM_BSET_V(5, 0, 4, bb, 3, a);
-//    printf("u[:3,:3] = [\n");
-//    for (i=0; i<5; i++) {
-//        printf (" [");
-//        for (j=0; j<5; j++)
-//            printf (" %f", bb[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    YAKFM_BSET_VVT(5, 1, 1, bb, 3, a, b);
-//    printf("u[:3,:3] = [\n");
-//    for (i=0; i<5; i++) {
-//        printf (" [");
-//        for (j=0; j<5; j++)
-//            printf (" %f", bb[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    YAKFM_BSET_MU(5, 1, 1, bb, 3, 3, d, u);
-//    printf("u[:3,:3] = [\n");
-//    for (i=0; i<5; i++) {
-//        printf (" [");
-//        for (j=0; j<5; j++)
-//            printf (" %f", bb[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    yakfm_ruv(5, v, u);
-//    printf("linalg.inv(u).dot(v) = [");
-//    for (i=0; i < 5; i++)
-//    {
-//        printf("%f ", v[i]);
-//    }
-//    printf("]\n");
-//
-//    yakfm_rum(3, 3, d, u);
-//    printf("linalg.inv(u[:3,:3]).dot(d) = [\n");
-//    for (i=0; i<3; i++) {
-//        printf (" [");
-//        for (j=0; j<3; j++)
-//            printf (" %f", d[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    double uu[] = {
-//        1,
-//        1,1,
-//        1,1,1,
-//        1,1,1,1,
-//        1,1,1,1,1,
-//        1,1,1,1,1,1
-//    };
-//
-//    double dd[] = {0,0,0,0,0,0,0};
-//
-//    double www[7][5] = {
-//        {1.0, 3.0, 0.8,  7.0, -1.0},
-//        {0.0, 1.0, 2.5,  0.3,  3.1},
-//        {1.0, 3.0, 0.8,  7.0, -1.0},
-//        {0.0, 1.0, 2.5,  0.3,  3.1},
-//        {2.0, 0.0, 1.0,  1.1,  7.7},
-//        {0.3, 0.7, 0.3,  1.0,  5.2},
-//        {4.0, 0.0, 1.7,  0.5,  1.0}
-//    };
-//
-//    double ddd[5] = {5.0, 4.0, 5.0, 2.0, 3.0};
-//
-//    double uuu[7][7];
-//
-//    yakfm_mwgsu(7, 5, uu, dd, www, ddd);
-//    yakfm_set_u(7, uuu, uu);
-//
-//    printf("uu = [\n");
-//    for (i=0; i<7; i++) {
-//        printf (" [");
-//        for (j=0; j<7; j++)
-//            printf (" %f", uuu[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    printf("dd = [");
-//    for (i=0; i < 7; i++)
-//    {
-//        printf("%f ", dd[i]);
-//    }
-//    printf("]\n");
-//    /*===================================================================*/
-//    /*Test rank 1 update*/
-//    /*===================================================================*/
-//    double r1u_u[] = {
-//        -0.26266353,
-//        -4.41769372, -0.67235406,
-//         0.02001549,  0.59843321, 1.36003554,
-//         0.31444615,  0.31495661, 0.74221542, 0.25676365
-//    };
-//    double r1u_d[] = {51.0959219,  24.14873007,  4.23863142, 79.52239408, 97.95};
-//    double r1u_uview[5][5];
-//
-//    double r1u_v[] = {1., 2.7, 3, .1, .2};
-//
-//    yakfm_set_u(5, r1u_uview, r1u_u);
-//    printf("r1u_u = [\n");
-//    for (i=0; i<5; i++) {
-//        printf (" [");
-//        for (j=0; j<5; j++)
-//            printf (" %f", r1u_uview[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-//
-//    printf("r1u_d = [");
-//    for (i=0; i < 5; i++)
-//    {
-//        printf("%f ", r1u_d[i]);
-//    }
-//    printf("]\n");
-//
-//    printf ("=============================================\n");
-//    yakfm_udu_up(5, r1u_u, r1u_d, .3, r1u_v);
-//    yakfm_set_u(5, r1u_uview, r1u_u);
-//    printf("r1u_u = [\n");
-//    for (i=0; i<5; i++) {
-//        printf (" [");
-//        for (j=0; j<5; j++)
-//            printf (" %f", r1u_uview[i][j]);
-//        printf ("]\n");
-//    }
-//
-//    printf("dd = [");
-//    for (i=0; i < 5; i++)
-//    {
-//        printf("%f ", r1u_d[i]);
-//    }
-//    printf("]\n");
-//
-//    double r1d_v1[] = {1., 2.7, 3, .1, .2};
-//
-//    printf ("=============================================\n");
-//    yakfm_udu_down(5, r1u_u, r1u_d, .3, r1d_v1);
-//    yakfm_set_u(5, r1u_uview, r1u_u);
-//    printf("r1u_u = [\n");
-//    for (i=0; i<5; i++) {
-//        printf (" [");
-//        for (j=0; j<5; j++)
-//            printf (" %f", r1u_uview[i][j]);
-//        printf ("]\n");
-//    }
-//
-//    printf("dd = [");
-//    for (i=0; i < 5; i++)
-//    {
-//        printf("%f ", r1u_d[i]);
-//    }
-//    printf("]\n");
-//
-//    double r1d_v2[] = {1., 2.7, 3, .1, .2};
-//
-//    printf ("=============================================\n");
-//    yakfm_udu_down(5, r1u_u, r1u_d, .3, r1d_v2);
-//    yakfm_set_u(5, r1u_uview, r1u_u);
-//    printf("r1u_u = [\n");
-//    for (i=0; i<5; i++) {
-//        printf (" [");
-//        for (j=0; j<5; j++)
-//            printf (" %f", r1u_uview[i][j]);
-//        printf ("]\n");
-//    }
-//
-//    printf("dd = [");
-//    for (i=0; i < 5; i++)
-//    {
-//        printf("%f ", r1u_d[i]);
-//    }
-//    printf("]\n");
-//    /*===================================================================*/
-//    /*Test BBU*/
-//    /*===================================================================*/
-//    double bbb[5][10] =
-//    {
-//        {0.7,  3,  0.8,  11,  12, 0, 0, 0, 0, 0},
-//        {0,  1.1,  2.5, 0.1, 0.2, 0, 0, 0, 0, 0},
-//        {3,  1.7, 1.01, 7,   0.5, 0, 0, 0, 0, 0},
-//        {0,  1.1,  2.5, 0.1, 0.2, 0, 0, 0, 0, 0},
-//        {3,  1.7, 1.01, 7,   0.5, 0, 0, 0, 0, 0},
-//    };
-//    double bbu[] =
-//    {3,
-//    0.8, 2.5,
-//    7.0, 0.3, 1.1,
-//    -1.0, 3.1, 7.7, 5.2};
-//
-//    YAKFM_BSET_BU(10, 0, 5, bbb, 5, 5, 10, 0, 0, bbb, bbu);
-//    printf("u[:3,:3] = [\n");
-//    for (i=0; i<5; i++) {
-//        printf (" [");
-//        for (j=5; j<10; j++)
-//            printf (" %f", bbb[i][j]);
-//        printf ("]\n");
-//    }
-//    printf("]\n");
-    return 0;
-}

tests/src/rob_bierman.c

@@ -0,0 +1,228 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jfx(yaflKalmanBaseSt * self, yaflFloat * w, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nx2;
+
+    (void)x;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(w,             YAFL_ST_INV_ARG_2);
+
+    nx  = self->Nx;
+    nx2 = nx * 2;
+
+    for (i = 0; i < nx; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx2 * i;
+        for (j = 0; j < nx; j++)
+        {
+            w[nci + j] = (i != j) ? 0.0 : 1.0;
+        }
+    }
+
+    w[nx2*0 + 2] = 0.1;
+    w[nx2*1 + 3] = 0.1;
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jhx(yaflKalmanBaseSt * self, yaflFloat * h, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nz;
+
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(h,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    nx = self->Nx;
+    nz = self->Nz;
+
+    for (i = 0; i < nz; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx * i;
+        for (j = 0; j < nx; j++)
+        {
+            h[nci + j] = 0.0;
+        }
+    }
+
+    h[nx*0 + 0] = 1.0;
+    h[nx*1 + 1] = 1.0;
+    return YAFL_ST_OK;
+}
+
+/*---------------------------------------------------------------------------*/
+yaflFloat g(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return x;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return x/3.0;
+    }
+
+    return (x > 0.0) ? 1.0 : -1.0;
+}
+
+yaflFloat gdot(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return 1.0;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return 1.0/3.0;
+    }
+
+    return 0.0;
+}
+
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_EKF_BASE_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflEKFRobustSt kf = YAFL_EKF_ROBUST_INITIALIZER(fx, jfx, hx, jhx, 0, g, gdot, NX, NZ, 0.0, 0.0, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        YAFL_EKF_ROBUST_BIERAMN_PREDICT(&kf);
+        yafl_ekf_robust_bierman_update(&kf, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/rob_joseph.c

@@ -0,0 +1,228 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jfx(yaflKalmanBaseSt * self, yaflFloat * w, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nx2;
+
+    (void)x;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(w,             YAFL_ST_INV_ARG_2);
+
+    nx  = self->Nx;
+    nx2 = nx * 2;
+
+    for (i = 0; i < nx; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx2 * i;
+        for (j = 0; j < nx; j++)
+        {
+            w[nci + j] = (i != j) ? 0.0 : 1.0;
+        }
+    }
+
+    w[nx2*0 + 2] = 0.1;
+    w[nx2*1 + 3] = 0.1;
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn jhx(yaflKalmanBaseSt * self, yaflFloat * h, yaflFloat * x)
+{
+    yaflInt i;
+    yaflInt nx;
+    yaflInt nz;
+
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(h,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    nx = self->Nx;
+    nz = self->Nz;
+
+    for (i = 0; i < nz; i++)
+    {
+        yaflInt j;
+        yaflInt nci;
+
+        nci = nx * i;
+        for (j = 0; j < nx; j++)
+        {
+            h[nci + j] = 0.0;
+        }
+    }
+
+    h[nx*0 + 0] = 1.0;
+    h[nx*1 + 1] = 1.0;
+    return YAFL_ST_OK;
+}
+
+/*---------------------------------------------------------------------------*/
+yaflFloat g(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return x;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return x/3.0;
+    }
+
+    return (x > 0.0) ? 1.0 : -1.0;
+}
+
+yaflFloat gdot(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return 1.0;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return 1.0/3.0;
+    }
+
+    return 0.0;
+}
+
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_EKF_BASE_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflEKFRobustSt kf = YAFL_EKF_ROBUST_INITIALIZER(fx, jfx, hx, jhx, 0, g, gdot, NX, NZ, 0.0, 0.0, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        YAFL_EKF_ROBUST_JOSEPH_PREDICT(&kf);
+        yafl_ekf_robust_joseph_update(&kf, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/rob_ukf_bierman.c

@@ -0,0 +1,169 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+/*---------------------------------------------------------------------------*/
+yaflFloat g(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return x;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return x/3.0;
+    }
+
+    return (x > 0.0) ? 1.0 : -1.0;
+}
+
+yaflFloat gdot(yaflKalmanBaseSt * self, yaflFloat x)
+{
+    (void)self;
+
+    if (3.0 >= fabs(x))
+    {
+        return 1.0;
+    }
+
+    if (6.0 >= fabs(x))
+    {
+        return 1.0/3.0;
+    }
+
+    return 0.0;
+}
+
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_UKF_BASE_MEMORY_MIXIN(NX, NZ);
+    YAFL_UKF_JULIER_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflUKFJulierSt sp = YAFL_UKF_JULIER_INITIALIZER(NX, 0, 0.0, kf_memory);
+yaflUKFRobustSt kf = YAFL_UKF_ROBUST_INITIALIZER(&sp.base, &yafl_ukf_julier_spm, fx, 0, 0, hx, 0, 0, g, gdot, NX, NZ, 0.0, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+
+    yafl_ukf_post_init(&kf.base);
+
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        yafl_ukf_robust_bierman_predict(&kf);
+        yafl_ukf_robust_bierman_update(&kf, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/ukf.c

@@ -0,0 +1,134 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_UKF_MEMORY_MIXIN(NX, NZ);
+    YAFL_UKF_JULIER_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflUKFJulierSt sp = YAFL_UKF_JULIER_INITIALIZER(NX, 0, 0.0, kf_memory);
+yaflUKFSt       kf = YAFL_UKF_INITIALIZER(&sp.base, &yafl_ukf_julier_spm, fx, 0, 0, hx, 0, 0, NX, NZ, 0.0, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+
+    yafl_ukf_post_init(&kf.base);
+
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        yafl_ukf_predict(&kf);
+        yafl_ukf_update(&kf.base, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}

tests/src/ukf_bierman.c

@@ -0,0 +1,134 @@
+/*******************************************************************************
+    Copyright 2020 anonimous <shkolnick-kun@gmail.com> and contributors.
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing,
+    software distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
+    See the License for the specific language governing permissions
+    and limitations under the License.
+******************************************************************************/
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <hdf5/serial/hdf5.h>
+#include <hdf5utils.h>
+#include <yafl.h>
+
+/*-----------------------------------------------------------------------------
+                            Kalman filter things
+-----------------------------------------------------------------------------*/
+#define NX 4
+#define NZ 2
+
+yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
+{
+    (void)xz;
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_2);
+
+    x[0] += 0.1 * x[2];
+    x[1] += 0.1 * x[3];
+    return YAFL_ST_OK;
+}
+
+yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
+{
+    YAFL_CHECK(self,          YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
+    YAFL_CHECK(y,             YAFL_ST_INV_ARG_2);
+    YAFL_CHECK(x,             YAFL_ST_INV_ARG_3);
+
+    y[0] = x[0];
+    y[1] = x[1];
+    return YAFL_ST_OK;
+}
+
+/*---------------------------------------------------------------------------*/
+typedef struct
+{
+    YAFL_UKF_BASE_MEMORY_MIXIN(NX, NZ);
+    YAFL_UKF_JULIER_MEMORY_MIXIN(NX, NZ);
+    yaflFloat dummy[30];
+} kfMemorySt;
+
+#define DP (0.1)
+#define DX (1.0e-6)
+#define DZ (400)
+kfMemorySt kf_memory =
+{
+    .x = {
+        [0] = 50.0,
+        [2] = 10.0
+    },
+
+    .Up = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dp = {DP, DP, DP, DP},
+
+    .Uq = {
+        0,
+        0,0,
+        0,0,0
+    },
+    .Dq = {DX, DX, DX, DX},
+
+    .Ur = {0},
+    .Dr = {DZ, DZ}
+};
+
+yaflUKFJulierSt sp = YAFL_UKF_JULIER_INITIALIZER(NX, 0, 0.0, kf_memory);
+yaflUKFBaseSt   kf = YAFL_UKF_BASE_INITIALIZER(&sp.base, &yafl_ukf_julier_spm, fx, 0, 0, hx, 0, 0, NX, NZ, 0.0, kf_memory);
+
+/*-----------------------------------------------------------------------------
+                                  Test data
+-----------------------------------------------------------------------------*/
+#define IN_FILE  "../data/input.h5"
+#define IN_DS    "noisy"
+
+#define OUT_FILE "../data/output.h5"
+#define OUT_DS   "kf_out"
+
+/*---------------------------------------------------------------------------*/
+int main (void)
+{
+    hid_t  file;
+    herr_t status;
+    int    i;
+
+    file = H5Fopen(IN_FILE, H5F_ACC_RDONLY, H5P_DEFAULT);
+    hdf5UtilsMatSt mat = hdf5_utils_read_array(file, IN_DS);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", IN_FILE, status);
+
+    assert(mat.shape.dim.y == NZ);
+
+    yafl_ukf_post_init(&kf);
+
+    for (i=0; i<mat.shape.dim.x; i++)
+    {
+        yafl_ukf_bierman_predict(&kf);
+        yafl_ukf_bierman_update(&kf, mat.data + NZ*i);
+        mat.data[NZ*i + 0] = kf.base.x[0];
+        mat.data[NZ*i + 1] = kf.base.x[1];
+    }
+
+    file = H5Fcreate(OUT_FILE, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
+    hdf5_utils_write_array(file, OUT_DS, &mat);
+    status = H5Fclose(file);
+    printf("File %s closed with status: %d\n", OUT_FILE, status);
+
+    free(mat.data);
+    return 0;
+}