From a0eec08b172172681730fb5560afeba37c0cb282 Mon Sep 17 00:00:00 2001
From: mulles <github.com@emu.lu>
Date: Wed, 6 Apr 2022 16:40:53 +0200
Subject: [PATCH 1/7] Pull Request SoC with Extended Kalman Filter

-Debugging is still done partially by printf, as I was not able to get the zephyr tool for it running.
-The equation system for the model is probably incorrect, we should discuss it.
---
 app/src/CMakeLists.txt          |   1 +
 app/src/bat_charger.cpp         | 226 +++++++-
 app/src/bat_charger.h           | 199 ++++++-
 app/src/kalman_soc.cpp          | 399 ++++++++++++++
 app/src/kalman_soc.h            |  25 +
 app/src/main.cpp                |   5 +-
 app/src/setup.cpp               |   1 +
 app/src/setup.h                 |   1 +
 tests/CMakeLists.txt            |   1 +
 tests/src/main.cpp              |   1 +
 tests/src/tests.h               |   2 +
 tests/src/tests_bat_charger.cpp |   2 +-
 tests/src/tests_dcdc.cpp        |   4 +-
 tests/src/tests_kalman_soc.cpp  | 901 ++++++++++++++++++++++++++++++++
 14 files changed, 1759 insertions(+), 9 deletions(-)
 create mode 100644 app/src/kalman_soc.cpp
 create mode 100644 app/src/kalman_soc.h
 create mode 100644 tests/src/tests_kalman_soc.cpp

diff --git a/app/src/CMakeLists.txt b/app/src/CMakeLists.txt
index 04dc7981..20424b90 100644
--- a/app/src/CMakeLists.txt
+++ b/app/src/CMakeLists.txt
@@ -20,6 +20,7 @@ target_sources(app PRIVATE
         pwm_switch_driver.c
         pwm_switch.cpp
         setup.cpp
+        kalman_soc.cpp
 )
 
 add_subdirectory(ext)
diff --git a/app/src/bat_charger.cpp b/app/src/bat_charger.cpp
index 60b6747a..320511c8 100644
--- a/app/src/bat_charger.cpp
+++ b/app/src/bat_charger.cpp
@@ -22,6 +22,12 @@ LOG_MODULE_REGISTER(bat_charger, CONFIG_BAT_LOG_LEVEL);
 extern DeviceStatus dev_stat;
 extern LoadOutput load;
 
+uint32_t milli_seconds_in_float = 0;
+uint32_t float_reset_duration = 600000;             // 10 minutes in milliseconds
+const uint32_t soc_scaled_hundred_percent = 100000; // 100% charge = 100000
+const uint32_t soc_scaled_max =
+    2 * soc_scaled_hundred_percent; // allow soc to track up higher than 100% to gauge efficiency
+
 // DISCHARGE_CURRENT_MAX used to estimate current-compensation of load disconnect voltage
 #if BOARD_HAS_LOAD_OUTPUT
 #define DISCHARGE_CURRENT_MAX DT_PROP(DT_CHILD(DT_PATH(outputs), load), current_max)
@@ -29,6 +35,60 @@ extern LoadOutput load;
 #define DISCHARGE_CURRENT_MAX DT_PROP(DT_PATH(pcb), dcdc_current_max)
 #endif
 
+float calculate_initial_soc(float battery_voltage_mV)
+{
+    // TODO will need to add 24 V compatability
+    const uint32_t soc_scaled_hundred_percent = 100000;
+    const uint8_t voltages_size = 10;
+    const float batt_soc_voltages[voltages_size] = { 12720, 12600, 12480, 12360, 12240,
+                                                     12120, 12000, 11880, 11760, 11640 };
+
+    uint8_t index;
+    for (index = 0; index < voltages_size; index++) {
+        if (battery_voltage_mV > batt_soc_voltages[index]) {
+            break;
+        }
+    }
+    return (voltages_size - index) * (soc_scaled_hundred_percent / voltages_size);
+}
+
+void diagonal_matrix(float *A, float value, int n, int m)
+{
+    int i, j;
+    for (i = 0; i < n; i++) {
+        for (j = 0; j < m; j++) {
+            if (i == j) {
+                A[i * n + j] = value;
+            }
+            else {
+                A[i * n + j] = 0;
+            }
+            LOG_DBG("%f ", A[i * n + j]);
+        }
+        LOG_DBG("\n");
+    }
+}
+
+void init_soc(EkfSoc *ekf_soc, float v0, float P0, float Q0, float R0, float initial_soc)
+{
+    // Init State vector
+    // use stored soc, unless it's out of range, in which case calculate new starting point
+    ekf_soc->x[0] = (initial_soc >= 0 && initial_soc <= soc_scaled_max) ? initial_soc
+                                                                        : calculate_initial_soc(v0);
+    ekf_soc->x[1] = 0.0; // TODO Check what init makes sense
+    ekf_soc->x[2] = 0.0; // TODO Check what init makes sense
+
+    LOG_DBG("Init Matrix F\n");
+    diagonal_matrix(&ekf_soc->F[0][0], 1, NUMBER_OF_STATES_SOC,
+                    NUMBER_OF_STATES_SOC); // F identity matrix
+    LOG_DBG("\nInit Matrix P\n");
+    diagonal_matrix(&ekf_soc->P[0][0], P0, NUMBER_OF_STATES_SOC, NUMBER_OF_STATES_SOC);
+    LOG_DBG("\nInit Matrix Q\n");
+    diagonal_matrix(&ekf_soc->Q[0][0], Q0, NUMBER_OF_STATES_SOC, NUMBER_OF_STATES_SOC);
+    LOG_DBG("\nInit Matrix R\n");
+    diagonal_matrix(&ekf_soc->R[0][0], R0, NUMBER_OF_OBSERVABLES_SOC, NUMBER_OF_OBSERVABLES_SOC);
+}
+
 void battery_conf_init(BatConf *bat, int type, int num_cells, float nominal_capacity)
 {
     bat->nominal_capacity = nominal_capacity;
@@ -362,7 +422,7 @@ void Charger::detect_num_batteries(BatConf *bat) const
     }
 }
 
-void Charger::update_soc(BatConf *bat_conf)
+void Charger::update_soc_voltage_based(BatConf *bat_conf)
 {
     static int soc_filtered = 0; // SOC / 100 for better filtering
 
@@ -657,7 +717,7 @@ void Charger::charge_control(BatConf *bat_conf)
     }
 }
 
-void Charger::init_terminal(BatConf *bat) const
+void Charger::init_terminal(BatConf *bat, EkfSoc *ekf_soc) const
 {
     port->bus->sink_voltage_intercept = bat->topping_voltage;
     port->bus->src_voltage_intercept = bat->load_disconnect_voltage;
@@ -681,4 +741,166 @@ void Charger::init_terminal(BatConf *bat) const
     port->bus->src_droop_res =
         -bat->wire_resistance / static_cast<float>(port->bus->series_multiplier)
         - bat->internal_resistance;
+
+    float P0 = 0.1;   // initial covariance of state noise  (aka process noise)
+    float Q0 = 0.001; // Initial state uncertainty covariance matrix
+    float R0 = 0.1;   // initial covariance of measurement noise
+    float battery_voltage_mV[1] = {
+        port->bus->voltage * 1000
+    }; // intial Voltage measurement to calculate SoC if initial_soc is out of range
+    float initial_soc = soc * 1000; // last known SoC
+    // Do generic EKF initialization
+    ekf_init(ekf_soc, NUMBER_OF_STATES_SOC, NUMBER_OF_OBSERVABLES_SOC);
+    init_soc(ekf_soc, battery_voltage_mV[0], P0, Q0, R0, initial_soc);
+}
+float clamp(float value, float min, float max)
+{
+    if (value > max) {
+        return max;
+    }
+    else if (value < min) {
+        return min;
+    }
+    return value;
+}
+
+float model_soc(EkfSoc *ekf_soc, bool is_battery_in_float, float battery_eff,
+                float battery_current_mA, float sample_period_milli_sec, float battery_capacity_Ah)
+{
+    // $\hat{x}_k = f(\hat{x}_{k-1})$
+    battery_eff = f(ekf_soc, is_battery_in_float, battery_eff, battery_current_mA,
+                    sample_period_milli_sec, battery_capacity_Ah);
+    LOG_DBG("The SoC by f()  %f \n", ekf_soc->x[0]);
+    // update measurable (voltage) based on predicted state (SoC)
+    h(ekf_soc, battery_current_mA);
+    return battery_eff;
+}
+
+float f(EkfSoc *ekf_soc, bool is_battery_in_float, float battery_eff, float battery_current_mA,
+        float sample_period_milli_sec, float battery_capacity_Ah)
+{
+    float milli_sec_to_hours = 3600000;
+    float charge_change = (battery_current_mA / 1000) * battery_eff / 100000
+                          * (sample_period_milli_sec / milli_sec_to_hours);
+    float previous_soc = ekf_soc->x[0];
+    float new_soc = (ekf_soc->x[0] * battery_capacity_Ah + charge_change * 1000)
+                    / battery_capacity_Ah; // scaling should be fine here
+    ekf_soc->fx[0] = new_soc;
+
+    if (is_battery_in_float) {
+        milli_seconds_in_float += sample_period_milli_sec;
+        if (milli_seconds_in_float > float_reset_duration) {
+
+            battery_eff = (float)battery_eff * (float)soc_scaled_hundred_percent / previous_soc;
+            battery_eff = clamp(battery_eff, 0, soc_scaled_hundred_percent);
+            ekf_soc->fx[0] = soc_scaled_hundred_percent;
+        }
+    }
+    else {
+        milli_seconds_in_float = 0;
+    }
+
+    return battery_eff;
+}
+
+void h(EkfSoc *ekf_soc, float battery_current_mA)
+{
+
+    // _hx is the voltage that most closely matches current SoC (a number)
+    // _H is an array of form [ocv gradient, measured current, 1] (the last parameter is the offset)
+    // x_[0] = SOC, _x[1] = R0 _x[2]=U1 units are unknown.
+
+    bool is_battery_12_V = true;
+    bool is_battery_lithium = false;
+    int index_R0 = 1;
+    int index_U1 = 2;
+
+    // Hardcoded  SoC-OCV Curve aka Lookuptable
+    float dummy_lead_acid_voltage[101] = {
+        11640, 11653, 11666, 11679, 11692, 11706, 11719, 11732, 11745, 11758, 11772, 11785, 11798,
+        11811, 11824, 11838, 11851, 11864, 11877, 11890, 11904, 11917, 11930, 11943, 11956, 11970,
+        11983, 11996, 12009, 12022, 12036, 12049, 12062, 12075, 12088, 12102, 12115, 12128, 12141,
+        12154, 12168, 12181, 12194, 12207, 12220, 12234, 12247, 12260, 12273, 12286, 12300, 12313,
+        12326, 12339, 12352, 12366, 12379, 12392, 12405, 12418, 12432, 12445, 12458, 12471, 12484,
+        12498, 12511, 12524, 12537, 12550, 12564, 12577, 12590, 12603, 12616, 12630, 12643, 12656,
+        12669, 12682, 12696, 12709, 12722, 12735, 12748, 12762, 12775, 12788, 12801, 12814, 12828,
+        12841, 12854, 12867, 12880, 12894, 12907, 12920, 12933, 12946, 12960
+    };
+    float dummy_lithium_voltage[101] = {
+        5000,  6266,  7434,  8085,  8531,  8867,  9134,  9355,  9543,  9705,  9847,  9974,  10088,
+        10191, 10285, 10372, 10451, 10525, 10595, 10659, 10720, 10777, 10831, 10882, 10931, 10977,
+        11021, 11063, 11104, 11142, 11180, 11216, 11251, 11284, 11317, 11349, 11379, 11409, 11438,
+        11467, 11495, 11522, 11548, 11574, 11600, 11625, 11650, 11675, 11699, 11723, 11746, 11769,
+        11793, 11815, 11838, 11861, 11883, 11906, 11928, 11950, 11972, 11994, 12017, 12039, 12061,
+        12083, 12105, 12127, 12150, 12172, 12195, 12217, 12240, 12263, 12286, 12309, 12333, 12356,
+        12380, 12404, 12428, 12452, 12477, 12501, 12526, 12552, 12577, 12603, 12629, 12655, 12682,
+        12708, 12735, 12763, 12790, 12818, 12846, 12875, 12903, 12931, 12960
+    };
+    float dummy_ocv_soc[101] = {
+        0,     1000,  2000,  3000,  4000,  5000,  6000,  7000,  8000,  9000,  10000, 11000, 12000,
+        13000, 14000, 15000, 16000, 17000, 18000, 19000, 20000, 21000, 22000, 23000, 24000, 25000,
+        26000, 27000, 28000, 29000, 30000, 31000, 32000, 33000, 34000, 35000, 36000, 37000, 38000,
+        39000, 40000, 41000, 42000, 43000, 44000, 45000, 46000, 47000, 48000, 49000, 50000, 51000,
+        52000, 53000, 54000, 55000, 56000, 57000, 58000, 59000, 60000, 61000, 62000, 63000, 64000,
+        65000, 66000, 67000, 68000, 69000, 70000, 71000, 72000, 73000, 74000, 75000, 76000, 77000,
+        78000, 79000, 80000, 81000, 82000, 83000, 84000, 85000, 86000, 87000, 88000, 89000, 90000,
+        91000, 92000, 93000, 94000, 95000, 96000, 97000, 98000, 99000, 100000
+    };
+    // update voltage closest to current state of charge as well as gradient
+    int i;
+    float multiplier;
+
+    if (is_battery_12_V) {
+        multiplier = 1;
+    }
+    else {
+        multiplier = 2;
+    }
+    for (i = 0; i < 101; i++) {
+
+        if (dummy_ocv_soc[i] > (float)ekf_soc->x[0]) {
+            if (is_battery_lithium) {
+                ekf_soc->hx[0] =
+                    (dummy_lithium_voltage[i] + dummy_lithium_voltage[i - 1]) * multiplier / 2
+                    + (battery_current_mA / 1000 * ekf_soc->x[index_R0] / 100)
+                    + ekf_soc->x[index_U1] / 100; // units should be good her
+                ekf_soc->H[0][0] =
+                    (dummy_lithium_voltage[i] - dummy_lithium_voltage[i - 1]) * multiplier * 100
+                    / (dummy_ocv_soc[i] - dummy_ocv_soc[i - 1]); // units are good here
+            }
+            else {
+                ekf_soc->hx[0] =
+                    (dummy_lead_acid_voltage[i] + dummy_lead_acid_voltage[i - 1]) * multiplier / 2
+                    + (battery_current_mA / 1000 * ekf_soc->x[index_R0] / 100)
+                    + ekf_soc->x[index_U1] / 100;
+                ekf_soc->H[0][0] = (dummy_lead_acid_voltage[i] - dummy_lead_acid_voltage[i - 1])
+                                   * multiplier * 100 / (dummy_ocv_soc[i] - dummy_ocv_soc[i - 1]);
+            }
+            ekf_soc->H[0][1] = battery_current_mA / 1000; // should be good in Amps
+            ekf_soc->H[0][2] = 1;                         // offset
+            printf("U0= I*R0 = %fmV \n", (battery_current_mA / 1000 * ekf_soc->x[index_R0]) / 100);
+            printf("U1= %fmV \n", ekf_soc->x[index_U1] / 100);
+            printf("For single Cell Lithium would be \nU0/4= I*R0/4Cells = %fmV \n",
+                   (battery_current_mA / 1000 * ekf_soc->x[index_R0]) / 4 / 100);
+            printf("U1/4Cells= %fmV \n", ekf_soc->x[index_U1] / 4 / 100);
+            return;
+        }
+    }
+}
+
+void Charger::update_soc(BatConf *bat_conf, EkfSoc *ekf_soc)
+{
+
+    int cholsl_error = 0;
+    float battery_eff = 100000; // fixed to 100% implemented to use later on.
+    float sample_period_milli_sec = 1000;
+    float battery_voltage_mV[1] = { port->bus->voltage * 1000 };
+
+    battery_eff = model_soc(ekf_soc, bat_conf->float_enabled, battery_eff, (port->current * 1000),
+                            sample_period_milli_sec, bat_conf->nominal_capacity);
+    cholsl_error = ekf_step(ekf_soc, battery_voltage_mV);
+    LOG_DBG("Numerical Error in EKF_Step 1=true, 0 = false %d\n", cholsl_error);
+    LOG_DBG("Soc after EKF and before clamp %f\n", ekf_soc->x[0]);
+    ekf_soc->x[0] = clamp((float)ekf_soc->x[0], 0, soc_scaled_hundred_percent);
+    soc = ekf_soc->x[0] / 1000;
 }
diff --git a/app/src/bat_charger.h b/app/src/bat_charger.h
index 15e0101c..5dc15d31 100644
--- a/app/src/bat_charger.h
+++ b/app/src/bat_charger.h
@@ -12,6 +12,7 @@
  * @brief Battery and charger configuration and functions
  */
 
+#include "kalman_soc.h"
 #include <stdbool.h>
 #include <stddef.h>
 #include <stdint.h>
@@ -21,6 +22,193 @@
 
 #define CHARGER_TIME_NEVER INT32_MIN
 
