Added C-build tests. Fixed errors in UKF initializers.

src/yafl.h

@@ -457,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,                                                           \
                                                                               \
@@ -571,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:
@@ -632,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/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/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/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="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/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/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/src/ada_rob_bierman.c

@@ -121,12 +121,12 @@ yaflFloat g(yaflKalmanBaseSt * self, yaflFloat x)
 {
     (void)self;
 
-    if (3.0 < fabs(x))
+    if (3.0 >= fabs(x))
     {
         return x;
     }
 
-    if (6.0 < fabs(x))
+    if (6.0 >= fabs(x))
     {
         return x/3.0;
     }
@@ -138,12 +138,12 @@ yaflFloat gdot(yaflKalmanBaseSt * self, yaflFloat x)
 {
     (void)self;
 
-    if (3.0 < fabs(x))
+    if (3.0 >= fabs(x))
     {
         return 1.0;
     }
 
-    if (6.0 < fabs(x))
+    if (6.0 >= fabs(x))
     {
         return 1.0/3.0;
     }
@@ -191,10 +191,10 @@ yaflEKFAdaptiveRobustSt kf = YAFL_EKF_ADAPTIVE_ROBUST_INITIALIZER(fx, jfx, hx, j
 /*-----------------------------------------------------------------------------
                                   Test data
 -----------------------------------------------------------------------------*/
-#define IN_FILE  "../../data/input.h5"
+#define IN_FILE  "../data/input.h5"
 #define IN_DS    "noisy"
 
-#define OUT_FILE "../../data/output.h5"
+#define OUT_FILE "../data/output.h5"
 #define OUT_DS   "kf_out"
 
 /*---------------------------------------------------------------------------*/

tests/src/ada_rob_joseph.c

@@ -121,12 +121,12 @@ yaflFloat g(yaflKalmanBaseSt * self, yaflFloat x)
 {
     (void)self;
 
-    if (3.0 < fabs(x))
+    if (3.0 >= fabs(x))
     {
         return x;
     }
 
-    if (6.0 < fabs(x))
+    if (6.0 >= fabs(x))
     {
         return x/3.0;
     }
@@ -138,12 +138,12 @@ yaflFloat gdot(yaflKalmanBaseSt * self, yaflFloat x)
 {
     (void)self;
 
-    if (3.0 < fabs(x))
+    if (3.0 >= fabs(x))
     {
         return 1.0;
     }
 
-    if (6.0 < fabs(x))
+    if (6.0 >= fabs(x))
     {
         return 1.0/3.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/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/bieman/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/rob_bierman.c

@@ -121,12 +121,12 @@ yaflFloat g(yaflKalmanBaseSt * self, yaflFloat x)
 {
     (void)self;
 
-    if (3.0 < fabs(x))
+    if (3.0 >= fabs(x))
     {
         return x;
     }
 
-    if (6.0 < fabs(x))
+    if (6.0 >= fabs(x))
     {
         return x/3.0;
     }
@@ -138,12 +138,12 @@ yaflFloat gdot(yaflKalmanBaseSt * self, yaflFloat x)
 {
     (void)self;
 
-    if (3.0 < fabs(x))
+    if (3.0 >= fabs(x))
     {
         return 1.0;
     }
 
-    if (6.0 < fabs(x))
+    if (6.0 >= fabs(x))
     {
         return 1.0/3.0;
     }

tests/src/rob_joseph.c

@@ -121,12 +121,12 @@ yaflFloat g(yaflKalmanBaseSt * self, yaflFloat x)
 {
     (void)self;
 
-    if (3.0 < fabs(x))
+    if (3.0 >= fabs(x))
     {
         return x;
     }
 
-    if (6.0 < fabs(x))
+    if (6.0 >= fabs(x))
     {
         return x/3.0;
     }
@@ -138,12 +138,12 @@ yaflFloat gdot(yaflKalmanBaseSt * self, yaflFloat x)
 {
     (void)self;
 
-    if (3.0 < fabs(x))
+    if (3.0 >= fabs(x))
     {
         return 1.0;
     }
 
-    if (6.0 < fabs(x))
+    if (6.0 >= fabs(x))
     {
         return 1.0/3.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;
+}