Added build test for EKF variants.

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)
 

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/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/hdf5_test.cbp → tests/projects/joseph/hdf5_test.cbp

@@ -2,7 +2,7 @@
 <CodeBlocks_project_file>
 	<FileVersion major="1" minor="6" />
 	<Project>
-		<Option title="hdf5_test" />
+		<Option title="joseph" />
 		<Option pch_mode="2" />
 		<Option compiler="gcc" />
 		<Build>
@@ -25,9 +25,9 @@
 			<Add option="-Wall" />
 			<Add option="-g" />
 			<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" />
@@ -38,22 +38,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/bierman.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/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/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/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

@@ -21,7 +21,7 @@ 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/bieman/bin/Debug/hdf5_test'])
 
 with h5py.File('../data/output.h5', 'r') as h5f:
     kf_out = h5f['kf_out'][:]

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;
+}