+//#define DEBUG 0
+#define NUMBER_OF_STATES_SOC      3
+#define NUMBER_OF_OBSERVABLES_SOC 1
+
+/**
+ * Extended Kalman Filter (EKF) configuration data
+ *
+ * Data will be initialized in Charger::init_terminal
+ */
+typedef struct
+{
+    /**
+     * number of state values
+     *
+     */
+    int n;
+
+    /**
+     * number of observables
+     *
+     */
+    int m;
+
+    /**
+     * state vector
+     *
+     * [ir0 hk0 SOC0] TODO
+     */
+    float x[NUMBER_OF_STATES_SOC];
+
+    /**
+     * prediction error covariance
+     *
+     */
+    float P[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     * process noise covariance
+     *
+     * uncertainty of current sensor, state equation defined as fx
+     */
+    float Q[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     *  measurement error covariance
+     *
+     * uncertainty of voltage sensor, output equation defined as hx
+     */
+    float R[NUMBER_OF_OBSERVABLES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+
+    /**
+     * Kalman gain; a.k.a. K
+     *
+     */
+    float G[NUMBER_OF_STATES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+
+    /**
+     * Jacobian of process model
+     *
+     */
+    float F[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     * Jacobian of measurement model
+     *
+     */
+    float H[NUMBER_OF_OBSERVABLES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     *  transpose of measurement Jacobian
+     *
+     */
+    float Ht[NUMBER_OF_STATES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+
+    /**
+     * transpose of process Jacobian
+     *
+     */
+    float Ft[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     *  P, post-prediction, pre-update
+     *
+     */
+    float Pp[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     * output of user defined f() state-transition function
+     *
+     */
+    float fx[NUMBER_OF_STATES_SOC];
+
+    /**
+     * output of user defined h() measurement function
+     *
+     */
+    float hx[NUMBER_OF_OBSERVABLES_SOC];
+
+    /**
+     * temporary storage
+     *
+     */
+    float tmp0[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+    float tmp1[NUMBER_OF_STATES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+    float tmp2[NUMBER_OF_OBSERVABLES_SOC][NUMBER_OF_STATES_SOC];
+    float tmp3[NUMBER_OF_OBSERVABLES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+    float tmp4[NUMBER_OF_OBSERVABLES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+    float tmp5[NUMBER_OF_OBSERVABLES_SOC];
+
+} EkfSoc;
+
+/**
+ * Clamp function to ensure SoC within min max boders.
+ *
+ * @param value SoC value for which to ensure to be within borders min, max
+ * @param min The minimum value 0% the SoC is allowed to have
+ * @param max The maximum value 100% the SoC is allowed to have
+ * @return float Either the handed value or 0%, 100$ when excisting borders.
+ */
+float clamp(float value, float min, float max);
+
+/**
+ * Calculates initial SoC based on handed voltage
+ *
+ * @param battery_voltage_mV
+ * @return float Inital SoC
+ */
+
+float calculate_initial_soc(float battery_voltage_mV);
+
+/**
+ * Initializes Extended Kalman Filter matrices based on handed parameters
+ *
+ * *
+ * WARNING TODO: It is unclear if the equations used are correct, thus the init of F might be wrong
+ *
+ * @param ekf_soc struct in which initialized matrices are stored
+ * @param v0 initial voltage on which the inital SoC is based
+ * @param P0 Diagonal values for P matrix
+ * @param Q0 Diagonal values for Q matrix
+ * @param R0 Diagonal values for R matrix
+ * @param initial_soc inital SoC if known.
+ */
+void init_soc(EkfSoc *ekf_soc, float v0, float P0, float Q0, float R0, float initial_soc);
+
+/**
+ * Unites f and h function and forms the complete battery model
+ *
+ * @param ekf_soc struct with EKF Parameters
+ * @param is_battery_in_float for Lead Acid Batterys Float Charging Status
+ * @param battery_eff Efficieny currently not used
+ * @param battery_current_mA
+ * @param sample_period_milli_sec Period between battery current and voltage measurements
+ * @param battery_capacity_Ah
+ * @return float Efficiency currently not used
+ */
+float model_soc(EkfSoc *ekf_soc, bool is_battery_in_float, float battery_eff,
+                float battery_current_mA, float sample_period_milli_sec, float battery_capacity_Ah);
+
+/**
+ * project the state of charge ahead one step using a Coulomb counting model
+ * (Integration of the current over time)
+ * x{k+1}(index_soc) = x{k} - \frac{1}{{Q_{C}}}\int_{0}^{\Delta t} {i(t)\ dt}
+ *
+ * @param ekf_soc  struct with EKF Parameters
+ * @param is_battery_in_float Is the lead acid batterie in float charge?
+ * @param battery_eff Efficiency c   urrently not used
+ * @param battery_current_mA
+ * @param sample_period_milli_sec Period between battery measurements
+ * @param battery_capacity_Ah
+ * @return float
+ */
+float f(EkfSoc *ekf_soc, bool is_battery_in_float, float battery_eff, float battery_current_mA,
+        float sample_period_milli_sec, float battery_capacity_Ah);
+
+/**
+ * predict the measurable value (voltage) ahead one step using the newly estimated state of charge
+ * {h}(k) = {OCV}(x{k}) - {R}_{0} i(t) - {R}_{1} {i}_{R_1}(t) in mV
+ *
+ * WARNING TODO: It is unclear if the equations used are correct and if so if their implementation
+ * is correct thus x vector and H Matrix might be wrong.
+ *
+ * @param ekf_soc  struct with EKF Parameters
+ * @param battery_current_mA
+ */
+void h(EkfSoc *ekf_soc, float battery_current_mA);
+
 /**
  * Battery cell types
  */
@@ -457,14 +645,21 @@ class Charger
      *
      * Must be called exactly once per second, otherwise SOC calculation gets wrong.
      */
-    void update_soc(BatConf *bat_conf);
+    void update_soc(BatConf *bat_conf, EkfSoc *ekf_soc);
+
+    /**
+     * SOC estimation
+     *
+     * WARNING: TODO obsolte function, replaced by void charge_control(BatConf *bat_conf);
+     */
+    void update_soc_voltage_based(BatConf *bat_conf);
 
     /**
      * Initialize terminal and dc bus for battery connection
      *
      * @param bat Configuration to be used for terminal setpoints
      */
-    void init_terminal(BatConf *bat) const;
+    void init_terminal(BatConf *bat, EkfSoc *ekf_soc) const;
 
 private:
     void enter_state(int next_state);
diff --git a/app/src/kalman_soc.cpp b/app/src/kalman_soc.cpp
new file mode 100644
index 00000000..270f9ef3
--- /dev/null
+++ b/app/src/kalman_soc.cpp
@@ -0,0 +1,399 @@
+/*
+ * TinyEKF: Extended Kalman Filter for embedded processors
+ *
+ * Copyright (C) 2015 Simon D. Levy
+ *
+ * MIT License
+ */
+
+#include "kalman_soc.h"
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#define DEBUG
+
+#include <logging/log.h>
+LOG_MODULE_REGISTER(kalman_soc, CONFIG_LOG_DEFAULT_LEVEL);
+
+typedef struct
+{
+
+    float *x; /* state vector */
+
+    float *P; /* prediction error covariance */
+    float *Q; /* process noise covariance */
+    float *R; /* measurement error covariance */
+
+    float *G; /* Kalman gain; a.k.a. K */
+
+    float *F; /* Jacobian of process model */
+    float *H; /* Jacobian of measurement model */
+
+    float *Ht; /* transpose of measurement Jacobian */
+    float *Ft; /* transpose of process Jacobian */
+    float *Pp; /* P, post-prediction, pre-update */
+
+    float *fx; /* output of user defined f() state-transition function */
+    float *hx; /* output of user defined h() measurement function */
+
+    /* temporary storage */
+    float *tmp0;
+    float *tmp1;
+    float *tmp2;
+    float *tmp3;
+    float *tmp4;
+    float *tmp5;
+
+} EKF;
+
+/* Cholesky-decomposition matrix-inversion code, adapated from
+   http://jean-pierre.moreau.pagesperso-orange.fr/Cplus/choles_cpp.txt */
+
+static int choldc1(float *a, float *p, int n)
+{
+    int i, j, k;
+    float sum;
+
+    for (i = 0; i < n; i++) {
+        for (j = i; j < n; j++) {
+            sum = a[i * n + j];
+            for (k = i - 1; k >= 0; k--) {
+                sum -= a[i * n + k] * a[j * n + k];
+            }
+            if (i == j) {
+                if (sum <= 0) {
+                    return 1; /* error */
+                }
+                p[i] = sqrt(sum);
+            }
+            else {
+                a[j * n + i] = sum / p[i];
+            }
+        }
+    }
+
+    return 0; /* success */
+}
+
+static int choldcsl(float *A, float *a, float *p, int n)
+{
+    int i, j, k;
+    float sum;
+    for (i = 0; i < n; i++) {
+        for (j = 0; j < n; j++) {
+            a[i * n + j] = A[i * n + j];
+        }
+    }
+    if (choldc1(a, p, n)) {
+        return 1;
+    }
+    for (i = 0; i < n; i++) {
+        a[i * n + i] = 1 / p[i];
+        for (j = i + 1; j < n; j++) {
+            sum = 0;
+            for (k = i; k < j; k++) {
+                sum -= a[j * n + k] * a[k * n + i];
+            }
+            a[j * n + i] = sum / p[j];
+        }
+    }
+
+    return 0; /* success */
+}
+
+static int cholsl(float *A, float *a, float *p, int n)
+{
+    int i, j, k;
+    if (choldcsl(A, a, p, n)) {
+        return 1;
+    }
+    for (i = 0; i < n; i++) {
+        for (j = i + 1; j < n; j++) {
+            a[i * n + j] = 0.0;
+        }
+    }
+    for (i = 0; i < n; i++) {
+        a[i * n + i] *= a[i * n + i];
+        for (k = i + 1; k < n; k++) {
+            a[i * n + i] += a[k * n + i] * a[k * n + i];
+        }
+        for (j = i + 1; j < n; j++) {
+            for (k = j; k < n; k++) {
+                a[i * n + j] += a[k * n + i] * a[k * n + j];
+            }
+        }
+    }
+    for (i = 0; i < n; i++) {
+        for (j = 0; j < i; j++) {
+            a[i * n + j] = a[j * n + i];
+        }
+    }
+
+    return 0; /* success */
+}
+
+static void zeros(float *a, int m, int n)
+{
+    int j;
+    for (j = 0; j < m * n; ++j) {
+        a[j] = 0;
+    }
+}
+
+#ifdef DEBUG
+static void dump(float *a, int m, int n, const char *fmt)
+{
+    int i, j;
+
+    char f[100];
+    sprintf(f, "%s ", fmt);
+    for (i = 0; i < m; ++i) {
+        for (j = 0; j < n; ++j) {
+            printf(f, a[i * n + j]);
+        }
+        printf("\n");
+    }
+}
+#endif
+
+/* C <- A * B */
+static void mulmat(float *a, float *b, float *c, int arows, int acols, int bcols)
+{
+    int i, j, l;
+
+    for (i = 0; i < arows; ++i) {
+        for (j = 0; j < bcols; ++j) {
+            c[i * bcols + j] = 0;
+            for (l = 0; l < acols; ++l) {
+                c[i * bcols + j] += a[i * acols + l] * b[l * bcols + j];
+            }
+        }
+    }
+}
+
+static void mulvec(float *a, float *x, float *y, int m, int n)
+{
+    int i, j;
+
+    for (i = 0; i < m; ++i) {
+        y[i] = 0;
+        for (j = 0; j < n; ++j) {
+            y[i] += x[j] * a[i * n + j];
+        }
+    }
+}
+
+static void transpose(float *a, float *at, int m, int n)
+{
+    int i, j;
+
+    for (i = 0; i < m; ++i) {
+        for (j = 0; j < n; ++j) {
+            at[j * m + i] = a[i * n + j];
+        }
+    }
+}
+
+/* A <- A + B */
+static void accum(float *a, float *b, int m, int n)
+{
+    int i, j;
+
+    for (i = 0; i < m; ++i) {
+        for (j = 0; j < n; ++j) {
+            a[i * n + j] += b[i * n + j];
+        }
+    }
+}
+
+/* C <- A + B */
+static void add(float *a, float *b, float *c, int n)
+{
+    int j;
+
+    for (j = 0; j < n; ++j) {
+        c[j] = a[j] + b[j];
+    }
+}
+
+/* C <- A - B */
+static void sub(float *a, float *b, float *c, int n)
+{
+    int j;
+
+    for (j = 0; j < n; ++j) {
+        c[j] = a[j] - b[j];
+    }
+}
+
+static void negate(float *a, int m, int n)
+{
+    int i, j;
+
+    for (i = 0; i < m; ++i) {
+        for (j = 0; j < n; ++j) {
+            a[i * n + j] = -a[i * n + j];
+        }
+    }
+}
+
+static void mat_addeye(float *a, int n)
+{
+    int i;
+    for (i = 0; i < n; ++i) {
+        a[i * n + i] += 1;
+    }
+}
+
+static void unpack(void *v, EKF *ekf, int n, int m)
+{
+    /* skip over n, m in data structure */
+    char *cptr = (char *)v;
+    cptr += 2 * sizeof(int);
+
+    float *dptr = (float *)cptr;
+    ekf->x = dptr;
+    dptr += n;
+    ekf->P = dptr;
+    dptr += n * n;
+    ekf->Q = dptr;
+    dptr += n * n;
+    ekf->R = dptr;
+    dptr += m * m;
+    ekf->G = dptr;
+    dptr += n * m;
+    ekf->F = dptr;
+    dptr += n * n;
+    ekf->H = dptr;
+    dptr += m * n;
+    ekf->Ht = dptr;
+    dptr += n * m;
+    ekf->Ft = dptr;
+    dptr += n * n;
+    ekf->Pp = dptr;
+    dptr += n * n;
+    ekf->fx = dptr;
+    dptr += n;
+    ekf->hx = dptr;
+    dptr += m;
+    ekf->tmp0 = dptr;
+    dptr += n * n;
+    ekf->tmp1 = dptr;
+    dptr += n * m;
+    ekf->tmp2 = dptr;
+    dptr += m * n;
+    ekf->tmp3 = dptr;
+    dptr += m * m;
+    ekf->tmp4 = dptr;
+    dptr += m * m;
+    ekf->tmp5 = dptr;
+}
+
+void ekf_init(void *v, int n, int m)
+{
+    /* retrieve n, m and set them in incoming data structure */
+    int *ptr = (int *)v;
+    *ptr = n;
+    ptr++;
+    *ptr = m;
+
+    /* unpack rest of incoming structure for initlization */
+    EKF ekf;
+    unpack(v, &ekf, n, m);
+
+    /* zero-out matrices */
+    zeros(ekf.P, n, n);
+    zeros(ekf.Q, n, n);
+    zeros(ekf.R, m, m);
+    zeros(ekf.G, n, m);
+    zeros(ekf.F, n, n);
+    zeros(ekf.H, m, n);
+}
+
+int ekf_step(void *v, float *z)
+{
+/* unpack incoming structure */
+#ifdef DEBUG
+    printf("\n\n\n *************************** \n");
+    printf("**********Step***************");
+    printf("\n************************** \n\n\n");
+    printf("Received measured voltage: %f mV \n", z[0]);
+#endif
+    int *ptr = (int *)v;
+    int n = *ptr;
+    ptr++;
+    int m = *ptr;
+
+    EKF ekf;
+    unpack(v, &ekf, n, m);
+
+#ifdef DEBUG
+    printf("Print Matrix of model calculations ekf.hx\n");
+    dump(ekf.hx, m, m, "%f");
+    printf("Print Matrix of model calculations ekf.H\n");
+    dump(ekf.H, m, n, "%f");
+    printf("Print Matrix of model calculations ekf.x\n");
+    dump(ekf.x, n, m, "%f");
+    printf("Print Matrix of model calculations ekf.fx\n");
+    dump(ekf.fx, n, m, "%f");
+#endif
+
+    /* P_k = F_{k-1} P_{k-1} F^T_{k-1} + Q_{k-1} */
+    mulmat(ekf.F, ekf.P, ekf.tmp0, n, n, n);
+    transpose(ekf.F, ekf.Ft, n, n);
+    mulmat(ekf.tmp0, ekf.Ft, ekf.Pp, n, n, n);
+    accum(ekf.Pp, ekf.Q, n, n);
+#ifdef DEBUG
+    printf("Print Matrix ekf.Pp\n");
+    dump(ekf.Pp, n, n, "%f");
+#endif
+    /* G_k = P_k H^T_k (H_k P_k H^T_k + R)^{-1} */
+    transpose(ekf.H, ekf.Ht, m, n);
+    mulmat(ekf.Pp, ekf.Ht, ekf.tmp1, n, n, m);
+    mulmat(ekf.H, ekf.Pp, ekf.tmp2, m, n, n);
+    mulmat(ekf.tmp2, ekf.Ht, ekf.tmp3, m, n, m);
+    accum(ekf.tmp3, ekf.R, m, m);
+    if (cholsl(ekf.tmp3, ekf.tmp4, ekf.tmp5, m)) {
+#ifdef DEBUG
+        printf("cholsl returned 1");
+#endif
+        return 1;
+    }
+    mulmat(ekf.tmp1, ekf.tmp4, ekf.G, n, m, m);
+#ifdef DEBUG
+    printf("Print Matrix ekf.G\n");
+    dump(ekf.G, n, m, "%f");
+#endif
+/* \hat{x}_k = \hat{x_k} + G_k(z_k - h(\hat{x}_k)) */
+#ifdef DEBUG
+    printf("Print Matrix ekf.x before KF manipulation\n");
+    dump(ekf.x, n, m, "%f");
+    printf("Measured voltage for substraction is %f \n", z[0]);
+    printf("Estimated voltage is Matrix ekf.hx: \n");
+    dump(ekf.hx, m, m, "%f");
+#endif
+    sub(z, ekf.hx, ekf.tmp5, m);
+#ifdef DEBUG
+    printf("Print Matrix Diff (z-hx) \n");
+    dump(ekf.tmp5, m, m, "%f");
+#endif
+    mulvec(ekf.G, ekf.tmp5, ekf.tmp2, n, m);
+    add(ekf.fx, ekf.tmp2, ekf.x, n);
+#ifdef DEBUG
+    printf("Print Matrix ekf.x \n");
+    dump(ekf.x, n, m, "%f");
+#endif
+    /* P_k = (I - G_k H_k) P_k */
+    mulmat(ekf.G, ekf.H, ekf.tmp0, n, m, n);
+    negate(ekf.tmp0, n, n);
+    mat_addeye(ekf.tmp0, n);
+    mulmat(ekf.tmp0, ekf.Pp, ekf.P, n, n, n);
+#ifdef DEBUG
+    printf("Print Matrix ekf.P\n");
+    dump(ekf.P, n, n, "%f");
+#endif
+
+    /* success */
+    return 0;
+}
diff --git a/app/src/kalman_soc.h b/app/src/kalman_soc.h
new file mode 100644
index 00000000..e1e2155c
--- /dev/null
+++ b/app/src/kalman_soc.h
@@ -0,0 +1,25 @@
+/*
+ * TinyEKF: Extended Kalman Filter for embedded processors
+ *
+ * Copyright (C) 2015 Simon D. Levy
+ *
+ * MIT License
+ */
+
+/**
+ * @brief
+ *
+ * @param v Battery voltage to calculate the inital SoC for the EKF
+ * @param n Matrix dimension n columns
+ * @param m Matrix dimension m rows
+ */
+void ekf_init(void *v, int n, int m);
+
+/**
+ * @brief
+ *
+ * @param v Pointer to struct containing EKF Data.
+ * @param z Pointer to voltage measurement for the iteration
+ * @return int Return success, dependend on numercial stability.
+ */
+int ekf_step(void *v, float *z);
diff --git a/app/src/main.cpp b/app/src/main.cpp
index 8d6bccc1..ab2455e9 100644
--- a/app/src/main.cpp
+++ b/app/src/main.cpp
@@ -27,6 +27,7 @@
 #include "leds.h"       // LED switching using charlieplexing
 #include "load.h"       // load and USB output management
 #include "pwm_switch.h" // PWM charge controller
+#include "kalman_soc.h"         // caculation of SoC using extended Kalman filter
 
 void main(void)
 {
@@ -60,7 +61,7 @@ void main(void)
     daq_setup();
 
     charger.detect_num_batteries(&bat_conf); // check if we have 24V instead of 12V system
-    charger.init_terminal(&bat_conf);
+    charger.init_terminal(&bat_conf, &ekf_soc);
 
 #if BOARD_HAS_LOAD_OUTPUT
     load.set_voltage_limits(bat_conf.load_disconnect_voltage, bat_conf.load_reconnect_voltage,
@@ -106,7 +107,7 @@ void main(void)
 
         dev_stat.update_energy();
         dev_stat.update_min_max_values();
-        charger.update_soc(&bat_conf);
+        charger.update_soc(&bat_conf,&ekf_soc);
 
 #if CONFIG_HS_MOSFET_FAIL_SAFE_PROTECTION && BOARD_HAS_DCDC
         if (dev_stat.has_error(ERR_DCDC_HS_MOSFET_SHORT)) {
diff --git a/app/src/setup.cpp b/app/src/setup.cpp
index 2bf9ecac..61bfd354 100644
--- a/app/src/setup.cpp
+++ b/app/src/setup.cpp
@@ -75,6 +75,7 @@ Charger charger(&bat_terminal);
 
 BatConf bat_conf;      // actual (used) battery configuration
 BatConf bat_conf_user; // temporary storage where the user can write to
+EkfSoc ekf_soc;        // Extended Kalman filter configuration
 
 DeviceStatus dev_stat;
 
diff --git a/app/src/setup.h b/app/src/setup.h
index bedf5ef4..5a653bb7 100644
--- a/app/src/setup.h
+++ b/app/src/setup.h
@@ -37,6 +37,7 @@ extern DeviceStatus dev_stat;
 extern Charger charger;
 extern BatConf bat_conf;
 extern BatConf bat_conf_user;
+extern EkfSoc ekf_soc;
 
 #if BOARD_HAS_LOAD_OUTPUT
 extern LoadOutput load;
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 389b4c1b..59a7fd92 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -33,6 +33,7 @@ target_sources(app PRIVATE
         src/tests_half_bridge.cpp
         src/tests_load.cpp
         src/tests_power_port.cpp
+        src/tests_kalman_soc.cpp
 )
 
 # determine git tag and commit hash for automatic firmware versioning
diff --git a/tests/src/main.cpp b/tests/src/main.cpp
index 4b3ddff2..4c808e92 100644
--- a/tests/src/main.cpp
+++ b/tests/src/main.cpp
@@ -33,6 +33,7 @@ void main()
     err += half_bridge_tests();
     err += dcdc_tests();
     err += device_status_tests();
+    err += kalman_soc_tests();
     err += load_tests();
 
 #ifdef CONFIG_CUSTOM_TESTS
diff --git a/tests/src/tests.h b/tests/src/tests.h
index c0d0eb60..3c2af09f 100644
--- a/tests/src/tests.h
+++ b/tests/src/tests.h
@@ -19,6 +19,8 @@ int dcdc_tests();
 
 int device_status_tests();
 
+int kalman_soc_tests();
+
 int load_tests();
 
 #ifdef CONFIG_CUSTOM_TESTS
diff --git a/tests/src/tests_bat_charger.cpp b/tests/src/tests_bat_charger.cpp
index 79922bb5..f1556b34 100644
--- a/tests/src/tests_bat_charger.cpp
+++ b/tests/src/tests_bat_charger.cpp
@@ -14,7 +14,7 @@
 static void init_structs()
 {
     battery_conf_init(&bat_conf, BAT_TYPE_FLOODED, 6, 100);
-    charger.init_terminal(&bat_conf);
+    charger.init_terminal(&bat_conf, &ekf_soc);
     charger.state = CHG_STATE_IDLE;
     charger.bat_temperature = 25;
     bat_terminal.bus->voltage = 14.0;
diff --git a/tests/src/tests_dcdc.cpp b/tests/src/tests_dcdc.cpp
index ed0e156d..4b49a956 100644
--- a/tests/src/tests_dcdc.cpp
+++ b/tests/src/tests_dcdc.cpp
@@ -26,7 +26,7 @@ static void init_structs_buck(int num_batteries = 1)
 
     battery_conf_init(&bat_conf, BAT_TYPE_GEL, 6, 100);
     charger.port = &lv_terminal;
-    charger.init_terminal(&bat_conf);
+    charger.init_terminal(&bat_conf, &ekf_soc);
     lv_terminal.bus->voltage = 14 * num_batteries;
     lv_terminal.bus->series_multiplier = num_batteries;
     lv_terminal.current = 0;
@@ -68,7 +68,7 @@ static void init_structs_boost(int num_batteries = 1)
     int num_cells = (num_batteries == 1) ? 10 : 5;
     battery_conf_init(&bat_conf, BAT_TYPE_NMC, num_cells, 9);
     charger.port = &hv_terminal;
-    charger.init_terminal(&bat_conf);
+    charger.init_terminal(&bat_conf, &ekf_soc);
     hv_terminal.bus->voltage = 3.7 * num_cells * num_batteries;
     hv_terminal.bus->series_multiplier = num_batteries;
     hv_terminal.current = 0;
diff --git a/tests/src/tests_kalman_soc.cpp b/tests/src/tests_kalman_soc.cpp
new file mode 100644
index 00000000..b47779ab
--- /dev/null
+++ b/tests/src/tests_kalman_soc.cpp
@@ -0,0 +1,901 @@
+/*
+ * Copyright (c) The Libre Solar Project Contributors
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "tests.h"
+#include <setup.h>
+#include <stdio.h>
+#include <time.h>
+
+#include "bat_charger.h"
+#include <math.h>
+#include <stdlib.h>
+#include <string.h>
+#include <strings.h>
+
+//#define DEBUG 0
+
+// GPS Model Backtest of TinyEKF Lib
+// See for implemented test example:
+// https://github.com/simondlevy/TinyEKF/tree/master/extras/c
+
+/* states */
+#define Nsta_gps 8
+
+/* observables */
+#define Nobs_gps 4
+
+typedef struct
+{
+
+    int n; /* number of state values */
+    int m; /* number of observables */
+
+    float x[Nsta_gps]; /* state vector */
+
+    float P[Nsta_gps][Nsta_gps]; /* prediction error covariance */
+    float Q[Nsta_gps][Nsta_gps]; /* process noise covariance */
+    float R[Nobs_gps][Nobs_gps]; /* measurement error covariance */
+
+    float G[Nsta_gps][Nobs_gps]; /* Kalman gain; a.k.a. K */
+
+    float F[Nsta_gps][Nsta_gps]; /* Jacobian of process model */
+    float H[Nobs_gps][Nsta_gps]; /* Jacobian of measurement model */
+
+    float Ht[Nsta_gps][Nobs_gps]; /* transpose of measurement Jacobian */
+    float Ft[Nsta_gps][Nsta_gps]; /* transpose of process Jacobian */
+    float Pp[Nsta_gps][Nsta_gps]; /* P, post-prediction, pre-update */
+
+    float fx[Nsta_gps]; /* output of user defined f() state-transition function */
+    float hx[Nobs_gps]; /* output of user defined h() measurement function */
+
+    /* temporary storage */
+    float tmp0[Nsta_gps][Nsta_gps];
+    float tmp1[Nsta_gps][Nobs_gps];
+    float tmp2[Nobs_gps][Nsta_gps];
+    float tmp3[Nobs_gps][Nobs_gps];
+    float tmp4[Nobs_gps][Nobs_gps];
+    float tmp5[Nobs_gps];
+
+} ekf_gps_t;
+
+// positioning interval
+static const float T = 1;
+
+static void blkfill(ekf_gps_t *ekf_gps, const float *a, int off)
+{
+    off *= 2;
+
+    ekf_gps->Q[off][off] = a[0];
+    ekf_gps->Q[off][off + 1] = a[1];
+    ekf_gps->Q[off + 1][off] = a[2];
+    ekf_gps->Q[off + 1][off + 1] = a[3];
+}
+
+static void init_gps(ekf_gps_t *ekf_gps)
+{
+    // Set Q, see [1]
+    const float Sf = 36;
+    const float Sg = 0.01;
+    const float sigma = 5; // state transition variance
+    const float Qb[4] = { Sf * T + Sg * T * T * T / 3, Sg * T * T / 2, Sg * T * T / 2, Sg * T };
+    const float Qxyz[4] = { sigma * sigma * T * T * T / 3, sigma * sigma * T * T / 2,
+                            sigma * sigma * T * T / 2, sigma * sigma * T };
+
+    blkfill(ekf_gps, Qxyz, 0);
+    blkfill(ekf_gps, Qxyz, 1);
+    blkfill(ekf_gps, Qxyz, 2);
+    blkfill(ekf_gps, Qb, 3);
+
+    // initial covariances of state noise, measurement noise
+    float P0 = 10;
+    float R0 = 36;
+
+    int i;
+
+    for (i = 0; i < Nsta_gps; ++i) {
+        ekf_gps->P[i][i] = P0;
+    }
+
+    for (i = 0; i < Nobs_gps; ++i) {
+        ekf_gps->R[i][i] = R0;
+    }
+
+    // position
+    ekf_gps->x[0] = -2.168816181271560e+006;
+    ekf_gps->x[2] = 4.386648549091666e+006;
+    ekf_gps->x[4] = 4.077161596428751e+006;
+
+    // velocity
+    ekf_gps->x[1] = 0;
+    ekf_gps->x[3] = 0;
+    ekf_gps->x[5] = 0;
+
+    // clock bias
+    ekf_gps->x[6] = 3.575261153706439e+006;
+
+    // clock drift
+    ekf_gps->x[7] = 4.549246345845814e+001;
+}
+
+static void model_gps(ekf_gps_t *ekf_gps, float SV[4][3])
+{
+
+    int i, j;
+
+    for (j = 0; j < 8; j += 2) {
+        ekf_gps->fx[j] = ekf_gps->x[j] + T * ekf_gps->x[j + 1];
+        ekf_gps->fx[j + 1] = ekf_gps->x[j + 1];
+    }
+
+    for (j = 0; j < 8; ++j) {
+        ekf_gps->F[j][j] = 1;
+    }
+    for (j = 0; j < 4; ++j) {
+        ekf_gps->F[2 * j][2 * j + 1] = T;
+    }
+    float dx[4][3];
+
+    for (i = 0; i < 4; ++i) {
+        ekf_gps->hx[i] = 0;
+        for (j = 0; j < 3; ++j) {
+            float d = ekf_gps->fx[j * 2] - SV[i][j];
+            dx[i][j] = d;
+            ekf_gps->hx[i] += d * d;
+        }
+        ekf_gps->hx[i] = pow(ekf_gps->hx[i], 0.5) + ekf_gps->fx[6];
+    }
+
+    for (i = 0; i < 4; ++i) {
+        for (j = 0; j < 3; ++j) {
+            ekf_gps->H[i][j * 2] = dx[i][j] / ekf_gps->hx[i];
+        }
+        ekf_gps->H[i][6] = 1;
+    }
+}
+
+#define datasetcolumns 25
+
+typedef struct
+{
+    float P11[datasetcolumns] = { -11602023.9489137, -11602700.409615,  -11603377.0261803,
+                                  -11604053.7986268, -11604730.7269448, -11605407.8111641,
+                                  -11606085.0512816, -11606762.44731,   -11607439.9992582,
+                                  -11608117.7071296, -11608795.5709421, -11609473.590699,
+                                  -11610151.7664095, -11610830.0980858, -11611508.5857306,
+                                  -11612187.2293523, -11612866.0289661, -11613544.9845813,
+                                  -11614224.0961969, -11614903.3638345, -11615582.7874894,
+                                  -11616262.3671785, -11616942.1029125, -11617621.9946924,
+                                  -11618302.042535 };
+    float P12[datasetcolumns] = {
+        14063117.4931116, 14060708.163762,  14058298.6961425, 14055889.0902859, 14053479.3463229,
+        14051069.4642412, 14048659.444148,  14046249.2860925, 14043838.9901378, 14041428.5563671,
+        14039017.9848112, 14036607.2755532, 14034196.4286552, 14031785.4441682, 14029374.3221777,
+        14026963.0627483, 14024551.6659205, 14022140.1317561, 14019728.4603524, 14017316.6517278,
+        14014904.7059932, 14012492.6231827, 14010080.4033526, 14007668.0465943, 14005255.5529418
+    };
+    float P13[datasetcolumns] = {
+        18811434.3112746, 18812823.4023028, 18814212.0761809, 18815600.3328957, 18816988.1723781,
+        18818375.5946411, 18819762.5996289, 18821149.1873193, 18822535.3576819, 18823921.1106749,
+        18825306.4462865, 18826691.3644751, 18828075.8652108, 18829459.9484704, 18830843.6142108,
+        18832226.8624009, 18833609.6930235, 18834992.1060491, 18836374.1014277, 18837755.6791551,
+        18839136.8391736, 18840517.5814695, 18841897.9060167, 18843277.8127688, 18844657.3017124
+    };
+    float P21[datasetcolumns] = { -20853271.5736342, -20855049.9291186, -20856828.1167654,
+                                  -20858606.1364935, -20860383.9882668, -20862161.6719905,
+                                  -20863939.1876304, -20865716.5351111, -20867493.7143885,
+                                  -20869270.7253602, -20871047.5680138, -20872824.2422708,
+                                  -20874600.7480677, -20876377.0853387, -20878153.2540492,
+                                  -20879929.2540921, -20881705.0854518, -20883480.7480603,
+                                  -20885256.2418182, -20887031.5667084, -20888806.7226403,
+                                  -20890581.7095696, -20892356.5274465, -20894131.1761796,
+                                  -20895905.6557381 };
+    float P22[datasetcolumns] = {
+        1806977.21185816, 1805887.13065807, 1804797.28049813, 1803707.66138322, 1802618.27329107,
+        1801529.116235,   1800440.19019116, 1799351.49516123, 1798263.03112756, 1797174.79810804,
+        1796086.79606563, 1794999.0250037,  1793911.48491644, 1792824.17579945, 1791737.097629,
+        1790650.25042617, 1789563.63415556, 1788477.24881424, 1787391.09441818, 1786305.17093311,
+        1785219.47836965, 1784134.01671018, 1783048.78594032, 1781963.78607134, 1780879.01707716
+    };
+    float P23[datasetcolumns] = {
+        16542682.1237923, 16540582.4659657, 16538482.4609004, 16536382.1086646, 16534281.4092741,
+        16532180.3628133, 16530078.9692955, 16527977.2287825, 16525875.1412993, 16523772.7069394,
+        16521669.9256904, 16519566.797618,  16517463.3227698, 16515359.5011966, 16513255.3329117,
+        16511150.818015,  16509045.9564974, 16506940.7484123, 16504835.1938502, 16502729.2928038,
+        16500623.0453534, 16498516.4515241, 16496409.5113475, 16494302.2249049, 16492194.5922053
+    };
+    float P31[datasetcolumns] = { -14355926.017234,  -14356344.1729806, -14356762.4791434,
+                                  -14357180.9357223, -14357599.5427193, -14358018.3001422,
+                                  -14358437.2079998, -14358856.2662888, -14359275.4750231,
+                                  -14359694.8342019, -14360114.3438323, -14360534.0039196,
+                                  -14360953.8144662, -14361373.7754756, -14361793.8869582,
+                                  -14362214.1489185, -14362634.5613554, -14363055.1242758,
+                                  -14363475.8376898, -14363896.7015941, -14364317.715999,
+                                  -14364738.880906,  -14365160.1963243, -14365581.6622592,
+                                  -14366003.2787072 };
+    float P32[datasetcolumns] = {
+        8650961.88410982, 8648384.47686198, 8645806.99474651, 8643229.4378562,  8640651.80627305,
+        8638074.10004161, 8635496.31920119, 8632918.4638651,  8630340.53404078, 8627762.52982713,
+        8625184.45127231, 8622606.29843616, 8620028.07139851, 8617449.77022857, 8614871.39495658,
+        8612292.94564608, 8609714.42239723, 8607135.82525976, 8604557.15426399, 8601978.40952272,
+        8599399.59106402, 8596820.69897167, 8594241.73327994, 8591662.69405015, 8589083.58139421
+    };
+    float P33[datasetcolumns] = {
+        20736354.9805864, 20737164.3397034, 20737973.2627679, 20738781.7497543, 20739589.8006407,
+        20740397.4154165, 20741204.5940731, 20742011.3365787, 20742817.6429346, 20743623.5131133,
+        20744428.9471034, 20745233.94489,   20746038.5064515, 20746842.6317701, 20747646.3208399,
+        20748449.5736446, 20749252.3901568, 20750054.7703644, 20750856.7142618, 20751658.2218172,
+        20752459.2930258, 20753259.9278649, 20754060.1263275, 20754859.8883983, 20755659.214046
+    };
+    float P41[datasetcolumns] = {
+        7475239.67530529, 7472917.32156931, 7470595.0720982,  7468272.92694682, 7465950.88614163,
+        7463628.94979391, 7461307.1179005,  7458985.39057082, 7456663.76782936, 7454342.24973383,
+        7452020.83634093, 7449699.52774197, 7447378.32393047, 7445057.2250017,  7442736.23102901,
+        7440415.34201686, 7438094.55805635, 7435773.87918626, 7433453.30548462, 7431132.83699074,
+        7428812.47375867, 7426492.21586256, 7424172.06332343, 7421852.01624228, 7419532.07462828
+    };
+    float P42[datasetcolumns] = {
+        12966181.2771377, 12967714.4596339, 12969247.7736988, 12970781.2192928, 12972314.7963952,
+        12973848.5049293, 12975382.344894,  12976916.3162136, 12978450.4188688, 12979984.6528181,
+        12981519.0180208, 12983053.5144131, 12984588.1419961, 12986122.9007034, 12987657.7904831,
+        12989192.8113291, 12990727.9631777, 12992263.2459998, 12993798.6597405, 12995334.2043704,
+        12996869.8798502, 12998405.6861275, 12999941.6231851, 13001477.6909524, 13003013.8894202
+    };
+    float P43[datasetcolumns] = {
+        21931576.7921751, 21931442.6029888, 21931307.9468087, 21931172.8236371, 21931037.233474,
+        21930901.1763249, 21930764.6521883, 21930627.6610695, 21930490.2029686, 21930352.2778878,
+        21930213.8858294, 21930075.0267975, 21929935.7007905, 21929795.9078129, 21929655.647868,
+        21929514.9209546, 21929373.7270772, 21929232.0662369, 21929089.9384372, 21928947.3436792,
+        21928804.2819651, 21928660.7532982, 21928516.7576786, 21928372.2951113, 21928227.3655957
+    };
+    float R1[datasetcolumns] = {
+        23568206.4173783, 23568427.7909862, 23568650.0894557, 23568869.5260895, 23569094.4420916,
+        23569315.4143446, 23569537.8873163, 23569760.0636344, 23569981.9083983, 23570205.8646385,
+        23570427.8664544, 23570650.321976,  23570873.1090517, 23571094.6397118, 23571317.6536404,
+        23571542.272989,  23571765.635922,  23571987.5330366, 23572212.1698355, 23572433.9098983,
+        23572658.6513985, 23572882.7297905, 23573105.2551131, 23573329.6650593, 23573552.3125334
+    };
+    float R2[datasetcolumns] = {
+        26183921.457745,  26184404.1127416, 26184884.7086125, 26185366.6481502, 26185845.7782029,
+        26186327.8049918, 26186808.2263608, 26187289.5027905, 26187768.842246,  26188253.1899141,
+        26188734.3965431, 26189215.4635703, 26189696.8272514, 26190179.3251966, 26190658.5076005,
+        26191142.2270611, 26191622.8229328, 26192101.5167307, 26192584.8348365, 26193065.3609074,
+        26193548.1555067, 26194030.4265996, 26194510.3070126, 26194992.9794606, 26195473.36593
+    };
+    float R3[datasetcolumns] = {
+        24652215.2627705, 24652621.9011857, 24653025.2764103, 24653428.8435874, 24653834.853795,
+        24654241.1781066, 24654645.1117385, 24655052.4830633, 24655456.8704009, 24655862.4792539,
+        24656267.6169511, 24656671.8995876, 24657077.3339386, 24657484.6529132, 24657890.0872643,
+        24658293.6893426, 24658699.8217026, 24659106.9487251, 24659511.3186132, 24659918.7073891,
+        24660325.0840524, 24660732.8916336, 24661138.8145914, 24661542.6609733, 24661950.1370006
+    };
+    float R4[datasetcolumns] = {
+        25606982.9330466, 25606499.4748001, 25606016.697112,  25605534.4603806, 25605048.9604585,
+        25604567.3344846, 25604081.9392636, 25603599.6850818, 25603116.4885881, 25602632.6115359,
+        25602148.1411763, 25601667.632016,  25601183.0395047, 25600699.4416557, 25600219.0895472,
+        25599735.3346461, 25599252.7314594, 25598769.0638094, 25598287.1935317, 25597804.9916998,
+        25597322.2140106, 25596841.2162436, 25596357.5136928, 25595876.9347309, 25595393.4415826
+    };
+} dataset_gps_t;
+
+void test_backtest_gps()
+{
+    dataset_gps_t dataset;
+
+    // Do generic EKF initialization
+    ekf_gps_t ekf_gps;
+    ekf_init(&ekf_gps, Nsta_gps, Nobs_gps);
+
+    // Do local initialization
+    init_gps(&ekf_gps);
+
+    // Make a place to store the data from the file and the output of the EKF
+    float SV_Pos[4][3];
+    float SV_Rho[4];
+    float Pos_KF[25][3];
+
+    int j, k;
+
+    // Loop till no more data
+    for (j = 0; j < 25; ++j) {
+
+        // Load iteration of dataset
+        SV_Pos[0][0] = dataset.P11[j];
+        SV_Pos[0][1] = dataset.P12[j];
+        SV_Pos[0][2] = dataset.P13[j];
+        SV_Pos[1][0] = dataset.P21[j];
+        SV_Pos[1][1] = dataset.P22[j];
+        SV_Pos[1][2] = dataset.P23[j];
+        SV_Pos[2][0] = dataset.P31[j];
+        SV_Pos[2][1] = dataset.P32[j];
+        SV_Pos[2][2] = dataset.P33[j];
+        SV_Pos[3][0] = dataset.P41[j];
+        SV_Pos[3][1] = dataset.P42[j];
+        SV_Pos[3][2] = dataset.P43[j];
+
+        SV_Rho[0] = dataset.R1[j];
+        SV_Rho[1] = dataset.R2[j];
+        SV_Rho[2] = dataset.R3[j];
+        SV_Rho[3] = dataset.R4[j];
+
+        model_gps(&ekf_gps, SV_Pos);
+
+        ekf_step(&ekf_gps, SV_Rho);
+
+        // grab positions, ignoring velocities
+        for (k = 0; k < 3; ++k) {
+            Pos_KF[j][k] = ekf_gps.x[2 * k];
+        }
+    }
+
+    // Compute means of filtered positions
+    float mean_Pos_KF[3] = { 0, 0, 0 };
+    for (j = 0; j < 25; ++j) {
+        for (k = 0; k < 3; ++k) {
+            mean_Pos_KF[k] += Pos_KF[j][k];
+        }
+    }
+    for (k = 0; k < 3; ++k) {
+        mean_Pos_KF[k] /= 25;
+    }
+
+    // Debugging Dump filtered positions minus their means
+    for (j = 0; j < 25; ++j) {
+        // printf("%f ,%f ,%f\n", Pos_KF[j][0]-mean_Pos_KF[0], Pos_KF[j][1]-mean_Pos_KF[1],
+        // Pos_KF[j][2]-mean_Pos_KF[2]); printf("%f %f %f\n", Pos_KF[j][0], Pos_KF[j][1],
+        // Pos_KF[j][2]);
+    }
+
+    TEST_ASSERT_FLOAT_WITHIN(0.00001, -1.61, Pos_KF[24][0] - mean_Pos_KF[0]);
+    TEST_ASSERT_FLOAT_WITHIN(0.00001, 0.5, Pos_KF[24][1] - mean_Pos_KF[1]);
+    TEST_ASSERT_FLOAT_WITHIN(0.00001, -0.58, Pos_KF[24][2] - mean_Pos_KF[2]);
+}
+
+// TinyEKF test with SoC model
+
+void test_ekf_init_func()
+{
+
+    // ekf_soc_t ekf_soc
+    ekf_soc.P[0][0] = 5;
+    ekf_init(&ekf_soc, NUMBER_OF_STATES_SOC, NUMBER_OF_OBSERVABLES_SOC);
+    TEST_ASSERT_EQUAL(true, ekf_soc.P[0][0] == 0.0 && ekf_soc.Q[0][0] == 0.0
+                                && ekf_soc.R[0][0] == 0.0 && ekf_soc.G[0][0] == 0.0
+                                && ekf_soc.F[0][0] == 0.0 && ekf_soc.H[0][0] == 0.0);
+
+    // TEST_ASSERT_EQUAL_FLOAT(0.0, ekf_soc.P[0][0]);
+    // TEST_ASSERT_EACH_EQUAL_FLOAT(0.0,*ekf_soc.P,1); // not working yet
+    // TEST_ASSERT_EQUAL_FLOAT(0,0)
+}
+
+// TODO Update to SoC
+void test_ekf_step_func()
+{
+    ekf_gps_t ekf_gps;
+    float SV_Rho[4];
+
+    ekf_init(&ekf_gps, Nsta_gps, Nobs_gps);
+    ekf_gps.P[0][0] = 5;
+    ekf_step(&ekf_gps, SV_Rho);
+    TEST_ASSERT_EQUAL(true, ekf_gps.P[0][0] == 5.0 && ekf_gps.Q[0][0] == 0.0
+                                && ekf_gps.R[0][0] == 0.0 && ekf_gps.G[0][0] == 0.0
+                                && ekf_gps.F[0][0] == 0.0 && ekf_gps.H[0][0] == 0.0);
+
+    // TEST_ASSERT_EQUAL_FLOAT(0.0, ekf_gps.P[0][0]);
+    // TEST_ASSERT_EACH_EQUAL_FLOAT(0.0,&ekf_gps.P,4); // not working yet
+    // TEST_ASSERT_EQUAL_FLOAT(0,0)
+}
+
+/// Test all functions implemented in bat_charger.cpp h & f and init_SoC functions
+
+void test_clamp_func()
+{
+    float value, min, max, result;
+    min = 0;
+    max = 100000;
+    value = 200000;
+    result = clamp(value, min, max);
+    TEST_ASSERT_FLOAT_WITHIN(0, 100000, result);
+}
+
+void test_calculate_initial_soc_func()
+{
+    float initialSoC, batteryVoltagemV;
+    batteryVoltagemV = 12000;
+    initialSoC = calculate_initial_soc(batteryVoltagemV);
+    TEST_ASSERT_FLOAT_WITHIN(0, 30000, initialSoC);
+}
+
+void test_init_soc_func_should_init_with_calculated_soc()
+{
+    // ekf_soc_t  ekf_soc
+    float P0 = 0.1;   // initial covariance of state noise  (aka process noise)
+    float Q0 = 0.001; // Initial state uncertainty covariance matrix
+    float R0 = 0.1;   // initial covariance of measurement noise
+
+    // uint32_t batteryEff = 10;
+    float v0 = 13000;
+    float initialSoC = 0xFFFFFFFFFFFFFFFF; // forces new SoC to be calculated
+    init_soc(&ekf_soc, v0, P0, Q0, R0, initialSoC);
+    TEST_ASSERT_FLOAT_WITHIN(0, 100000, ekf_soc.x[0]);
+}
+
+void test_init_soc_func_should_init_with_initial_soc()
+{
+    // ekf_soc_t  ekf_soc
+    float P0 = 0.1;   // initial covariance of state noise  (aka process noise)
+    float Q0 = 0.001; // Initial state uncertainty covariance matrix
+    float R0 = 0.1;   // initial covariance of measurement noise
+    // uint32_t batteryEff = 10;
+    float v0 = 13000;
+    float initialSoC = 10.0; // forces new SoC to be calculated
+
+    // uint32_t initialSoC = 10;
+    init_soc(&ekf_soc, v0, P0, Q0, R0, initialSoC);
+    TEST_ASSERT_FLOAT_WITHIN(0, 10, ekf_soc.x[0]);
+}
+
+void test_f_func()
+{
+    // ekf_soc_t  ekf_soc
+    bool isBatteryInFloat = false;
+    float batteryEff = 100000;
+    float batteryCurrentmA = 1000;
+    float samplePeriodMilliSec = 100;
+    float batteryCapacity = 50;
+    f(&ekf_soc, isBatteryInFloat, batteryEff, batteryCurrentmA, samplePeriodMilliSec,
+      batteryCapacity);
+}
+
+void test_h_func()
+{
+    // ekf_soc_t  ekf_soc
+    float batteryCurrentmA = 1000;
+    h(&ekf_soc, batteryCurrentmA);
+}
+
+void test_should_increase_soc_no_float_leadacid_12V()
+{
+    int cholsl_error = 0;
+    const uint32_t SOC_SCALED_HUNDRED_PERCENT = 100000;
+
+    // Do generic EKF initialization
+    // ekf_soc_t ekf_soc
+    ekf_init(&ekf_soc, NUMBER_OF_STATES_SOC, NUMBER_OF_OBSERVABLES_SOC);
+
+    // Do local initialization
+    float P0 = 0.1;   // initial covariance of state noise  (aka process noise)
+    float Q0 = 0.001; // Initial state uncertainty covariance matrix
+    float R0 = 0.1;   // initial covariance of measurement noise
+    float batteryVoltagemV[1] = {
+        12500
+    }; // intial Voltage measurement to calculate SoC if initialSoc is out of range
+    const float batteryCapacityAh = 50;
+    float initialSoC = 50000;
+
+    float batteryEff = 100000;
+    batteryVoltagemV[0] = 12500;
+    float batteryCurrentmA = 1000;
+    float samplePeriodMilliSec = 1000;
+    bool isBatteryInFloat = false;
+
+    float expectedResult = 50053.7539;
+#ifdef DEBUG
+    printf("The SoC before init_soc %f \n", ekf_soc.x[0]);
+#endif
+
+    init_soc(&ekf_soc, batteryVoltagemV[0], P0, Q0, R0, initialSoC);
+
+#ifdef DEBUG
+    printf("The SoC by init_soc %f \n", ekf_soc.x[0]);
+#endif
+
+    batteryEff = model_soc(&ekf_soc, isBatteryInFloat, batteryEff, batteryCurrentmA,
+                           samplePeriodMilliSec, batteryCapacityAh);
+
+#ifdef DEBUG
+    printf("battvol inside test %f \n", batteryVoltagemV[0]);
+    printf("The SoC before ekf_step %f \n", ekf_soc.x[0]);
+#endif
+
+    cholsl_error = ekf_step(&ekf_soc, batteryVoltagemV);
+
+#ifdef DEBUG
+    if (cholsl_error != 0) {
+        printf("EKFSTEP Failed %d \n", cholsl_error);
+    }
+    printf("The SoC before clamp %f \n", ekf_soc.x[0]);
+#endif
+
+    ekf_soc.x[0] = clamp((float)ekf_soc.x[0], 0, SOC_SCALED_HUNDRED_PERCENT);
+
+    TEST_ASSERT_FLOAT_WITHIN(1, expectedResult, ekf_soc.x[0]);
+}
+
+void test_update_soc_should_increase_soc_no_float_leadacid_12V()
+{
+    float expectedResult = 50053.7539;
+    charger.soc = 50;
+    charger.port->bus->voltage = 12.500;
+    charger.init_terminal(&bat_conf, &ekf_soc);
+
+    bat_conf.float_enabled = false;
+    bat_conf.nominal_capacity = 50;
+    charger.port->bus->voltage = 12.500;
+    charger.port->current = 1;
+    charger.update_soc(&bat_conf, &ekf_soc);
+#ifdef DEBUG
+    printf("Soc after EKF and clamp %f\n", ekf_soc.x[0]);
+#endif
+
+    TEST_ASSERT_FLOAT_WITHIN(1, expectedResult, ekf_soc.x[0]);
+}
+
+//// SoC Backtest
+
+#define datasetcolumns_SoC 996
+
+typedef struct
+{
+    float batteryVoltagemV[datasetcolumns_SoC] = {
+        12230, 12240, 12240, 12230, 12230, 12240, 12230, 12230, 12520, 12540, 12560, 12570, 12580,
+        12590, 12590, 12610, 12600, 12610, 12610, 12620, 12620, 12630, 12640, 12630, 12640, 12640,
+        12640, 12640, 12650, 12660, 12660, 12660, 12660, 12670, 12670, 12670, 12670, 12670, 12670,
+        12680, 12670, 12680, 12680, 12680, 12690, 12680, 12680, 12680, 12690, 12680, 12680, 12680,
+        12690, 12690, 12690, 12690, 12690, 12690, 12690, 12690, 12700, 12690, 12690, 12690, 12700,
+        12700, 12700, 12700, 12700, 12700, 12700, 12700, 12700, 12700, 12700, 12700, 12710, 12700,
+        12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710,
+        12710, 12710, 12710, 12710, 12720, 12720, 12710, 12710, 12710, 12720, 12710, 12720, 12720,
+        12720, 12720, 12720, 12730, 12720, 12720, 12720, 12720, 12730, 12720, 12720, 12720, 12720,
+        12720, 12720, 12720, 12720, 12720, 12720, 12720, 12720, 12720, 12720, 12730, 12730, 12730,
+        12720, 12720, 12720, 12720, 12720, 12720, 12730, 12720, 12720, 12720, 12720, 12720, 12730,
+        12730, 12730, 12730, 12730, 12730, 12740, 12720, 12730, 12730, 12740, 12730, 12730, 12730,
+        12730, 12730, 12730, 12730, 12730, 12730, 12740, 12730, 12730, 12730, 12730, 12730, 12730,
+        12730, 12740, 12730, 12730, 12730, 12730, 12730, 12730, 12730, 12730, 12730, 12730, 12740,
+        12730, 12740, 12730, 12730, 12730, 12740, 12730, 12740, 12740, 12730, 12730, 12730, 12730,
+        12730, 12740, 12730, 12740, 12730, 12740, 12740, 12740, 12730, 12740, 12740, 12740, 12740,
+        12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12750, 12740, 12740, 12740, 12740,
+        12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740,
+        12740, 12740, 12740, 12740, 12740, 12740, 12750, 12740, 12740, 12740, 12740, 12750, 12740,
+        12750, 12740, 12740, 12750, 12740, 12740, 12740, 12740, 12750, 12750, 12740, 12750, 12740,
+        12750, 12740, 12740, 12750, 12740, 12740, 12750, 12750, 12740, 12740, 12750, 12740, 12750,
+        12750, 12750, 12750, 12750, 12750, 12740, 12750, 12740, 12750, 12740, 12750, 12760, 12750,
+        12740, 12750, 12750, 12760, 12750, 12760, 12750, 12750, 12750, 12750, 12760, 12750, 12750,
+        12760, 12750, 12760, 12750, 12750, 12750, 12750, 12750, 12750, 12750, 12750, 12750, 12750,
+        12750, 12750, 12750, 12750, 12750, 12750, 12750, 12760, 12750, 12760, 12760, 12760, 12750,
+        12760, 12750, 12750, 12750, 12750, 12760, 12760, 12750, 12750, 12750, 12750, 12750, 12760,
+        12750, 12750, 12750, 12750, 12750, 12750, 12750, 12750, 12750, 12760, 12750, 12760, 12750,
+        12760, 12750, 12750, 12760, 12750, 12750, 12750, 12760, 12750, 12750, 12750, 12750, 12750,
+        12750, 12750, 12750, 12770, 12750, 12760, 12760, 12750, 12750, 12750, 12750, 12760, 12750,
+        12750, 12760, 12750, 12750, 12750, 12750, 12750, 12760, 12750, 12750, 12750, 12750, 12750,
+        12760, 12750, 12750, 12760, 12760, 12760, 12760, 12760, 12760, 12750, 12760, 12750, 12750,
+        12750, 12760, 12760, 12750, 12750, 12760, 12750, 12760, 12750, 12760, 12770, 12750, 12760,
+        12750, 12770, 12750, 12750, 12760, 12760, 12760, 12750, 12750, 12760, 12750, 12750, 12760,
+        12750, 12760, 12770, 12750, 12770, 12770, 12750, 12770, 12760, 12760, 12760, 12760, 12760,
+        12770, 12760, 12770, 12750, 12760, 12760, 12760, 12760, 12760, 12750, 12750, 12760, 12760,
+        12760, 12760, 12760, 12760, 12760, 12770, 12760, 12760, 12760, 12760, 12760, 12760, 12760,
+        12760, 12770, 12760, 12760, 12770, 12760, 12760, 12750, 12760, 12770, 12760, 12760, 12760,
+        12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12770, 12760, 12760, 12770,
+        12760, 12770, 12760, 12770, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760,
+        12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760,
+        12760, 12760, 12770, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12770, 12760,
+        12770, 12770, 12760, 12760, 12770, 12760, 12760, 12760, 12770, 12760, 12770, 12760, 12770,
+        12760, 12770, 12770, 12770, 12760, 12760, 12770, 12760, 12760, 12760, 12770, 12770, 12760,
+        12770, 12770, 12760, 12770, 12760, 12770, 12770, 12760, 12770, 12770, 12770, 12770, 12770,
+        12770, 12770, 12760, 12770, 12770, 12770, 12770, 12770, 12770, 12760, 12770, 12780, 12770,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12760, 12760,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770,
+        12770, 12770, 12770, 12770, 12780, 12770, 12770, 12770, 12770, 12780, 12770, 12770, 12770,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12780, 12770, 12780,
+        12770, 12770, 12770, 12780, 12770, 12780, 12780, 12770, 12780, 12770, 12770, 12770, 12770,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770,
+        12780, 12770, 12780, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12780, 12770, 12770,
+        12770, 12770, 12780, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12780,
+        12770, 12780, 12770, 12770, 12770, 12780, 12780, 12770, 12770, 12770, 12770, 12770, 12770,
+        12770, 12780, 12770, 12780, 12770, 12770, 12780, 12770, 12770, 12770, 12770, 12780, 12770,
+        12770, 12770, 12770, 12770, 12770, 12780, 12770, 12780, 12780, 12770, 12770, 12770, 12770,
+        12770, 12780, 12770, 12770, 12770, 12780, 12780, 12780, 12770, 12770, 12770, 12780, 12780,
+        12770, 12770, 12780, 12770, 12770, 12770, 12770, 12780, 12770, 12780, 12780, 12780, 12770,
+        12770, 12780, 12770, 12770, 12780, 12770, 12770, 12780, 12780, 12770, 12770, 12780, 12780,
+        12770, 12780, 12780, 12770, 12770, 12770, 12780, 12780, 12780, 12780, 12780, 12770, 12770,
+        12780, 12780, 12780, 12780, 12780, 12780, 12780, 12790, 12780, 12790, 12780, 12780, 12770,
+        12770, 12780, 12770, 12770, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780,
+        12780, 12790, 12790, 12780, 12780, 12780, 12790, 12780, 12780, 12780, 12780, 12780, 12780,
+        12780, 12780, 12780, 12790, 12780, 12770, 12790, 12780, 12790, 12780, 12790, 12780, 12780,
+        12790, 12780, 12790, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780,
+        12780, 12780, 12780, 12780, 12790, 12780, 12780, 12780, 12790, 12790, 12780, 12770, 12780,
+        12780, 12780, 12770, 12780, 12770, 12770, 12780, 12780, 12780, 12780, 12780, 12780, 12780,
+        12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12770, 12780, 12790, 12780, 12790,
+        12790, 12790, 12790, 12780, 12780, 12790, 12780, 12780, 12790, 12790, 12780, 12780, 12780,
+        12780, 12780, 12780, 12780, 12780, 12780, 12790, 12780, 12780, 12780, 12790, 12780, 12780,
+        12790, 12790, 12790, 12790, 12780, 12790, 12780, 12780, 12780, 12780, 12780, 12780, 12780,
+        12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12790, 12780, 12780, 12780,
+        12790, 12780, 12780, 12790, 12780, 12780, 12780, 12790, 12780, 12790, 12780, 12790, 12790,
+        12780, 12780, 12790, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12790, 12790,
+        12780, 12780, 12780, 12780, 12790, 12780, 12780, 12790
+    };
+    float batteryCurrentmA[datasetcolumns_SoC] = {
+        -3000, -3000, -3000, -3000, -3000, -3000, -3000, -2990, 10, 10, 10, 10, 10, 10, 10, 0,  0,
+        10,    10,    0,     10,    10,    10,    10,    0,     0,  10, 10, 10, 10, 10, 10, 10, 10,
+        10,    0,     10,    10,    10,    10,    0,     10,    0,  0,  10, 10, 0,  0,  10, 10, 0,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 0,  0,
+        10,    10,    0,     0,     10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 0,  0,  10,
+        10,    10,    10,    0,     10,    10,    0,     0,     0,  10, 10, 10, 10, 10, 10, 0,  10,
+        10,    10,    0,     0,     0,     10,    10,    10,    10, 10, 10, 0,  10, 0,  10, 10, 10,
+        10,    10,    10,    10,    10,    10,    0,     10,    10, 10, 10, 0,  10, 10, 10, 10, 10,
+        10,    0,     10,    10,    0,     255.6, 10,    10,    10, 0,  10, 10, 10, 10, 10, 10, 10,
+        0,     10,    10,    10,    0,     10,    0,     10,    0,  10, 10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    0,     10,    0,     10, 0,  10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    10,    0,     10,    10, 10, 0,  10, 10, 10, 0,  10, 10,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    0,     10,    0,     0,  0,  10, 10, 10, 10, 10, 0,  10,
+        10,    10,    10,    10,    10,    10,    10,    10,    0,  10, 0,  10, 10, 10, 10, 0,  0,
+        10,    10,    10,    10,    10,    10,    10,    0,     10, 10, 10, 10, 10, 0,  10, 10, 10,
+        10,    10,    10,    10,    10,    10,    10,    0,     10, 0,  10, 10, 10, 10, 10, 10, 10,
+        10,    0,     10,    10,    10,    10,    0,     10,    0,  10, 10, 0,  10, 10, 10, 10, 10,
+        0,     10,    20,    10,    10,    10,    10,    10,    0,  10, 0,  10, 10, 10, 10, 10, 10,
+        10,    0,     10,    10,    10,    0,     10,    10,    10, 0,  10, 10, 10, 0,  10, 10, 10,
+        0,     0,     10,    10,    0,     10,    0,     10,    10, 10, 10, 10, 10, 0,  0,  10, 10,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    0,     0,     10,    10,    10,    10, 0,  10, 0,  0,  10, 10, 10, 10,
+        10,    10,    10,    10,    10,    0,     10,    0,     10, 10, 10, 10, 10, 10, 10, 0,  10,
+        10,    10,    0,     0,     10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        0,     10,    10,    0,     0,     10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 0,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        0,     10,    10,    0,     0,     10,    10,    10,    10, 10, 10, 0,  10, 10, 10, 10, 10,
+        10,    10,    10,    10,    0,     0,     10,    10,    0,  10, 0,  10, 10, 10, 10, 10, 10,
+        10,    10,    0,     10,    10,    0,     10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        10,    10,    0,     10,    10,    10,    0,     10,    10, 10, 10, 10, 0,  10, 10, 0,  10,
+        0,     10,    10,    0,     10,    10,    10,    10,    10, 10, 10, 10, 10, 20, 10, 10, 0,
+        10,    0,     0,     0,     10,    0,     10,    0,     10, 10, 10, 10, 0,  0,  10, 0,  10,
+        10,    10,    0,     10,    10,    10,    10,    10,    10, 10, 10, 0,  10, 0,  10, 10, 0,
+        0,     0,     10,    10,    0,     10,    10,    0,     10, 10, 10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 0,  0,  10, 10, 10, 10, 10,
+        10,    0,     10,    10,    10,    10,    0,     10,    0,  10, 10, 10, 10, 10, 10, 10, 10,
+        10,    0,     10,    10,    10,    10,    0,     10,    0,  0,  0,  10, 10, 10, 10, 0,  0,
+        10,    10,    10,    10,    0,     10,    10,    10,    0,  10, 10, 10, 10, 10, 10, 10, 10,
+        20,    10,    10,    10,    10,    10,    0,     10,    0,  10, 10, 0,  0,  0,  10, 0,  0,
+        0,     10,    0,     10,    10,    10,    10,    0,     0,  10, 10, 10, 10, 10, 10, 0,  10,
+        10,    0,     10,    0,     0,     0,     10,    0,     10, 0,  10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        0,     10,    10,    10,    0,     0,     10,    10,    10, 10, 10, 10, 10, 0,  0,  10, 10,
+        10,    10,    10,    10,    0,     10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 0,  10,
+        10,    10,    10,    10,    10,    10,    10,    10,    0,  10, 10, 10, 10, 0,  10, 10, 0,
+        10,    10,    0,     0,     10,    10,    10,    10,    10, 10, 10, 0,  10, 10, 0,  10, 10,
+        10,    10,    10,    0,     10,    10,    0,     10,    0,  10, 0,  10, 0,  10, 10, 10, 10,
+        0,     10,    10,    10,    10,    10,    0,     0,     10, 10, 10, 10, 10, 10, 0,  10, 10,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 0,  10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    10,    10,    0,     10, 0,  10, 10, 10, 10, 10, 10, 0,
+        0,     0,     0,     0,     10,    10,    0,     0,     10, 10, 10, 10, 10, 10, 0,  0,  0,
+        10,    0,     10,    10,    10,    10,    10,    0,     10, 10, 10, 10, 10, 10, 0,  10, 0,
+        10,    10,    10,    10,    0,     0,     10,    0,     10, 10, 10, 10, 0,  10, 10, 10, 10,
+        10,    10,    0,     10,    10,    10,    10,    10,    0,  10, 0,  10, 0,  10, 10, 10, 0,
+        0,     10,    10,    10,    10,    0,     10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        10,    0,     0,     10,    10,    10,    10,    0,     10, 10, 10, 0,  10, 0,  10, 0,  0,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 0,  10, 10, 10, 10, 10, 10, 0,
+        10,    10,    0,     10,    10,    10,    0,     10,    10, 10
+    };
+    bool isBatteryInFloat[datasetcolumns_SoC] = {
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false
+    };
+    float samplePeriodMilliSec[datasetcolumns_SoC] = {
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000
+    };
+} dataset_SoC_t;
+
+void test_backtest_SoC()
+{
+    dataset_SoC_t dataset;
+    int cholsl_error;
+    // float batteryVoltage = 13.01; //batteryvoltage measurement TODO delete
+
+    // Do generic EKF initialization
+    // ekf_soc_t ekf_soc
+    ekf_init(&ekf_soc, NUMBER_OF_STATES_SOC, NUMBER_OF_OBSERVABLES_SOC);
+
+    // Do local initialization
+    float P0 = 0.1;   // initial covariance of state noise  (aka process noise)
+    float Q0 = 0.001; // Initial state uncertainty covariance matrix
+    float R0 = 0.1;   // initial covariance of measurement noise
+    float initialSoC = 0xFFFFFFFFFFF;
+    init_soc(&ekf_soc, dataset.batteryVoltagemV[0], P0, Q0, R0, initialSoC);
+    float batteryCapacityAh = 12;
+    float batteryEff = 85000;
+    float expectedResult = 71491.4609;
+    const uint32_t SOC_SCALED_HUNDRED_PERCENT = 100000; // 100% charge = 100000
+
+    int j;
+
+    // Loop till no more data
+    for (j = 1; j < datasetcolumns_SoC; ++j) {
+
+        // $\hat{x}_k = f(\hat{x}_{k-1})$
+        batteryEff =
+            f(&ekf_soc, dataset.isBatteryInFloat[j], batteryEff, dataset.batteryCurrentmA[j],
+              dataset.samplePeriodMilliSec[j], batteryCapacityAh);
+        // update measurable (voltage) based on predicted state (SOC)
+        h(&ekf_soc, dataset.batteryCurrentmA[j]);
+#ifdef DEBUG
+        printf("battvol inside test %f \n", dataset.batteryVoltagemV[j]);
+#endif
+        cholsl_error = ekf_step(&ekf_soc, &dataset.batteryVoltagemV[j]);
+#ifdef DEBUG
+        if (cholsl_error != 0)
+            printf("EKFSTEP Failed");
+#endif
+        ekf_soc.x[0] = clamp((float)ekf_soc.x[0], 0, SOC_SCALED_HUNDRED_PERCENT);
+#ifdef DEBUG
+        printf("\n\n\nThe SoC after clamp %f \n\n\n", ekf_soc.x[0]);
+#endif
+    }
+
+    TEST_ASSERT_FLOAT_WITHIN(1, expectedResult, ekf_soc.x[0]);
+}
+
+int kalman_soc_tests()
+{
+
+    UNITY_BEGIN();
+
+    // RUN_TEST(test_backtest_gps);
+    RUN_TEST(test_ekf_init_func);
+    RUN_TEST(test_ekf_step_func);
+    RUN_TEST(test_clamp_func);
+    RUN_TEST(test_calculate_initial_soc_func);
+    // RUN_TEST(test_init_soc_func_should_init_with_initial_soc);
+    // RUN_TEST(test_init_soc_func_should_init_with_calculated_soc);
+    // RUN_TEST(test_f_func);
+    // RUN_TEST(test_h_func);
+    RUN_TEST(test_should_increase_soc_no_float_leadacid_12V);
+    RUN_TEST(test_update_soc_should_increase_soc_no_float_leadacid_12V);
+
+    RUN_TEST(test_backtest_SoC);
+
+    return UNITY_END();
+}

From 83326d9dc4932e2d38e30f8c1fb8eb33879aeb6a Mon Sep 17 00:00:00 2001
From: mulles <github.com@emu.lu>
Date: Wed, 6 Apr 2022 16:40:53 +0200
Subject: [PATCH 2/7] Pull Request SoC with Extended Kalman Filter

-Debugging is still done partially by printf, as I was not able to get the zephyr tool for it running.
-The equation system for the model is probably incorrect, we should discuss it.
---
 app/src/CMakeLists.txt          |   1 +
 app/src/bat_charger.cpp         | 226 +++++++-
 app/src/bat_charger.h           | 199 ++++++-
 app/src/kalman_soc.cpp          | 399 ++++++++++++++
 app/src/kalman_soc.h            |  25 +
 app/src/main.cpp                |   5 +-
 app/src/setup.cpp               |   1 +
 app/src/setup.h                 |   1 +
 tests/CMakeLists.txt            |   1 +
 tests/src/main.cpp              |   1 +
 tests/src/tests.h               |   2 +
 tests/src/tests_bat_charger.cpp |   2 +-
 tests/src/tests_dcdc.cpp        |   4 +-
 tests/src/tests_kalman_soc.cpp  | 901 ++++++++++++++++++++++++++++++++
 14 files changed, 1759 insertions(+), 9 deletions(-)
 create mode 100644 app/src/kalman_soc.cpp
 create mode 100644 app/src/kalman_soc.h
 create mode 100644 tests/src/tests_kalman_soc.cpp

diff --git a/app/src/CMakeLists.txt b/app/src/CMakeLists.txt
index 04dc7981..20424b90 100644
--- a/app/src/CMakeLists.txt
+++ b/app/src/CMakeLists.txt
@@ -20,6 +20,7 @@ target_sources(app PRIVATE
         pwm_switch_driver.c
         pwm_switch.cpp
         setup.cpp
+        kalman_soc.cpp
 )
 
 add_subdirectory(ext)
diff --git a/app/src/bat_charger.cpp b/app/src/bat_charger.cpp
index 60b6747a..320511c8 100644
--- a/app/src/bat_charger.cpp
+++ b/app/src/bat_charger.cpp
@@ -22,6 +22,12 @@ LOG_MODULE_REGISTER(bat_charger, CONFIG_BAT_LOG_LEVEL);
 extern DeviceStatus dev_stat;
 extern LoadOutput load;
 
+uint32_t milli_seconds_in_float = 0;
+uint32_t float_reset_duration = 600000;             // 10 minutes in milliseconds
+const uint32_t soc_scaled_hundred_percent = 100000; // 100% charge = 100000
+const uint32_t soc_scaled_max =
+    2 * soc_scaled_hundred_percent; // allow soc to track up higher than 100% to gauge efficiency
+
 // DISCHARGE_CURRENT_MAX used to estimate current-compensation of load disconnect voltage
 #if BOARD_HAS_LOAD_OUTPUT
 #define DISCHARGE_CURRENT_MAX DT_PROP(DT_CHILD(DT_PATH(outputs), load), current_max)
@@ -29,6 +35,60 @@ extern LoadOutput load;
 #define DISCHARGE_CURRENT_MAX DT_PROP(DT_PATH(pcb), dcdc_current_max)
 #endif
 
+float calculate_initial_soc(float battery_voltage_mV)
+{
+    // TODO will need to add 24 V compatability
+    const uint32_t soc_scaled_hundred_percent = 100000;
+    const uint8_t voltages_size = 10;
+    const float batt_soc_voltages[voltages_size] = { 12720, 12600, 12480, 12360, 12240,
+                                                     12120, 12000, 11880, 11760, 11640 };
+
+    uint8_t index;
+    for (index = 0; index < voltages_size; index++) {
+        if (battery_voltage_mV > batt_soc_voltages[index]) {
+            break;
+        }
+    }
+    return (voltages_size - index) * (soc_scaled_hundred_percent / voltages_size);
+}
+
+void diagonal_matrix(float *A, float value, int n, int m)
+{
+    int i, j;
+    for (i = 0; i < n; i++) {
+        for (j = 0; j < m; j++) {
+            if (i == j) {
+                A[i * n + j] = value;
+            }
+            else {
+                A[i * n + j] = 0;
+            }
+            LOG_DBG("%f ", A[i * n + j]);
+        }
+        LOG_DBG("\n");
+    }
+}
+
+void init_soc(EkfSoc *ekf_soc, float v0, float P0, float Q0, float R0, float initial_soc)
+{
+    // Init State vector
+    // use stored soc, unless it's out of range, in which case calculate new starting point
+    ekf_soc->x[0] = (initial_soc >= 0 && initial_soc <= soc_scaled_max) ? initial_soc
+                                                                        : calculate_initial_soc(v0);
+    ekf_soc->x[1] = 0.0; // TODO Check what init makes sense
+    ekf_soc->x[2] = 0.0; // TODO Check what init makes sense
+
+    LOG_DBG("Init Matrix F\n");
+    diagonal_matrix(&ekf_soc->F[0][0], 1, NUMBER_OF_STATES_SOC,
+                    NUMBER_OF_STATES_SOC); // F identity matrix
+    LOG_DBG("\nInit Matrix P\n");
+    diagonal_matrix(&ekf_soc->P[0][0], P0, NUMBER_OF_STATES_SOC, NUMBER_OF_STATES_SOC);
+    LOG_DBG("\nInit Matrix Q\n");
+    diagonal_matrix(&ekf_soc->Q[0][0], Q0, NUMBER_OF_STATES_SOC, NUMBER_OF_STATES_SOC);
+    LOG_DBG("\nInit Matrix R\n");
+    diagonal_matrix(&ekf_soc->R[0][0], R0, NUMBER_OF_OBSERVABLES_SOC, NUMBER_OF_OBSERVABLES_SOC);
+}
+
 void battery_conf_init(BatConf *bat, int type, int num_cells, float nominal_capacity)
 {
     bat->nominal_capacity = nominal_capacity;
@@ -362,7 +422,7 @@ void Charger::detect_num_batteries(BatConf *bat) const
     }
 }
 
-void Charger::update_soc(BatConf *bat_conf)
+void Charger::update_soc_voltage_based(BatConf *bat_conf)
 {
     static int soc_filtered = 0; // SOC / 100 for better filtering
 
@@ -657,7 +717,7 @@ void Charger::charge_control(BatConf *bat_conf)
     }
 }
 
-void Charger::init_terminal(BatConf *bat) const
+void Charger::init_terminal(BatConf *bat, EkfSoc *ekf_soc) const
 {
     port->bus->sink_voltage_intercept = bat->topping_voltage;
     port->bus->src_voltage_intercept = bat->load_disconnect_voltage;
@@ -681,4 +741,166 @@ void Charger::init_terminal(BatConf *bat) const
     port->bus->src_droop_res =
         -bat->wire_resistance / static_cast<float>(port->bus->series_multiplier)
         - bat->internal_resistance;
+
+    float P0 = 0.1;   // initial covariance of state noise  (aka process noise)
+    float Q0 = 0.001; // Initial state uncertainty covariance matrix
+    float R0 = 0.1;   // initial covariance of measurement noise
+    float battery_voltage_mV[1] = {
+        port->bus->voltage * 1000
+    }; // intial Voltage measurement to calculate SoC if initial_soc is out of range
+    float initial_soc = soc * 1000; // last known SoC
+    // Do generic EKF initialization
+    ekf_init(ekf_soc, NUMBER_OF_STATES_SOC, NUMBER_OF_OBSERVABLES_SOC);
+    init_soc(ekf_soc, battery_voltage_mV[0], P0, Q0, R0, initial_soc);
+}
+float clamp(float value, float min, float max)
+{
+    if (value > max) {
+        return max;
+    }
+    else if (value < min) {
+        return min;
+    }
+    return value;
+}
+
+float model_soc(EkfSoc *ekf_soc, bool is_battery_in_float, float battery_eff,
+                float battery_current_mA, float sample_period_milli_sec, float battery_capacity_Ah)
+{
+    // $\hat{x}_k = f(\hat{x}_{k-1})$
+    battery_eff = f(ekf_soc, is_battery_in_float, battery_eff, battery_current_mA,
+                    sample_period_milli_sec, battery_capacity_Ah);
+    LOG_DBG("The SoC by f()  %f \n", ekf_soc->x[0]);
+    // update measurable (voltage) based on predicted state (SoC)
+    h(ekf_soc, battery_current_mA);
+    return battery_eff;
+}
+
+float f(EkfSoc *ekf_soc, bool is_battery_in_float, float battery_eff, float battery_current_mA,
+        float sample_period_milli_sec, float battery_capacity_Ah)
+{
+    float milli_sec_to_hours = 3600000;
+    float charge_change = (battery_current_mA / 1000) * battery_eff / 100000
+                          * (sample_period_milli_sec / milli_sec_to_hours);
+    float previous_soc = ekf_soc->x[0];
+    float new_soc = (ekf_soc->x[0] * battery_capacity_Ah + charge_change * 1000)
+                    / battery_capacity_Ah; // scaling should be fine here
+    ekf_soc->fx[0] = new_soc;
+
+    if (is_battery_in_float) {
+        milli_seconds_in_float += sample_period_milli_sec;
+        if (milli_seconds_in_float > float_reset_duration) {
+
+            battery_eff = (float)battery_eff * (float)soc_scaled_hundred_percent / previous_soc;
+            battery_eff = clamp(battery_eff, 0, soc_scaled_hundred_percent);
+            ekf_soc->fx[0] = soc_scaled_hundred_percent;
+        }
+    }
+    else {
+        milli_seconds_in_float = 0;
+    }
+
+    return battery_eff;
+}
+
+void h(EkfSoc *ekf_soc, float battery_current_mA)
+{
+
+    // _hx is the voltage that most closely matches current SoC (a number)
+    // _H is an array of form [ocv gradient, measured current, 1] (the last parameter is the offset)
+    // x_[0] = SOC, _x[1] = R0 _x[2]=U1 units are unknown.
+
+    bool is_battery_12_V = true;
+    bool is_battery_lithium = false;
+    int index_R0 = 1;
+    int index_U1 = 2;
+
+    // Hardcoded  SoC-OCV Curve aka Lookuptable
+    float dummy_lead_acid_voltage[101] = {
+        11640, 11653, 11666, 11679, 11692, 11706, 11719, 11732, 11745, 11758, 11772, 11785, 11798,
+        11811, 11824, 11838, 11851, 11864, 11877, 11890, 11904, 11917, 11930, 11943, 11956, 11970,
+        11983, 11996, 12009, 12022, 12036, 12049, 12062, 12075, 12088, 12102, 12115, 12128, 12141,
+        12154, 12168, 12181, 12194, 12207, 12220, 12234, 12247, 12260, 12273, 12286, 12300, 12313,
+        12326, 12339, 12352, 12366, 12379, 12392, 12405, 12418, 12432, 12445, 12458, 12471, 12484,
+        12498, 12511, 12524, 12537, 12550, 12564, 12577, 12590, 12603, 12616, 12630, 12643, 12656,
+        12669, 12682, 12696, 12709, 12722, 12735, 12748, 12762, 12775, 12788, 12801, 12814, 12828,
+        12841, 12854, 12867, 12880, 12894, 12907, 12920, 12933, 12946, 12960
+    };
+    float dummy_lithium_voltage[101] = {
+        5000,  6266,  7434,  8085,  8531,  8867,  9134,  9355,  9543,  9705,  9847,  9974,  10088,
+        10191, 10285, 10372, 10451, 10525, 10595, 10659, 10720, 10777, 10831, 10882, 10931, 10977,
+        11021, 11063, 11104, 11142, 11180, 11216, 11251, 11284, 11317, 11349, 11379, 11409, 11438,
+        11467, 11495, 11522, 11548, 11574, 11600, 11625, 11650, 11675, 11699, 11723, 11746, 11769,
+        11793, 11815, 11838, 11861, 11883, 11906, 11928, 11950, 11972, 11994, 12017, 12039, 12061,
+        12083, 12105, 12127, 12150, 12172, 12195, 12217, 12240, 12263, 12286, 12309, 12333, 12356,
+        12380, 12404, 12428, 12452, 12477, 12501, 12526, 12552, 12577, 12603, 12629, 12655, 12682,
+        12708, 12735, 12763, 12790, 12818, 12846, 12875, 12903, 12931, 12960
+    };
+    float dummy_ocv_soc[101] = {
+        0,     1000,  2000,  3000,  4000,  5000,  6000,  7000,  8000,  9000,  10000, 11000, 12000,
+        13000, 14000, 15000, 16000, 17000, 18000, 19000, 20000, 21000, 22000, 23000, 24000, 25000,
+        26000, 27000, 28000, 29000, 30000, 31000, 32000, 33000, 34000, 35000, 36000, 37000, 38000,
+        39000, 40000, 41000, 42000, 43000, 44000, 45000, 46000, 47000, 48000, 49000, 50000, 51000,
+        52000, 53000, 54000, 55000, 56000, 57000, 58000, 59000, 60000, 61000, 62000, 63000, 64000,
+        65000, 66000, 67000, 68000, 69000, 70000, 71000, 72000, 73000, 74000, 75000, 76000, 77000,
+        78000, 79000, 80000, 81000, 82000, 83000, 84000, 85000, 86000, 87000, 88000, 89000, 90000,
+        91000, 92000, 93000, 94000, 95000, 96000, 97000, 98000, 99000, 100000
+    };
+    // update voltage closest to current state of charge as well as gradient
+    int i;
+    float multiplier;
+
+    if (is_battery_12_V) {
+        multiplier = 1;
+    }
+    else {
+        multiplier = 2;
+    }
+    for (i = 0; i < 101; i++) {
+
+        if (dummy_ocv_soc[i] > (float)ekf_soc->x[0]) {
+            if (is_battery_lithium) {
+                ekf_soc->hx[0] =
+                    (dummy_lithium_voltage[i] + dummy_lithium_voltage[i - 1]) * multiplier / 2
+                    + (battery_current_mA / 1000 * ekf_soc->x[index_R0] / 100)
+                    + ekf_soc->x[index_U1] / 100; // units should be good her
+                ekf_soc->H[0][0] =
+                    (dummy_lithium_voltage[i] - dummy_lithium_voltage[i - 1]) * multiplier * 100
+                    / (dummy_ocv_soc[i] - dummy_ocv_soc[i - 1]); // units are good here
+            }
+            else {
+                ekf_soc->hx[0] =
+                    (dummy_lead_acid_voltage[i] + dummy_lead_acid_voltage[i - 1]) * multiplier / 2
+                    + (battery_current_mA / 1000 * ekf_soc->x[index_R0] / 100)
+                    + ekf_soc->x[index_U1] / 100;
+                ekf_soc->H[0][0] = (dummy_lead_acid_voltage[i] - dummy_lead_acid_voltage[i - 1])
+                                   * multiplier * 100 / (dummy_ocv_soc[i] - dummy_ocv_soc[i - 1]);
+            }
+            ekf_soc->H[0][1] = battery_current_mA / 1000; // should be good in Amps
+            ekf_soc->H[0][2] = 1;                         // offset
+            printf("U0= I*R0 = %fmV \n", (battery_current_mA / 1000 * ekf_soc->x[index_R0]) / 100);
+            printf("U1= %fmV \n", ekf_soc->x[index_U1] / 100);
+            printf("For single Cell Lithium would be \nU0/4= I*R0/4Cells = %fmV \n",
+                   (battery_current_mA / 1000 * ekf_soc->x[index_R0]) / 4 / 100);
+            printf("U1/4Cells= %fmV \n", ekf_soc->x[index_U1] / 4 / 100);
+            return;
+        }
+    }
+}
+
+void Charger::update_soc(BatConf *bat_conf, EkfSoc *ekf_soc)
+{
+
+    int cholsl_error = 0;
+    float battery_eff = 100000; // fixed to 100% implemented to use later on.
+    float sample_period_milli_sec = 1000;
+    float battery_voltage_mV[1] = { port->bus->voltage * 1000 };
+
+    battery_eff = model_soc(ekf_soc, bat_conf->float_enabled, battery_eff, (port->current * 1000),
+                            sample_period_milli_sec, bat_conf->nominal_capacity);
+    cholsl_error = ekf_step(ekf_soc, battery_voltage_mV);
+    LOG_DBG("Numerical Error in EKF_Step 1=true, 0 = false %d\n", cholsl_error);
+    LOG_DBG("Soc after EKF and before clamp %f\n", ekf_soc->x[0]);
+    ekf_soc->x[0] = clamp((float)ekf_soc->x[0], 0, soc_scaled_hundred_percent);
+    soc = ekf_soc->x[0] / 1000;
 }
diff --git a/app/src/bat_charger.h b/app/src/bat_charger.h
index 15e0101c..5dc15d31 100644
--- a/app/src/bat_charger.h
+++ b/app/src/bat_charger.h
@@ -12,6 +12,7 @@
  * @brief Battery and charger configuration and functions
  */
 
+#include "kalman_soc.h"
 #include <stdbool.h>
 #include <stddef.h>
 #include <stdint.h>
@@ -21,6 +22,193 @@
 
 #define CHARGER_TIME_NEVER INT32_MIN
 
+//#define DEBUG 0
+#define NUMBER_OF_STATES_SOC      3
+#define NUMBER_OF_OBSERVABLES_SOC 1
+
+/**
+ * Extended Kalman Filter (EKF) configuration data
+ *
+ * Data will be initialized in Charger::init_terminal
+ */
+typedef struct
+{
+    /**
+     * number of state values
+     *
+     */
+    int n;
+
+    /**
+     * number of observables
+     *
+     */
+    int m;
+
+    /**
+     * state vector
+     *
+     * [ir0 hk0 SOC0] TODO
+     */
+    float x[NUMBER_OF_STATES_SOC];
+
+    /**
+     * prediction error covariance
+     *
+     */
+    float P[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     * process noise covariance
+     *
+     * uncertainty of current sensor, state equation defined as fx
+     */
+    float Q[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     *  measurement error covariance
+     *
+     * uncertainty of voltage sensor, output equation defined as hx
+     */
+    float R[NUMBER_OF_OBSERVABLES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+
+    /**
+     * Kalman gain; a.k.a. K
+     *
+     */
+    float G[NUMBER_OF_STATES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+
+    /**
+     * Jacobian of process model
+     *
+     */
+    float F[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     * Jacobian of measurement model
+     *
+     */
+    float H[NUMBER_OF_OBSERVABLES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     *  transpose of measurement Jacobian
+     *
+     */
+    float Ht[NUMBER_OF_STATES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+
+    /**
+     * transpose of process Jacobian
+     *
+     */
+    float Ft[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     *  P, post-prediction, pre-update
+     *
+     */
+    float Pp[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+
+    /**
+     * output of user defined f() state-transition function
+     *
+     */
+    float fx[NUMBER_OF_STATES_SOC];
+
+    /**
+     * output of user defined h() measurement function
+     *
+     */
+    float hx[NUMBER_OF_OBSERVABLES_SOC];
+
+    /**
+     * temporary storage
+     *
+     */
+    float tmp0[NUMBER_OF_STATES_SOC][NUMBER_OF_STATES_SOC];
+    float tmp1[NUMBER_OF_STATES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+    float tmp2[NUMBER_OF_OBSERVABLES_SOC][NUMBER_OF_STATES_SOC];
+    float tmp3[NUMBER_OF_OBSERVABLES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+    float tmp4[NUMBER_OF_OBSERVABLES_SOC][NUMBER_OF_OBSERVABLES_SOC];
+    float tmp5[NUMBER_OF_OBSERVABLES_SOC];
+
+} EkfSoc;
+
+/**
+ * Clamp function to ensure SoC within min max boders.
+ *
+ * @param value SoC value for which to ensure to be within borders min, max
+ * @param min The minimum value 0% the SoC is allowed to have
+ * @param max The maximum value 100% the SoC is allowed to have
+ * @return float Either the handed value or 0%, 100$ when excisting borders.
+ */
+float clamp(float value, float min, float max);
+
+/**
+ * Calculates initial SoC based on handed voltage
+ *
+ * @param battery_voltage_mV
+ * @return float Inital SoC
+ */
+
+float calculate_initial_soc(float battery_voltage_mV);
+
+/**
+ * Initializes Extended Kalman Filter matrices based on handed parameters
+ *
+ * *
+ * WARNING TODO: It is unclear if the equations used are correct, thus the init of F might be wrong
+ *
+ * @param ekf_soc struct in which initialized matrices are stored
+ * @param v0 initial voltage on which the inital SoC is based
+ * @param P0 Diagonal values for P matrix
+ * @param Q0 Diagonal values for Q matrix
+ * @param R0 Diagonal values for R matrix
+ * @param initial_soc inital SoC if known.
+ */
+void init_soc(EkfSoc *ekf_soc, float v0, float P0, float Q0, float R0, float initial_soc);
+
+/**
+ * Unites f and h function and forms the complete battery model
+ *
+ * @param ekf_soc struct with EKF Parameters
+ * @param is_battery_in_float for Lead Acid Batterys Float Charging Status
+ * @param battery_eff Efficieny currently not used
+ * @param battery_current_mA
+ * @param sample_period_milli_sec Period between battery current and voltage measurements
+ * @param battery_capacity_Ah
+ * @return float Efficiency currently not used
+ */
+float model_soc(EkfSoc *ekf_soc, bool is_battery_in_float, float battery_eff,
+                float battery_current_mA, float sample_period_milli_sec, float battery_capacity_Ah);
+
+/**
+ * project the state of charge ahead one step using a Coulomb counting model
+ * (Integration of the current over time)
+ * x{k+1}(index_soc) = x{k} - \frac{1}{{Q_{C}}}\int_{0}^{\Delta t} {i(t)\ dt}
+ *
+ * @param ekf_soc  struct with EKF Parameters
+ * @param is_battery_in_float Is the lead acid batterie in float charge?
+ * @param battery_eff Efficiency c   urrently not used
+ * @param battery_current_mA
+ * @param sample_period_milli_sec Period between battery measurements
+ * @param battery_capacity_Ah
+ * @return float
+ */
+float f(EkfSoc *ekf_soc, bool is_battery_in_float, float battery_eff, float battery_current_mA,
+        float sample_period_milli_sec, float battery_capacity_Ah);
+
+/**
+ * predict the measurable value (voltage) ahead one step using the newly estimated state of charge
+ * {h}(k) = {OCV}(x{k}) - {R}_{0} i(t) - {R}_{1} {i}_{R_1}(t) in mV
+ *
+ * WARNING TODO: It is unclear if the equations used are correct and if so if their implementation
+ * is correct thus x vector and H Matrix might be wrong.
+ *
+ * @param ekf_soc  struct with EKF Parameters
+ * @param battery_current_mA
+ */
+void h(EkfSoc *ekf_soc, float battery_current_mA);
+
 /**
  * Battery cell types
  */
@@ -457,14 +645,21 @@ class Charger
      *
      * Must be called exactly once per second, otherwise SOC calculation gets wrong.
      */
-    void update_soc(BatConf *bat_conf);
+    void update_soc(BatConf *bat_conf, EkfSoc *ekf_soc);
+
+    /**
+     * SOC estimation
+     *
+     * WARNING: TODO obsolte function, replaced by void charge_control(BatConf *bat_conf);
+     */
+    void update_soc_voltage_based(BatConf *bat_conf);
 
     /**
      * Initialize terminal and dc bus for battery connection
      *
      * @param bat Configuration to be used for terminal setpoints
      */
-    void init_terminal(BatConf *bat) const;
+    void init_terminal(BatConf *bat, EkfSoc *ekf_soc) const;
 
 private:
     void enter_state(int next_state);
diff --git a/app/src/kalman_soc.cpp b/app/src/kalman_soc.cpp
new file mode 100644
index 00000000..270f9ef3
--- /dev/null
+++ b/app/src/kalman_soc.cpp
@@ -0,0 +1,399 @@
+/*
+ * TinyEKF: Extended Kalman Filter for embedded processors
+ *
+ * Copyright (C) 2015 Simon D. Levy
+ *
+ * MIT License
+ */
+
+#include "kalman_soc.h"
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#define DEBUG
+
+#include <logging/log.h>
+LOG_MODULE_REGISTER(kalman_soc, CONFIG_LOG_DEFAULT_LEVEL);
+
+typedef struct
+{
+
+    float *x; /* state vector */
+
+    float *P; /* prediction error covariance */
+    float *Q; /* process noise covariance */
+    float *R; /* measurement error covariance */
+
+    float *G; /* Kalman gain; a.k.a. K */
+
+    float *F; /* Jacobian of process model */
+    float *H; /* Jacobian of measurement model */
+
+    float *Ht; /* transpose of measurement Jacobian */
+    float *Ft; /* transpose of process Jacobian */
+    float *Pp; /* P, post-prediction, pre-update */
+
+    float *fx; /* output of user defined f() state-transition function */
+    float *hx; /* output of user defined h() measurement function */
+
+    /* temporary storage */
+    float *tmp0;
+    float *tmp1;
+    float *tmp2;
+    float *tmp3;
+    float *tmp4;
+    float *tmp5;
+
+} EKF;
+
+/* Cholesky-decomposition matrix-inversion code, adapated from
+   http://jean-pierre.moreau.pagesperso-orange.fr/Cplus/choles_cpp.txt */
+
+static int choldc1(float *a, float *p, int n)
+{
+    int i, j, k;
+    float sum;
+
+    for (i = 0; i < n; i++) {
+        for (j = i; j < n; j++) {
+            sum = a[i * n + j];
+            for (k = i - 1; k >= 0; k--) {
+                sum -= a[i * n + k] * a[j * n + k];
+            }
+            if (i == j) {
+                if (sum <= 0) {
+                    return 1; /* error */
+                }
+                p[i] = sqrt(sum);
+            }
+            else {
+                a[j * n + i] = sum / p[i];
+            }
+        }
+    }
+
+    return 0; /* success */
+}
+
+static int choldcsl(float *A, float *a, float *p, int n)
+{
+    int i, j, k;
+    float sum;
+    for (i = 0; i < n; i++) {
+        for (j = 0; j < n; j++) {
+            a[i * n + j] = A[i * n + j];
+        }
+    }
+    if (choldc1(a, p, n)) {
+        return 1;
+    }
+    for (i = 0; i < n; i++) {
+        a[i * n + i] = 1 / p[i];
+        for (j = i + 1; j < n; j++) {
+            sum = 0;
+            for (k = i; k < j; k++) {
+                sum -= a[j * n + k] * a[k * n + i];
+            }
+            a[j * n + i] = sum / p[j];
+        }
+    }
+
+    return 0; /* success */
+}
+
+static int cholsl(float *A, float *a, float *p, int n)
+{
+    int i, j, k;
+    if (choldcsl(A, a, p, n)) {
+        return 1;
+    }
+    for (i = 0; i < n; i++) {
+        for (j = i + 1; j < n; j++) {
+            a[i * n + j] = 0.0;
+        }
+    }
+    for (i = 0; i < n; i++) {
+        a[i * n + i] *= a[i * n + i];
+        for (k = i + 1; k < n; k++) {
+            a[i * n + i] += a[k * n + i] * a[k * n + i];
+        }
+        for (j = i + 1; j < n; j++) {
+            for (k = j; k < n; k++) {
+                a[i * n + j] += a[k * n + i] * a[k * n + j];
+            }
+        }
+    }
+    for (i = 0; i < n; i++) {
+        for (j = 0; j < i; j++) {
+            a[i * n + j] = a[j * n + i];
+        }
+    }
+
+    return 0; /* success */
+}
+
+static void zeros(float *a, int m, int n)
+{
+    int j;
+    for (j = 0; j < m * n; ++j) {
+        a[j] = 0;
+    }
+}
+
+#ifdef DEBUG
+static void dump(float *a, int m, int n, const char *fmt)
+{
+    int i, j;
+
+    char f[100];
+    sprintf(f, "%s ", fmt);
+    for (i = 0; i < m; ++i) {
+        for (j = 0; j < n; ++j) {
+            printf(f, a[i * n + j]);
+        }
+        printf("\n");
+    }
+}
+#endif
+
+/* C <- A * B */
+static void mulmat(float *a, float *b, float *c, int arows, int acols, int bcols)
+{
+    int i, j, l;
+
+    for (i = 0; i < arows; ++i) {
+        for (j = 0; j < bcols; ++j) {
+            c[i * bcols + j] = 0;
+            for (l = 0; l < acols; ++l) {
+                c[i * bcols + j] += a[i * acols + l] * b[l * bcols + j];
+            }
+        }
+    }
+}
+
+static void mulvec(float *a, float *x, float *y, int m, int n)
+{
+    int i, j;
+
+    for (i = 0; i < m; ++i) {
+        y[i] = 0;
+        for (j = 0; j < n; ++j) {
+            y[i] += x[j] * a[i * n + j];
+        }
+    }
+}
+
+static void transpose(float *a, float *at, int m, int n)
+{
+    int i, j;
+
+    for (i = 0; i < m; ++i) {
+        for (j = 0; j < n; ++j) {
+            at[j * m + i] = a[i * n + j];
+        }
+    }
+}
+
+/* A <- A + B */
+static void accum(float *a, float *b, int m, int n)
+{
+    int i, j;
+
+    for (i = 0; i < m; ++i) {
+        for (j = 0; j < n; ++j) {
+            a[i * n + j] += b[i * n + j];
+        }
+    }
+}
+
+/* C <- A + B */
+static void add(float *a, float *b, float *c, int n)
+{
+    int j;
+
+    for (j = 0; j < n; ++j) {
+        c[j] = a[j] + b[j];
+    }
+}
+
+/* C <- A - B */
+static void sub(float *a, float *b, float *c, int n)
+{
+    int j;
+
+    for (j = 0; j < n; ++j) {
+        c[j] = a[j] - b[j];
+    }
+}
+
+static void negate(float *a, int m, int n)
+{
+    int i, j;
+
+    for (i = 0; i < m; ++i) {
+        for (j = 0; j < n; ++j) {
+            a[i * n + j] = -a[i * n + j];
+        }
+    }
+}
+
+static void mat_addeye(float *a, int n)
+{
+    int i;
+    for (i = 0; i < n; ++i) {
+        a[i * n + i] += 1;
+    }
+}
+
+static void unpack(void *v, EKF *ekf, int n, int m)
+{
+    /* skip over n, m in data structure */
+    char *cptr = (char *)v;
+    cptr += 2 * sizeof(int);
+
+    float *dptr = (float *)cptr;
+    ekf->x = dptr;
+    dptr += n;
+    ekf->P = dptr;
+    dptr += n * n;
+    ekf->Q = dptr;
+    dptr += n * n;
+    ekf->R = dptr;
+    dptr += m * m;
+    ekf->G = dptr;
+    dptr += n * m;
+    ekf->F = dptr;
+    dptr += n * n;
+    ekf->H = dptr;
+    dptr += m * n;
+    ekf->Ht = dptr;
+    dptr += n * m;
+    ekf->Ft = dptr;
+    dptr += n * n;
+    ekf->Pp = dptr;
+    dptr += n * n;
+    ekf->fx = dptr;
+    dptr += n;
+    ekf->hx = dptr;
+    dptr += m;
+    ekf->tmp0 = dptr;
+    dptr += n * n;
+    ekf->tmp1 = dptr;
+    dptr += n * m;
+    ekf->tmp2 = dptr;
+    dptr += m * n;
+    ekf->tmp3 = dptr;
+    dptr += m * m;
+    ekf->tmp4 = dptr;
+    dptr += m * m;
+    ekf->tmp5 = dptr;
+}
+
+void ekf_init(void *v, int n, int m)
+{
+    /* retrieve n, m and set them in incoming data structure */
+    int *ptr = (int *)v;
+    *ptr = n;
+    ptr++;
+    *ptr = m;
+
+    /* unpack rest of incoming structure for initlization */
+    EKF ekf;
+    unpack(v, &ekf, n, m);
+
+    /* zero-out matrices */
+    zeros(ekf.P, n, n);
+    zeros(ekf.Q, n, n);
+    zeros(ekf.R, m, m);
+    zeros(ekf.G, n, m);
+    zeros(ekf.F, n, n);
+    zeros(ekf.H, m, n);
+}
+
+int ekf_step(void *v, float *z)
+{
+/* unpack incoming structure */
+#ifdef DEBUG
+    printf("\n\n\n *************************** \n");
+    printf("**********Step***************");
+    printf("\n************************** \n\n\n");
+    printf("Received measured voltage: %f mV \n", z[0]);
+#endif
+    int *ptr = (int *)v;
+    int n = *ptr;
+    ptr++;
+    int m = *ptr;
+
+    EKF ekf;
+    unpack(v, &ekf, n, m);
+
+#ifdef DEBUG
+    printf("Print Matrix of model calculations ekf.hx\n");
+    dump(ekf.hx, m, m, "%f");
+    printf("Print Matrix of model calculations ekf.H\n");
+    dump(ekf.H, m, n, "%f");
+    printf("Print Matrix of model calculations ekf.x\n");
+    dump(ekf.x, n, m, "%f");
+    printf("Print Matrix of model calculations ekf.fx\n");
+    dump(ekf.fx, n, m, "%f");
+#endif
+
+    /* P_k = F_{k-1} P_{k-1} F^T_{k-1} + Q_{k-1} */
+    mulmat(ekf.F, ekf.P, ekf.tmp0, n, n, n);
+    transpose(ekf.F, ekf.Ft, n, n);
+    mulmat(ekf.tmp0, ekf.Ft, ekf.Pp, n, n, n);
+    accum(ekf.Pp, ekf.Q, n, n);
+#ifdef DEBUG
+    printf("Print Matrix ekf.Pp\n");
+    dump(ekf.Pp, n, n, "%f");
+#endif
+    /* G_k = P_k H^T_k (H_k P_k H^T_k + R)^{-1} */
+    transpose(ekf.H, ekf.Ht, m, n);
+    mulmat(ekf.Pp, ekf.Ht, ekf.tmp1, n, n, m);
+    mulmat(ekf.H, ekf.Pp, ekf.tmp2, m, n, n);
+    mulmat(ekf.tmp2, ekf.Ht, ekf.tmp3, m, n, m);
+    accum(ekf.tmp3, ekf.R, m, m);
+    if (cholsl(ekf.tmp3, ekf.tmp4, ekf.tmp5, m)) {
+#ifdef DEBUG
+        printf("cholsl returned 1");
+#endif
+        return 1;
+    }
+    mulmat(ekf.tmp1, ekf.tmp4, ekf.G, n, m, m);
+#ifdef DEBUG
+    printf("Print Matrix ekf.G\n");
+    dump(ekf.G, n, m, "%f");
+#endif
+/* \hat{x}_k = \hat{x_k} + G_k(z_k - h(\hat{x}_k)) */
+#ifdef DEBUG
+    printf("Print Matrix ekf.x before KF manipulation\n");
+    dump(ekf.x, n, m, "%f");
+    printf("Measured voltage for substraction is %f \n", z[0]);
+    printf("Estimated voltage is Matrix ekf.hx: \n");
+    dump(ekf.hx, m, m, "%f");
+#endif
+    sub(z, ekf.hx, ekf.tmp5, m);
+#ifdef DEBUG
+    printf("Print Matrix Diff (z-hx) \n");
+    dump(ekf.tmp5, m, m, "%f");
+#endif
+    mulvec(ekf.G, ekf.tmp5, ekf.tmp2, n, m);
+    add(ekf.fx, ekf.tmp2, ekf.x, n);
+#ifdef DEBUG
+    printf("Print Matrix ekf.x \n");
+    dump(ekf.x, n, m, "%f");
+#endif
+    /* P_k = (I - G_k H_k) P_k */
+    mulmat(ekf.G, ekf.H, ekf.tmp0, n, m, n);
+    negate(ekf.tmp0, n, n);
+    mat_addeye(ekf.tmp0, n);
+    mulmat(ekf.tmp0, ekf.Pp, ekf.P, n, n, n);
+#ifdef DEBUG
+    printf("Print Matrix ekf.P\n");
+    dump(ekf.P, n, n, "%f");
+#endif
+
+    /* success */
+    return 0;
+}
diff --git a/app/src/kalman_soc.h b/app/src/kalman_soc.h
new file mode 100644
index 00000000..e1e2155c
--- /dev/null
+++ b/app/src/kalman_soc.h
@@ -0,0 +1,25 @@
+/*
+ * TinyEKF: Extended Kalman Filter for embedded processors
+ *
+ * Copyright (C) 2015 Simon D. Levy
+ *
+ * MIT License
+ */
+
+/**
+ * @brief
+ *
+ * @param v Battery voltage to calculate the inital SoC for the EKF
+ * @param n Matrix dimension n columns
+ * @param m Matrix dimension m rows
+ */
+void ekf_init(void *v, int n, int m);
+
+/**
+ * @brief
+ *
+ * @param v Pointer to struct containing EKF Data.
+ * @param z Pointer to voltage measurement for the iteration
+ * @return int Return success, dependend on numercial stability.
+ */
+int ekf_step(void *v, float *z);
diff --git a/app/src/main.cpp b/app/src/main.cpp
index 8d6bccc1..ab2455e9 100644
--- a/app/src/main.cpp
+++ b/app/src/main.cpp
@@ -27,6 +27,7 @@
 #include "leds.h"       // LED switching using charlieplexing
 #include "load.h"       // load and USB output management
 #include "pwm_switch.h" // PWM charge controller
+#include "kalman_soc.h"         // caculation of SoC using extended Kalman filter
 
 void main(void)
 {
@@ -60,7 +61,7 @@ void main(void)
     daq_setup();
 
     charger.detect_num_batteries(&bat_conf); // check if we have 24V instead of 12V system
-    charger.init_terminal(&bat_conf);
+    charger.init_terminal(&bat_conf, &ekf_soc);
 
 #if BOARD_HAS_LOAD_OUTPUT
     load.set_voltage_limits(bat_conf.load_disconnect_voltage, bat_conf.load_reconnect_voltage,
@@ -106,7 +107,7 @@ void main(void)
 
         dev_stat.update_energy();
         dev_stat.update_min_max_values();
-        charger.update_soc(&bat_conf);
+        charger.update_soc(&bat_conf,&ekf_soc);
 
 #if CONFIG_HS_MOSFET_FAIL_SAFE_PROTECTION && BOARD_HAS_DCDC
         if (dev_stat.has_error(ERR_DCDC_HS_MOSFET_SHORT)) {
diff --git a/app/src/setup.cpp b/app/src/setup.cpp
index 2bf9ecac..61bfd354 100644
--- a/app/src/setup.cpp
+++ b/app/src/setup.cpp
@@ -75,6 +75,7 @@ Charger charger(&bat_terminal);
 
 BatConf bat_conf;      // actual (used) battery configuration
 BatConf bat_conf_user; // temporary storage where the user can write to
+EkfSoc ekf_soc;        // Extended Kalman filter configuration
 
 DeviceStatus dev_stat;
 
diff --git a/app/src/setup.h b/app/src/setup.h
index bedf5ef4..5a653bb7 100644
--- a/app/src/setup.h
+++ b/app/src/setup.h
@@ -37,6 +37,7 @@ extern DeviceStatus dev_stat;
 extern Charger charger;
 extern BatConf bat_conf;
 extern BatConf bat_conf_user;
+extern EkfSoc ekf_soc;
 
 #if BOARD_HAS_LOAD_OUTPUT
 extern LoadOutput load;
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 389b4c1b..59a7fd92 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -33,6 +33,7 @@ target_sources(app PRIVATE
         src/tests_half_bridge.cpp
         src/tests_load.cpp
         src/tests_power_port.cpp
+        src/tests_kalman_soc.cpp
 )
 
 # determine git tag and commit hash for automatic firmware versioning
diff --git a/tests/src/main.cpp b/tests/src/main.cpp
index 4b3ddff2..4c808e92 100644
--- a/tests/src/main.cpp
+++ b/tests/src/main.cpp
@@ -33,6 +33,7 @@ void main()
     err += half_bridge_tests();
     err += dcdc_tests();
     err += device_status_tests();
+    err += kalman_soc_tests();
     err += load_tests();
 
 #ifdef CONFIG_CUSTOM_TESTS
diff --git a/tests/src/tests.h b/tests/src/tests.h
index c0d0eb60..3c2af09f 100644
--- a/tests/src/tests.h
+++ b/tests/src/tests.h
@@ -19,6 +19,8 @@ int dcdc_tests();
 
 int device_status_tests();
 
+int kalman_soc_tests();
+
 int load_tests();
 
 #ifdef CONFIG_CUSTOM_TESTS
diff --git a/tests/src/tests_bat_charger.cpp b/tests/src/tests_bat_charger.cpp
index 79922bb5..f1556b34 100644
--- a/tests/src/tests_bat_charger.cpp
+++ b/tests/src/tests_bat_charger.cpp
@@ -14,7 +14,7 @@
 static void init_structs()
 {
     battery_conf_init(&bat_conf, BAT_TYPE_FLOODED, 6, 100);
-    charger.init_terminal(&bat_conf);
+    charger.init_terminal(&bat_conf, &ekf_soc);
     charger.state = CHG_STATE_IDLE;
     charger.bat_temperature = 25;
     bat_terminal.bus->voltage = 14.0;
diff --git a/tests/src/tests_dcdc.cpp b/tests/src/tests_dcdc.cpp
index ed0e156d..4b49a956 100644
--- a/tests/src/tests_dcdc.cpp
+++ b/tests/src/tests_dcdc.cpp
@@ -26,7 +26,7 @@ static void init_structs_buck(int num_batteries = 1)
 
     battery_conf_init(&bat_conf, BAT_TYPE_GEL, 6, 100);
     charger.port = &lv_terminal;
-    charger.init_terminal(&bat_conf);
+    charger.init_terminal(&bat_conf, &ekf_soc);
     lv_terminal.bus->voltage = 14 * num_batteries;
     lv_terminal.bus->series_multiplier = num_batteries;
     lv_terminal.current = 0;
@@ -68,7 +68,7 @@ static void init_structs_boost(int num_batteries = 1)
     int num_cells = (num_batteries == 1) ? 10 : 5;
     battery_conf_init(&bat_conf, BAT_TYPE_NMC, num_cells, 9);
     charger.port = &hv_terminal;
-    charger.init_terminal(&bat_conf);
+    charger.init_terminal(&bat_conf, &ekf_soc);
     hv_terminal.bus->voltage = 3.7 * num_cells * num_batteries;
     hv_terminal.bus->series_multiplier = num_batteries;
     hv_terminal.current = 0;
diff --git a/tests/src/tests_kalman_soc.cpp b/tests/src/tests_kalman_soc.cpp
new file mode 100644
index 00000000..b47779ab
--- /dev/null
+++ b/tests/src/tests_kalman_soc.cpp
@@ -0,0 +1,901 @@
+/*
+ * Copyright (c) The Libre Solar Project Contributors
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "tests.h"
+#include <setup.h>
+#include <stdio.h>
+#include <time.h>
+
+#include "bat_charger.h"
+#include <math.h>
+#include <stdlib.h>
+#include <string.h>
+#include <strings.h>
+
+//#define DEBUG 0
+
+// GPS Model Backtest of TinyEKF Lib
+// See for implemented test example:
+// https://github.com/simondlevy/TinyEKF/tree/master/extras/c
+
+/* states */
+#define Nsta_gps 8
+
+/* observables */
+#define Nobs_gps 4
+
+typedef struct
+{
+
+    int n; /* number of state values */
+    int m; /* number of observables */
+
+    float x[Nsta_gps]; /* state vector */
+
+    float P[Nsta_gps][Nsta_gps]; /* prediction error covariance */
+    float Q[Nsta_gps][Nsta_gps]; /* process noise covariance */
+    float R[Nobs_gps][Nobs_gps]; /* measurement error covariance */
+
+    float G[Nsta_gps][Nobs_gps]; /* Kalman gain; a.k.a. K */
+
+    float F[Nsta_gps][Nsta_gps]; /* Jacobian of process model */
+    float H[Nobs_gps][Nsta_gps]; /* Jacobian of measurement model */
+
+    float Ht[Nsta_gps][Nobs_gps]; /* transpose of measurement Jacobian */
+    float Ft[Nsta_gps][Nsta_gps]; /* transpose of process Jacobian */
+    float Pp[Nsta_gps][Nsta_gps]; /* P, post-prediction, pre-update */
+
+    float fx[Nsta_gps]; /* output of user defined f() state-transition function */
+    float hx[Nobs_gps]; /* output of user defined h() measurement function */
+
+    /* temporary storage */
+    float tmp0[Nsta_gps][Nsta_gps];
+    float tmp1[Nsta_gps][Nobs_gps];
+    float tmp2[Nobs_gps][Nsta_gps];
+    float tmp3[Nobs_gps][Nobs_gps];
+    float tmp4[Nobs_gps][Nobs_gps];
+    float tmp5[Nobs_gps];
+
+} ekf_gps_t;
+
+// positioning interval
+static const float T = 1;
+
+static void blkfill(ekf_gps_t *ekf_gps, const float *a, int off)
+{
+    off *= 2;
+
+    ekf_gps->Q[off][off] = a[0];
+    ekf_gps->Q[off][off + 1] = a[1];
+    ekf_gps->Q[off + 1][off] = a[2];
+    ekf_gps->Q[off + 1][off + 1] = a[3];
+}
+
+static void init_gps(ekf_gps_t *ekf_gps)
+{
+    // Set Q, see [1]
+    const float Sf = 36;
+    const float Sg = 0.01;
+    const float sigma = 5; // state transition variance
+    const float Qb[4] = { Sf * T + Sg * T * T * T / 3, Sg * T * T / 2, Sg * T * T / 2, Sg * T };
+    const float Qxyz[4] = { sigma * sigma * T * T * T / 3, sigma * sigma * T * T / 2,
+                            sigma * sigma * T * T / 2, sigma * sigma * T };
+
+    blkfill(ekf_gps, Qxyz, 0);
+    blkfill(ekf_gps, Qxyz, 1);
+    blkfill(ekf_gps, Qxyz, 2);
+    blkfill(ekf_gps, Qb, 3);
+
+    // initial covariances of state noise, measurement noise
+    float P0 = 10;
+    float R0 = 36;
+
+    int i;
+
+    for (i = 0; i < Nsta_gps; ++i) {
+        ekf_gps->P[i][i] = P0;
+    }
+
+    for (i = 0; i < Nobs_gps; ++i) {
+        ekf_gps->R[i][i] = R0;
+    }
+
+    // position
+    ekf_gps->x[0] = -2.168816181271560e+006;
+    ekf_gps->x[2] = 4.386648549091666e+006;
+    ekf_gps->x[4] = 4.077161596428751e+006;
+
+    // velocity
+    ekf_gps->x[1] = 0;
+    ekf_gps->x[3] = 0;
+    ekf_gps->x[5] = 0;
+
+    // clock bias
+    ekf_gps->x[6] = 3.575261153706439e+006;
+
+    // clock drift
+    ekf_gps->x[7] = 4.549246345845814e+001;
+}
+
+static void model_gps(ekf_gps_t *ekf_gps, float SV[4][3])
+{
+
+    int i, j;
+
+    for (j = 0; j < 8; j += 2) {
+        ekf_gps->fx[j] = ekf_gps->x[j] + T * ekf_gps->x[j + 1];
+        ekf_gps->fx[j + 1] = ekf_gps->x[j + 1];
+    }
+
+    for (j = 0; j < 8; ++j) {
+        ekf_gps->F[j][j] = 1;
+    }
+    for (j = 0; j < 4; ++j) {
+        ekf_gps->F[2 * j][2 * j + 1] = T;
+    }
+    float dx[4][3];
+
+    for (i = 0; i < 4; ++i) {
+        ekf_gps->hx[i] = 0;
+        for (j = 0; j < 3; ++j) {
+            float d = ekf_gps->fx[j * 2] - SV[i][j];
+            dx[i][j] = d;
+            ekf_gps->hx[i] += d * d;
+        }
+        ekf_gps->hx[i] = pow(ekf_gps->hx[i], 0.5) + ekf_gps->fx[6];
+    }
+
+    for (i = 0; i < 4; ++i) {
+        for (j = 0; j < 3; ++j) {
+            ekf_gps->H[i][j * 2] = dx[i][j] / ekf_gps->hx[i];
+        }
+        ekf_gps->H[i][6] = 1;
+    }
+}
+
+#define datasetcolumns 25
+
+typedef struct
+{
+    float P11[datasetcolumns] = { -11602023.9489137, -11602700.409615,  -11603377.0261803,
+                                  -11604053.7986268, -11604730.7269448, -11605407.8111641,
+                                  -11606085.0512816, -11606762.44731,   -11607439.9992582,
+                                  -11608117.7071296, -11608795.5709421, -11609473.590699,
+                                  -11610151.7664095, -11610830.0980858, -11611508.5857306,
+                                  -11612187.2293523, -11612866.0289661, -11613544.9845813,
+                                  -11614224.0961969, -11614903.3638345, -11615582.7874894,
+                                  -11616262.3671785, -11616942.1029125, -11617621.9946924,
+                                  -11618302.042535 };
+    float P12[datasetcolumns] = {
+        14063117.4931116, 14060708.163762,  14058298.6961425, 14055889.0902859, 14053479.3463229,
+        14051069.4642412, 14048659.444148,  14046249.2860925, 14043838.9901378, 14041428.5563671,
+        14039017.9848112, 14036607.2755532, 14034196.4286552, 14031785.4441682, 14029374.3221777,
+        14026963.0627483, 14024551.6659205, 14022140.1317561, 14019728.4603524, 14017316.6517278,
+        14014904.7059932, 14012492.6231827, 14010080.4033526, 14007668.0465943, 14005255.5529418
+    };
+    float P13[datasetcolumns] = {
+        18811434.3112746, 18812823.4023028, 18814212.0761809, 18815600.3328957, 18816988.1723781,
+        18818375.5946411, 18819762.5996289, 18821149.1873193, 18822535.3576819, 18823921.1106749,
+        18825306.4462865, 18826691.3644751, 18828075.8652108, 18829459.9484704, 18830843.6142108,
+        18832226.8624009, 18833609.6930235, 18834992.1060491, 18836374.1014277, 18837755.6791551,
+        18839136.8391736, 18840517.5814695, 18841897.9060167, 18843277.8127688, 18844657.3017124
+    };
+    float P21[datasetcolumns] = { -20853271.5736342, -20855049.9291186, -20856828.1167654,
+                                  -20858606.1364935, -20860383.9882668, -20862161.6719905,
+                                  -20863939.1876304, -20865716.5351111, -20867493.7143885,
+                                  -20869270.7253602, -20871047.5680138, -20872824.2422708,
+                                  -20874600.7480677, -20876377.0853387, -20878153.2540492,
+                                  -20879929.2540921, -20881705.0854518, -20883480.7480603,
+                                  -20885256.2418182, -20887031.5667084, -20888806.7226403,
+                                  -20890581.7095696, -20892356.5274465, -20894131.1761796,
+                                  -20895905.6557381 };
+    float P22[datasetcolumns] = {
+        1806977.21185816, 1805887.13065807, 1804797.28049813, 1803707.66138322, 1802618.27329107,
+        1801529.116235,   1800440.19019116, 1799351.49516123, 1798263.03112756, 1797174.79810804,
+        1796086.79606563, 1794999.0250037,  1793911.48491644, 1792824.17579945, 1791737.097629,
+        1790650.25042617, 1789563.63415556, 1788477.24881424, 1787391.09441818, 1786305.17093311,
+        1785219.47836965, 1784134.01671018, 1783048.78594032, 1781963.78607134, 1780879.01707716
+    };
+    float P23[datasetcolumns] = {
+        16542682.1237923, 16540582.4659657, 16538482.4609004, 16536382.1086646, 16534281.4092741,
+        16532180.3628133, 16530078.9692955, 16527977.2287825, 16525875.1412993, 16523772.7069394,
+        16521669.9256904, 16519566.797618,  16517463.3227698, 16515359.5011966, 16513255.3329117,
+        16511150.818015,  16509045.9564974, 16506940.7484123, 16504835.1938502, 16502729.2928038,
+        16500623.0453534, 16498516.4515241, 16496409.5113475, 16494302.2249049, 16492194.5922053
+    };
+    float P31[datasetcolumns] = { -14355926.017234,  -14356344.1729806, -14356762.4791434,
+                                  -14357180.9357223, -14357599.5427193, -14358018.3001422,
+                                  -14358437.2079998, -14358856.2662888, -14359275.4750231,
+                                  -14359694.8342019, -14360114.3438323, -14360534.0039196,
+                                  -14360953.8144662, -14361373.7754756, -14361793.8869582,
+                                  -14362214.1489185, -14362634.5613554, -14363055.1242758,
+                                  -14363475.8376898, -14363896.7015941, -14364317.715999,
+                                  -14364738.880906,  -14365160.1963243, -14365581.6622592,
+                                  -14366003.2787072 };
+    float P32[datasetcolumns] = {
+        8650961.88410982, 8648384.47686198, 8645806.99474651, 8643229.4378562,  8640651.80627305,
+        8638074.10004161, 8635496.31920119, 8632918.4638651,  8630340.53404078, 8627762.52982713,
+        8625184.45127231, 8622606.29843616, 8620028.07139851, 8617449.77022857, 8614871.39495658,
+        8612292.94564608, 8609714.42239723, 8607135.82525976, 8604557.15426399, 8601978.40952272,
+        8599399.59106402, 8596820.69897167, 8594241.73327994, 8591662.69405015, 8589083.58139421
+    };
+    float P33[datasetcolumns] = {
+        20736354.9805864, 20737164.3397034, 20737973.2627679, 20738781.7497543, 20739589.8006407,
+        20740397.4154165, 20741204.5940731, 20742011.3365787, 20742817.6429346, 20743623.5131133,
+        20744428.9471034, 20745233.94489,   20746038.5064515, 20746842.6317701, 20747646.3208399,
+        20748449.5736446, 20749252.3901568, 20750054.7703644, 20750856.7142618, 20751658.2218172,
+        20752459.2930258, 20753259.9278649, 20754060.1263275, 20754859.8883983, 20755659.214046
+    };
+    float P41[datasetcolumns] = {
+        7475239.67530529, 7472917.32156931, 7470595.0720982,  7468272.92694682, 7465950.88614163,
+        7463628.94979391, 7461307.1179005,  7458985.39057082, 7456663.76782936, 7454342.24973383,
+        7452020.83634093, 7449699.52774197, 7447378.32393047, 7445057.2250017,  7442736.23102901,
+        7440415.34201686, 7438094.55805635, 7435773.87918626, 7433453.30548462, 7431132.83699074,
+        7428812.47375867, 7426492.21586256, 7424172.06332343, 7421852.01624228, 7419532.07462828
+    };
+    float P42[datasetcolumns] = {
+        12966181.2771377, 12967714.4596339, 12969247.7736988, 12970781.2192928, 12972314.7963952,
+        12973848.5049293, 12975382.344894,  12976916.3162136, 12978450.4188688, 12979984.6528181,
+        12981519.0180208, 12983053.5144131, 12984588.1419961, 12986122.9007034, 12987657.7904831,
+        12989192.8113291, 12990727.9631777, 12992263.2459998, 12993798.6597405, 12995334.2043704,
+        12996869.8798502, 12998405.6861275, 12999941.6231851, 13001477.6909524, 13003013.8894202
+    };
+    float P43[datasetcolumns] = {
+        21931576.7921751, 21931442.6029888, 21931307.9468087, 21931172.8236371, 21931037.233474,
+        21930901.1763249, 21930764.6521883, 21930627.6610695, 21930490.2029686, 21930352.2778878,
+        21930213.8858294, 21930075.0267975, 21929935.7007905, 21929795.9078129, 21929655.647868,
+        21929514.9209546, 21929373.7270772, 21929232.0662369, 21929089.9384372, 21928947.3436792,
+        21928804.2819651, 21928660.7532982, 21928516.7576786, 21928372.2951113, 21928227.3655957
+    };
+    float R1[datasetcolumns] = {
+        23568206.4173783, 23568427.7909862, 23568650.0894557, 23568869.5260895, 23569094.4420916,
+        23569315.4143446, 23569537.8873163, 23569760.0636344, 23569981.9083983, 23570205.8646385,
+        23570427.8664544, 23570650.321976,  23570873.1090517, 23571094.6397118, 23571317.6536404,
+        23571542.272989,  23571765.635922,  23571987.5330366, 23572212.1698355, 23572433.9098983,
+        23572658.6513985, 23572882.7297905, 23573105.2551131, 23573329.6650593, 23573552.3125334
+    };
+    float R2[datasetcolumns] = {
+        26183921.457745,  26184404.1127416, 26184884.7086125, 26185366.6481502, 26185845.7782029,
+        26186327.8049918, 26186808.2263608, 26187289.5027905, 26187768.842246,  26188253.1899141,
+        26188734.3965431, 26189215.4635703, 26189696.8272514, 26190179.3251966, 26190658.5076005,
+        26191142.2270611, 26191622.8229328, 26192101.5167307, 26192584.8348365, 26193065.3609074,
+        26193548.1555067, 26194030.4265996, 26194510.3070126, 26194992.9794606, 26195473.36593
+    };
+    float R3[datasetcolumns] = {
+        24652215.2627705, 24652621.9011857, 24653025.2764103, 24653428.8435874, 24653834.853795,
+        24654241.1781066, 24654645.1117385, 24655052.4830633, 24655456.8704009, 24655862.4792539,
+        24656267.6169511, 24656671.8995876, 24657077.3339386, 24657484.6529132, 24657890.0872643,
+        24658293.6893426, 24658699.8217026, 24659106.9487251, 24659511.3186132, 24659918.7073891,
+        24660325.0840524, 24660732.8916336, 24661138.8145914, 24661542.6609733, 24661950.1370006
+    };
+    float R4[datasetcolumns] = {
+        25606982.9330466, 25606499.4748001, 25606016.697112,  25605534.4603806, 25605048.9604585,
+        25604567.3344846, 25604081.9392636, 25603599.6850818, 25603116.4885881, 25602632.6115359,
+        25602148.1411763, 25601667.632016,  25601183.0395047, 25600699.4416557, 25600219.0895472,
+        25599735.3346461, 25599252.7314594, 25598769.0638094, 25598287.1935317, 25597804.9916998,
+        25597322.2140106, 25596841.2162436, 25596357.5136928, 25595876.9347309, 25595393.4415826
+    };
+} dataset_gps_t;
+
+void test_backtest_gps()
+{
+    dataset_gps_t dataset;
+
+    // Do generic EKF initialization
+    ekf_gps_t ekf_gps;
+    ekf_init(&ekf_gps, Nsta_gps, Nobs_gps);
+
+    // Do local initialization
+    init_gps(&ekf_gps);
+
+    // Make a place to store the data from the file and the output of the EKF
+    float SV_Pos[4][3];
+    float SV_Rho[4];
+    float Pos_KF[25][3];
+
+    int j, k;
+
+    // Loop till no more data
+    for (j = 0; j < 25; ++j) {
+
+        // Load iteration of dataset
+        SV_Pos[0][0] = dataset.P11[j];
+        SV_Pos[0][1] = dataset.P12[j];
+        SV_Pos[0][2] = dataset.P13[j];
+        SV_Pos[1][0] = dataset.P21[j];
+        SV_Pos[1][1] = dataset.P22[j];
+        SV_Pos[1][2] = dataset.P23[j];
+        SV_Pos[2][0] = dataset.P31[j];
+        SV_Pos[2][1] = dataset.P32[j];
+        SV_Pos[2][2] = dataset.P33[j];
+        SV_Pos[3][0] = dataset.P41[j];
+        SV_Pos[3][1] = dataset.P42[j];
+        SV_Pos[3][2] = dataset.P43[j];
+
+        SV_Rho[0] = dataset.R1[j];
+        SV_Rho[1] = dataset.R2[j];
+        SV_Rho[2] = dataset.R3[j];
+        SV_Rho[3] = dataset.R4[j];
+
+        model_gps(&ekf_gps, SV_Pos);
+
+        ekf_step(&ekf_gps, SV_Rho);
+
+        // grab positions, ignoring velocities
+        for (k = 0; k < 3; ++k) {
+            Pos_KF[j][k] = ekf_gps.x[2 * k];
+        }
+    }
+
+    // Compute means of filtered positions
+    float mean_Pos_KF[3] = { 0, 0, 0 };
+    for (j = 0; j < 25; ++j) {
+        for (k = 0; k < 3; ++k) {
+            mean_Pos_KF[k] += Pos_KF[j][k];
+        }
+    }
+    for (k = 0; k < 3; ++k) {
+        mean_Pos_KF[k] /= 25;
+    }
+
+    // Debugging Dump filtered positions minus their means
+    for (j = 0; j < 25; ++j) {
+        // printf("%f ,%f ,%f\n", Pos_KF[j][0]-mean_Pos_KF[0], Pos_KF[j][1]-mean_Pos_KF[1],
+        // Pos_KF[j][2]-mean_Pos_KF[2]); printf("%f %f %f\n", Pos_KF[j][0], Pos_KF[j][1],
+        // Pos_KF[j][2]);
+    }
+
+    TEST_ASSERT_FLOAT_WITHIN(0.00001, -1.61, Pos_KF[24][0] - mean_Pos_KF[0]);
+    TEST_ASSERT_FLOAT_WITHIN(0.00001, 0.5, Pos_KF[24][1] - mean_Pos_KF[1]);
+    TEST_ASSERT_FLOAT_WITHIN(0.00001, -0.58, Pos_KF[24][2] - mean_Pos_KF[2]);
+}
+
+// TinyEKF test with SoC model
+
+void test_ekf_init_func()
+{
+
+    // ekf_soc_t ekf_soc
+    ekf_soc.P[0][0] = 5;
+    ekf_init(&ekf_soc, NUMBER_OF_STATES_SOC, NUMBER_OF_OBSERVABLES_SOC);
+    TEST_ASSERT_EQUAL(true, ekf_soc.P[0][0] == 0.0 && ekf_soc.Q[0][0] == 0.0
+                                && ekf_soc.R[0][0] == 0.0 && ekf_soc.G[0][0] == 0.0
+                                && ekf_soc.F[0][0] == 0.0 && ekf_soc.H[0][0] == 0.0);
+
+    // TEST_ASSERT_EQUAL_FLOAT(0.0, ekf_soc.P[0][0]);
+    // TEST_ASSERT_EACH_EQUAL_FLOAT(0.0,*ekf_soc.P,1); // not working yet
+    // TEST_ASSERT_EQUAL_FLOAT(0,0)
+}
+
+// TODO Update to SoC
+void test_ekf_step_func()
+{
+    ekf_gps_t ekf_gps;
+    float SV_Rho[4];
+
+    ekf_init(&ekf_gps, Nsta_gps, Nobs_gps);
+    ekf_gps.P[0][0] = 5;
+    ekf_step(&ekf_gps, SV_Rho);
+    TEST_ASSERT_EQUAL(true, ekf_gps.P[0][0] == 5.0 && ekf_gps.Q[0][0] == 0.0
+                                && ekf_gps.R[0][0] == 0.0 && ekf_gps.G[0][0] == 0.0
+                                && ekf_gps.F[0][0] == 0.0 && ekf_gps.H[0][0] == 0.0);
+
+    // TEST_ASSERT_EQUAL_FLOAT(0.0, ekf_gps.P[0][0]);
+    // TEST_ASSERT_EACH_EQUAL_FLOAT(0.0,&ekf_gps.P,4); // not working yet
+    // TEST_ASSERT_EQUAL_FLOAT(0,0)
+}
+
+/// Test all functions implemented in bat_charger.cpp h & f and init_SoC functions
+
+void test_clamp_func()
+{
+    float value, min, max, result;
+    min = 0;
+    max = 100000;
+    value = 200000;
+    result = clamp(value, min, max);
+    TEST_ASSERT_FLOAT_WITHIN(0, 100000, result);
+}
+
+void test_calculate_initial_soc_func()
+{
+    float initialSoC, batteryVoltagemV;
+    batteryVoltagemV = 12000;
+    initialSoC = calculate_initial_soc(batteryVoltagemV);
+    TEST_ASSERT_FLOAT_WITHIN(0, 30000, initialSoC);
+}
+
+void test_init_soc_func_should_init_with_calculated_soc()
+{
+    // ekf_soc_t  ekf_soc
+    float P0 = 0.1;   // initial covariance of state noise  (aka process noise)
+    float Q0 = 0.001; // Initial state uncertainty covariance matrix
+    float R0 = 0.1;   // initial covariance of measurement noise
+
+    // uint32_t batteryEff = 10;
+    float v0 = 13000;
+    float initialSoC = 0xFFFFFFFFFFFFFFFF; // forces new SoC to be calculated
+    init_soc(&ekf_soc, v0, P0, Q0, R0, initialSoC);
+    TEST_ASSERT_FLOAT_WITHIN(0, 100000, ekf_soc.x[0]);
+}
+
+void test_init_soc_func_should_init_with_initial_soc()
+{
+    // ekf_soc_t  ekf_soc
+    float P0 = 0.1;   // initial covariance of state noise  (aka process noise)
+    float Q0 = 0.001; // Initial state uncertainty covariance matrix
+    float R0 = 0.1;   // initial covariance of measurement noise
+    // uint32_t batteryEff = 10;
+    float v0 = 13000;
+    float initialSoC = 10.0; // forces new SoC to be calculated
+
+    // uint32_t initialSoC = 10;
+    init_soc(&ekf_soc, v0, P0, Q0, R0, initialSoC);
+    TEST_ASSERT_FLOAT_WITHIN(0, 10, ekf_soc.x[0]);
+}
+
+void test_f_func()
+{
+    // ekf_soc_t  ekf_soc
+    bool isBatteryInFloat = false;
+    float batteryEff = 100000;
+    float batteryCurrentmA = 1000;
+    float samplePeriodMilliSec = 100;
+    float batteryCapacity = 50;
+    f(&ekf_soc, isBatteryInFloat, batteryEff, batteryCurrentmA, samplePeriodMilliSec,
+      batteryCapacity);
+}
+
+void test_h_func()
+{
+    // ekf_soc_t  ekf_soc
+    float batteryCurrentmA = 1000;
+    h(&ekf_soc, batteryCurrentmA);
+}
+
+void test_should_increase_soc_no_float_leadacid_12V()
+{
+    int cholsl_error = 0;
+    const uint32_t SOC_SCALED_HUNDRED_PERCENT = 100000;
+
+    // Do generic EKF initialization
+    // ekf_soc_t ekf_soc
+    ekf_init(&ekf_soc, NUMBER_OF_STATES_SOC, NUMBER_OF_OBSERVABLES_SOC);
+
+    // Do local initialization
+    float P0 = 0.1;   // initial covariance of state noise  (aka process noise)
+    float Q0 = 0.001; // Initial state uncertainty covariance matrix
+    float R0 = 0.1;   // initial covariance of measurement noise
+    float batteryVoltagemV[1] = {
+        12500
+    }; // intial Voltage measurement to calculate SoC if initialSoc is out of range
+    const float batteryCapacityAh = 50;
+    float initialSoC = 50000;
+
+    float batteryEff = 100000;
+    batteryVoltagemV[0] = 12500;
+    float batteryCurrentmA = 1000;
+    float samplePeriodMilliSec = 1000;
+    bool isBatteryInFloat = false;
+
+    float expectedResult = 50053.7539;
+#ifdef DEBUG
+    printf("The SoC before init_soc %f \n", ekf_soc.x[0]);
+#endif
+
+    init_soc(&ekf_soc, batteryVoltagemV[0], P0, Q0, R0, initialSoC);
+
+#ifdef DEBUG
+    printf("The SoC by init_soc %f \n", ekf_soc.x[0]);
+#endif
+
+    batteryEff = model_soc(&ekf_soc, isBatteryInFloat, batteryEff, batteryCurrentmA,
+                           samplePeriodMilliSec, batteryCapacityAh);
+
+#ifdef DEBUG
+    printf("battvol inside test %f \n", batteryVoltagemV[0]);
+    printf("The SoC before ekf_step %f \n", ekf_soc.x[0]);
+#endif
+
+    cholsl_error = ekf_step(&ekf_soc, batteryVoltagemV);
+
+#ifdef DEBUG
+    if (cholsl_error != 0) {
+        printf("EKFSTEP Failed %d \n", cholsl_error);
+    }
+    printf("The SoC before clamp %f \n", ekf_soc.x[0]);
+#endif
+
+    ekf_soc.x[0] = clamp((float)ekf_soc.x[0], 0, SOC_SCALED_HUNDRED_PERCENT);
+
+    TEST_ASSERT_FLOAT_WITHIN(1, expectedResult, ekf_soc.x[0]);
+}
+
+void test_update_soc_should_increase_soc_no_float_leadacid_12V()
+{
+    float expectedResult = 50053.7539;
+    charger.soc = 50;
+    charger.port->bus->voltage = 12.500;
+    charger.init_terminal(&bat_conf, &ekf_soc);
+
+    bat_conf.float_enabled = false;
+    bat_conf.nominal_capacity = 50;
+    charger.port->bus->voltage = 12.500;
+    charger.port->current = 1;
+    charger.update_soc(&bat_conf, &ekf_soc);
+#ifdef DEBUG
+    printf("Soc after EKF and clamp %f\n", ekf_soc.x[0]);
+#endif
+
+    TEST_ASSERT_FLOAT_WITHIN(1, expectedResult, ekf_soc.x[0]);
+}
+
+//// SoC Backtest
+
+#define datasetcolumns_SoC 996
+
+typedef struct
+{
+    float batteryVoltagemV[datasetcolumns_SoC] = {
+        12230, 12240, 12240, 12230, 12230, 12240, 12230, 12230, 12520, 12540, 12560, 12570, 12580,
+        12590, 12590, 12610, 12600, 12610, 12610, 12620, 12620, 12630, 12640, 12630, 12640, 12640,
+        12640, 12640, 12650, 12660, 12660, 12660, 12660, 12670, 12670, 12670, 12670, 12670, 12670,
+        12680, 12670, 12680, 12680, 12680, 12690, 12680, 12680, 12680, 12690, 12680, 12680, 12680,
+        12690, 12690, 12690, 12690, 12690, 12690, 12690, 12690, 12700, 12690, 12690, 12690, 12700,
+        12700, 12700, 12700, 12700, 12700, 12700, 12700, 12700, 12700, 12700, 12700, 12710, 12700,
+        12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710, 12710,
+        12710, 12710, 12710, 12710, 12720, 12720, 12710, 12710, 12710, 12720, 12710, 12720, 12720,
+        12720, 12720, 12720, 12730, 12720, 12720, 12720, 12720, 12730, 12720, 12720, 12720, 12720,
+        12720, 12720, 12720, 12720, 12720, 12720, 12720, 12720, 12720, 12720, 12730, 12730, 12730,
+        12720, 12720, 12720, 12720, 12720, 12720, 12730, 12720, 12720, 12720, 12720, 12720, 12730,
+        12730, 12730, 12730, 12730, 12730, 12740, 12720, 12730, 12730, 12740, 12730, 12730, 12730,
+        12730, 12730, 12730, 12730, 12730, 12730, 12740, 12730, 12730, 12730, 12730, 12730, 12730,
+        12730, 12740, 12730, 12730, 12730, 12730, 12730, 12730, 12730, 12730, 12730, 12730, 12740,
+        12730, 12740, 12730, 12730, 12730, 12740, 12730, 12740, 12740, 12730, 12730, 12730, 12730,
+        12730, 12740, 12730, 12740, 12730, 12740, 12740, 12740, 12730, 12740, 12740, 12740, 12740,
+        12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12750, 12740, 12740, 12740, 12740,
+        12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740, 12740,
+        12740, 12740, 12740, 12740, 12740, 12740, 12750, 12740, 12740, 12740, 12740, 12750, 12740,
+        12750, 12740, 12740, 12750, 12740, 12740, 12740, 12740, 12750, 12750, 12740, 12750, 12740,
+        12750, 12740, 12740, 12750, 12740, 12740, 12750, 12750, 12740, 12740, 12750, 12740, 12750,
+        12750, 12750, 12750, 12750, 12750, 12740, 12750, 12740, 12750, 12740, 12750, 12760, 12750,
+        12740, 12750, 12750, 12760, 12750, 12760, 12750, 12750, 12750, 12750, 12760, 12750, 12750,
+        12760, 12750, 12760, 12750, 12750, 12750, 12750, 12750, 12750, 12750, 12750, 12750, 12750,
+        12750, 12750, 12750, 12750, 12750, 12750, 12750, 12760, 12750, 12760, 12760, 12760, 12750,
+        12760, 12750, 12750, 12750, 12750, 12760, 12760, 12750, 12750, 12750, 12750, 12750, 12760,
+        12750, 12750, 12750, 12750, 12750, 12750, 12750, 12750, 12750, 12760, 12750, 12760, 12750,
+        12760, 12750, 12750, 12760, 12750, 12750, 12750, 12760, 12750, 12750, 12750, 12750, 12750,
+        12750, 12750, 12750, 12770, 12750, 12760, 12760, 12750, 12750, 12750, 12750, 12760, 12750,
+        12750, 12760, 12750, 12750, 12750, 12750, 12750, 12760, 12750, 12750, 12750, 12750, 12750,
+        12760, 12750, 12750, 12760, 12760, 12760, 12760, 12760, 12760, 12750, 12760, 12750, 12750,
+        12750, 12760, 12760, 12750, 12750, 12760, 12750, 12760, 12750, 12760, 12770, 12750, 12760,
+        12750, 12770, 12750, 12750, 12760, 12760, 12760, 12750, 12750, 12760, 12750, 12750, 12760,
+        12750, 12760, 12770, 12750, 12770, 12770, 12750, 12770, 12760, 12760, 12760, 12760, 12760,
+        12770, 12760, 12770, 12750, 12760, 12760, 12760, 12760, 12760, 12750, 12750, 12760, 12760,
+        12760, 12760, 12760, 12760, 12760, 12770, 12760, 12760, 12760, 12760, 12760, 12760, 12760,
+        12760, 12770, 12760, 12760, 12770, 12760, 12760, 12750, 12760, 12770, 12760, 12760, 12760,
+        12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12770, 12760, 12760, 12770,
+        12760, 12770, 12760, 12770, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760,
+        12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760,
+        12760, 12760, 12770, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12760, 12770, 12760,
+        12770, 12770, 12760, 12760, 12770, 12760, 12760, 12760, 12770, 12760, 12770, 12760, 12770,
+        12760, 12770, 12770, 12770, 12760, 12760, 12770, 12760, 12760, 12760, 12770, 12770, 12760,
+        12770, 12770, 12760, 12770, 12760, 12770, 12770, 12760, 12770, 12770, 12770, 12770, 12770,
+        12770, 12770, 12760, 12770, 12770, 12770, 12770, 12770, 12770, 12760, 12770, 12780, 12770,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12760, 12760,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770,
+        12770, 12770, 12770, 12770, 12780, 12770, 12770, 12770, 12770, 12780, 12770, 12770, 12770,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12780, 12770, 12780,
+        12770, 12770, 12770, 12780, 12770, 12780, 12780, 12770, 12780, 12770, 12770, 12770, 12770,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770,
+        12780, 12770, 12780, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12780, 12770, 12770,
+        12770, 12770, 12780, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770,
+        12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12770, 12780,
+        12770, 12780, 12770, 12770, 12770, 12780, 12780, 12770, 12770, 12770, 12770, 12770, 12770,
+        12770, 12780, 12770, 12780, 12770, 12770, 12780, 12770, 12770, 12770, 12770, 12780, 12770,
+        12770, 12770, 12770, 12770, 12770, 12780, 12770, 12780, 12780, 12770, 12770, 12770, 12770,
+        12770, 12780, 12770, 12770, 12770, 12780, 12780, 12780, 12770, 12770, 12770, 12780, 12780,
+        12770, 12770, 12780, 12770, 12770, 12770, 12770, 12780, 12770, 12780, 12780, 12780, 12770,
+        12770, 12780, 12770, 12770, 12780, 12770, 12770, 12780, 12780, 12770, 12770, 12780, 12780,
+        12770, 12780, 12780, 12770, 12770, 12770, 12780, 12780, 12780, 12780, 12780, 12770, 12770,
+        12780, 12780, 12780, 12780, 12780, 12780, 12780, 12790, 12780, 12790, 12780, 12780, 12770,
+        12770, 12780, 12770, 12770, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780,
+        12780, 12790, 12790, 12780, 12780, 12780, 12790, 12780, 12780, 12780, 12780, 12780, 12780,
+        12780, 12780, 12780, 12790, 12780, 12770, 12790, 12780, 12790, 12780, 12790, 12780, 12780,
+        12790, 12780, 12790, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780,
+        12780, 12780, 12780, 12780, 12790, 12780, 12780, 12780, 12790, 12790, 12780, 12770, 12780,
+        12780, 12780, 12770, 12780, 12770, 12770, 12780, 12780, 12780, 12780, 12780, 12780, 12780,
+        12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12770, 12780, 12790, 12780, 12790,
+        12790, 12790, 12790, 12780, 12780, 12790, 12780, 12780, 12790, 12790, 12780, 12780, 12780,
+        12780, 12780, 12780, 12780, 12780, 12780, 12790, 12780, 12780, 12780, 12790, 12780, 12780,
+        12790, 12790, 12790, 12790, 12780, 12790, 12780, 12780, 12780, 12780, 12780, 12780, 12780,
+        12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12790, 12780, 12780, 12780,
+        12790, 12780, 12780, 12790, 12780, 12780, 12780, 12790, 12780, 12790, 12780, 12790, 12790,
+        12780, 12780, 12790, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12780, 12790, 12790,
+        12780, 12780, 12780, 12780, 12790, 12780, 12780, 12790
+    };
+    float batteryCurrentmA[datasetcolumns_SoC] = {
+        -3000, -3000, -3000, -3000, -3000, -3000, -3000, -2990, 10, 10, 10, 10, 10, 10, 10, 0,  0,
+        10,    10,    0,     10,    10,    10,    10,    0,     0,  10, 10, 10, 10, 10, 10, 10, 10,
+        10,    0,     10,    10,    10,    10,    0,     10,    0,  0,  10, 10, 0,  0,  10, 10, 0,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 0,  0,
+        10,    10,    0,     0,     10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 0,  0,  10,
+        10,    10,    10,    0,     10,    10,    0,     0,     0,  10, 10, 10, 10, 10, 10, 0,  10,
+        10,    10,    0,     0,     0,     10,    10,    10,    10, 10, 10, 0,  10, 0,  10, 10, 10,
+        10,    10,    10,    10,    10,    10,    0,     10,    10, 10, 10, 0,  10, 10, 10, 10, 10,
+        10,    0,     10,    10,    0,     255.6, 10,    10,    10, 0,  10, 10, 10, 10, 10, 10, 10,
+        0,     10,    10,    10,    0,     10,    0,     10,    0,  10, 10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    0,     10,    0,     10, 0,  10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    10,    0,     10,    10, 10, 0,  10, 10, 10, 0,  10, 10,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    0,     10,    0,     0,  0,  10, 10, 10, 10, 10, 0,  10,
+        10,    10,    10,    10,    10,    10,    10,    10,    0,  10, 0,  10, 10, 10, 10, 0,  0,
+        10,    10,    10,    10,    10,    10,    10,    0,     10, 10, 10, 10, 10, 0,  10, 10, 10,
+        10,    10,    10,    10,    10,    10,    10,    0,     10, 0,  10, 10, 10, 10, 10, 10, 10,
+        10,    0,     10,    10,    10,    10,    0,     10,    0,  10, 10, 0,  10, 10, 10, 10, 10,
+        0,     10,    20,    10,    10,    10,    10,    10,    0,  10, 0,  10, 10, 10, 10, 10, 10,
+        10,    0,     10,    10,    10,    0,     10,    10,    10, 0,  10, 10, 10, 0,  10, 10, 10,
+        0,     0,     10,    10,    0,     10,    0,     10,    10, 10, 10, 10, 10, 0,  0,  10, 10,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    0,     0,     10,    10,    10,    10, 0,  10, 0,  0,  10, 10, 10, 10,
+        10,    10,    10,    10,    10,    0,     10,    0,     10, 10, 10, 10, 10, 10, 10, 0,  10,
+        10,    10,    0,     0,     10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        0,     10,    10,    0,     0,     10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 0,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        0,     10,    10,    0,     0,     10,    10,    10,    10, 10, 10, 0,  10, 10, 10, 10, 10,
+        10,    10,    10,    10,    0,     0,     10,    10,    0,  10, 0,  10, 10, 10, 10, 10, 10,
+        10,    10,    0,     10,    10,    0,     10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        10,    10,    0,     10,    10,    10,    0,     10,    10, 10, 10, 10, 0,  10, 10, 0,  10,
+        0,     10,    10,    0,     10,    10,    10,    10,    10, 10, 10, 10, 10, 20, 10, 10, 0,
+        10,    0,     0,     0,     10,    0,     10,    0,     10, 10, 10, 10, 0,  0,  10, 0,  10,
+        10,    10,    0,     10,    10,    10,    10,    10,    10, 10, 10, 0,  10, 0,  10, 10, 0,
+        0,     0,     10,    10,    0,     10,    10,    0,     10, 10, 10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 0,  0,  10, 10, 10, 10, 10,
+        10,    0,     10,    10,    10,    10,    0,     10,    0,  10, 10, 10, 10, 10, 10, 10, 10,
+        10,    0,     10,    10,    10,    10,    0,     10,    0,  0,  0,  10, 10, 10, 10, 0,  0,
+        10,    10,    10,    10,    0,     10,    10,    10,    0,  10, 10, 10, 10, 10, 10, 10, 10,
+        20,    10,    10,    10,    10,    10,    0,     10,    0,  10, 10, 0,  0,  0,  10, 0,  0,
+        0,     10,    0,     10,    10,    10,    10,    0,     0,  10, 10, 10, 10, 10, 10, 0,  10,
+        10,    0,     10,    0,     0,     0,     10,    0,     10, 0,  10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        0,     10,    10,    10,    0,     0,     10,    10,    10, 10, 10, 10, 10, 0,  0,  10, 10,
+        10,    10,    10,    10,    0,     10,    10,    10,    10, 10, 10, 10, 10, 10, 10, 0,  10,
+        10,    10,    10,    10,    10,    10,    10,    10,    0,  10, 10, 10, 10, 0,  10, 10, 0,
+        10,    10,    0,     0,     10,    10,    10,    10,    10, 10, 10, 0,  10, 10, 0,  10, 10,
+        10,    10,    10,    0,     10,    10,    0,     10,    0,  10, 0,  10, 0,  10, 10, 10, 10,
+        0,     10,    10,    10,    10,    10,    0,     0,     10, 10, 10, 10, 10, 10, 0,  10, 10,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 0,  10, 10, 10, 10, 10, 10, 10,
+        10,    10,    10,    10,    10,    10,    10,    0,     10, 0,  10, 10, 10, 10, 10, 10, 0,
+        0,     0,     0,     0,     10,    10,    0,     0,     10, 10, 10, 10, 10, 10, 0,  0,  0,
+        10,    0,     10,    10,    10,    10,    10,    0,     10, 10, 10, 10, 10, 10, 0,  10, 0,
+        10,    10,    10,    10,    0,     0,     10,    0,     10, 10, 10, 10, 0,  10, 10, 10, 10,
+        10,    10,    0,     10,    10,    10,    10,    10,    0,  10, 0,  10, 0,  10, 10, 10, 0,
+        0,     10,    10,    10,    10,    0,     10,    10,    10, 10, 10, 10, 10, 10, 10, 10, 10,
+        10,    0,     0,     10,    10,    10,    10,    0,     10, 10, 10, 0,  10, 0,  10, 0,  0,
+        10,    10,    10,    10,    10,    10,    10,    10,    10, 0,  10, 10, 10, 10, 10, 10, 0,
+        10,    10,    0,     10,    10,    10,    0,     10,    10, 10
+    };
+    bool isBatteryInFloat[datasetcolumns_SoC] = {
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false, false, false, false, false, false,
+        false, false, false, false, false, false, false, false
+    };
+    float samplePeriodMilliSec[datasetcolumns_SoC] = {
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,
+        1000, 1000, 1000, 1000, 1000, 1000
+    };
+} dataset_SoC_t;
+
+void test_backtest_SoC()
+{
+    dataset_SoC_t dataset;
+    int cholsl_error;
+    // float batteryVoltage = 13.01; //batteryvoltage measurement TODO delete
+
+    // Do generic EKF initialization
+    // ekf_soc_t ekf_soc
+    ekf_init(&ekf_soc, NUMBER_OF_STATES_SOC, NUMBER_OF_OBSERVABLES_SOC);
+
+    // Do local initialization
+    float P0 = 0.1;   // initial covariance of state noise  (aka process noise)
+    float Q0 = 0.001; // Initial state uncertainty covariance matrix
+    float R0 = 0.1;   // initial covariance of measurement noise
+    float initialSoC = 0xFFFFFFFFFFF;
+    init_soc(&ekf_soc, dataset.batteryVoltagemV[0], P0, Q0, R0, initialSoC);
+    float batteryCapacityAh = 12;
+    float batteryEff = 85000;
+    float expectedResult = 71491.4609;
+    const uint32_t SOC_SCALED_HUNDRED_PERCENT = 100000; // 100% charge = 100000
+
+    int j;
+
+    // Loop till no more data
+    for (j = 1; j < datasetcolumns_SoC; ++j) {
+
+        // $\hat{x}_k = f(\hat{x}_{k-1})$
+        batteryEff =
+            f(&ekf_soc, dataset.isBatteryInFloat[j], batteryEff, dataset.batteryCurrentmA[j],
+              dataset.samplePeriodMilliSec[j], batteryCapacityAh);
+        // update measurable (voltage) based on predicted state (SOC)
+        h(&ekf_soc, dataset.batteryCurrentmA[j]);
+#ifdef DEBUG
+        printf("battvol inside test %f \n", dataset.batteryVoltagemV[j]);
+#endif
+        cholsl_error = ekf_step(&ekf_soc, &dataset.batteryVoltagemV[j]);
+#ifdef DEBUG
+        if (cholsl_error != 0)
+            printf("EKFSTEP Failed");
+#endif
+        ekf_soc.x[0] = clamp((float)ekf_soc.x[0], 0, SOC_SCALED_HUNDRED_PERCENT);
+#ifdef DEBUG
+        printf("\n\n\nThe SoC after clamp %f \n\n\n", ekf_soc.x[0]);
+#endif
+    }
+
+    TEST_ASSERT_FLOAT_WITHIN(1, expectedResult, ekf_soc.x[0]);
+}
+
+int kalman_soc_tests()
+{
+
+    UNITY_BEGIN();
+
+    // RUN_TEST(test_backtest_gps);
+    RUN_TEST(test_ekf_init_func);
+    RUN_TEST(test_ekf_step_func);
+    RUN_TEST(test_clamp_func);
+    RUN_TEST(test_calculate_initial_soc_func);
+    // RUN_TEST(test_init_soc_func_should_init_with_initial_soc);
+    // RUN_TEST(test_init_soc_func_should_init_with_calculated_soc);
+    // RUN_TEST(test_f_func);
+    // RUN_TEST(test_h_func);
+    RUN_TEST(test_should_increase_soc_no_float_leadacid_12V);
+    RUN_TEST(test_update_soc_should_increase_soc_no_float_leadacid_12V);
+
+    RUN_TEST(test_backtest_SoC);
+
+    return UNITY_END();
+}

From 47febf7bc65d85ffe26e5aa192d94083293a5596 Mon Sep 17 00:00:00 2001
From: mulles <mulles@users.noreply.github.com>
Date: Fri, 6 May 2022 15:30:19 +0200
Subject: [PATCH 3/7] Code-Style Correction
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Added space after "," in function charger.update

Co-authored-by: Martin Jäger <17674105+martinjaeger@users.noreply.github.com>
---
 app/src/main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/app/src/main.cpp b/app/src/main.cpp
index ab2455e9..eb735f2e 100644
--- a/app/src/main.cpp
+++ b/app/src/main.cpp
@@ -107,7 +107,7 @@ void main(void)
 
         dev_stat.update_energy();
         dev_stat.update_min_max_values();
-        charger.update_soc(&bat_conf,&ekf_soc);
+        charger.update_soc(&bat_conf, &ekf_soc);
 
 #if CONFIG_HS_MOSFET_FAIL_SAFE_PROTECTION && BOARD_HAS_DCDC
         if (dev_stat.has_error(ERR_DCDC_HS_MOSFET_SHORT)) {

From 41579df69687920d819c97a5e2b0054b481cfa41 Mon Sep 17 00:00:00 2001
From: mulles <mulles@users.noreply.github.com>
Date: Fri, 6 May 2022 15:44:35 +0200
Subject: [PATCH 4/7] Code-Style Correction - Removed Space

Remove Space Between Comment and function
---
 app/src/bat_charger.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/app/src/bat_charger.h b/app/src/bat_charger.h
index 5dc15d31..7bf07115 100644
--- a/app/src/bat_charger.h
+++ b/app/src/bat_charger.h
@@ -149,7 +149,6 @@ float clamp(float value, float min, float max);
  * @param battery_voltage_mV
  * @return float Inital SoC
  */
-
 float calculate_initial_soc(float battery_voltage_mV);
 
 /**

From 9668833ef08a076480fe1befaa9d40106cf36ea3 Mon Sep 17 00:00:00 2001
From: mulles <mulles@users.noreply.github.com>
Date: Fri, 6 May 2022 15:48:28 +0200
Subject: [PATCH 5/7] Code-Style Correction - added empty line

---
 app/src/bat_charger.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/app/src/bat_charger.cpp b/app/src/bat_charger.cpp
index 320511c8..3f97c308 100644
--- a/app/src/bat_charger.cpp
+++ b/app/src/bat_charger.cpp
@@ -753,6 +753,7 @@ void Charger::init_terminal(BatConf *bat, EkfSoc *ekf_soc) const
     ekf_init(ekf_soc, NUMBER_OF_STATES_SOC, NUMBER_OF_OBSERVABLES_SOC);
     init_soc(ekf_soc, battery_voltage_mV[0], P0, Q0, R0, initial_soc);
 }
+
 float clamp(float value, float min, float max)
 {
     if (value > max) {

From 540232ce5942ee9386e71e2df3985eac65d1946c Mon Sep 17 00:00:00 2001
From: mulles <mulles@users.noreply.github.com>
Date: Fri, 6 May 2022 15:52:39 +0200
Subject: [PATCH 6/7] Code-Style Correction

Remove unnecessary empty line
---
 app/src/bat_charger.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/app/src/bat_charger.cpp b/app/src/bat_charger.cpp
index 3f97c308..2b54c98e 100644
--- a/app/src/bat_charger.cpp
+++ b/app/src/bat_charger.cpp
@@ -806,7 +806,6 @@ float f(EkfSoc *ekf_soc, bool is_battery_in_float, float battery_eff, float batt
 
 void h(EkfSoc *ekf_soc, float battery_current_mA)
 {
-
     // _hx is the voltage that most closely matches current SoC (a number)
     // _H is an array of form [ocv gradient, measured current, 1] (the last parameter is the offset)
     // x_[0] = SOC, _x[1] = R0 _x[2]=U1 units are unknown.
@@ -891,7 +890,6 @@ void h(EkfSoc *ekf_soc, float battery_current_mA)
 
 void Charger::update_soc(BatConf *bat_conf, EkfSoc *ekf_soc)
 {
-
     int cholsl_error = 0;
     float battery_eff = 100000; // fixed to 100% implemented to use later on.
     float sample_period_milli_sec = 1000;

From e476f7edf347615df2bda6479dc9411eefb0f275 Mon Sep 17 00:00:00 2001
From: mulles <mulles@users.noreply.github.com>
Date: Tue, 10 May 2022 10:41:32 +0200
Subject: [PATCH 7/7] Correction of a comments

Name the correct function that has been replaced.
---
 app/src/bat_charger.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/app/src/bat_charger.h b/app/src/bat_charger.h
index 7bf07115..e7b74f55 100644
--- a/app/src/bat_charger.h
+++ b/app/src/bat_charger.h
@@ -649,7 +649,7 @@ class Charger
     /**
      * SOC estimation
      *
-     * WARNING: TODO obsolte function, replaced by void charge_control(BatConf *bat_conf);
+     * WARNING: TODO obsolte function, replaced by void update_soc(BatConf *bat_conf, EkfSoc *ekf_soc)
      */
     void update_soc_voltage_based(BatConf *bat_conf);