Federico Ferrari / Mbed 2 deprecated BMS_OX_FMF

A public repository for BMS algorithms for a NUCLEO BOARD.

Dependencies:   mbed

Hi Everyone!

Welcome to this repository from Howey's Research Group at the University of Oxford.

The code published here incorporates BMS algorithms for diagnosis functions such as SOC, SOH and Power estimation on a Kokam 53Ah Li-ion battery. This code was designed to work with a NUCLEO F401-RE board and to be tested with a dSPACE HIL Simulator. A short guide on how the set up works is available at https://bitbucket.org/ff95/bms .

The code is made up of three key parts. "Headers" and "Source" folders and the "main.cpp" file. As the code was generated by converting a Simulink model ( available on the BitBucket page), the headers and source code files generated by the conversion are in the corresponding "Headers" and "Source" folders. The "main.cpp" file sets up the ADC, the USB data transmission and starts the estimation (once a character "y" has been received by the computer it is connected to). It also transmits the data from the estimation via USB. Explanation on how to set up the communication with the board is available at BitBucket webpage, from where a MATLAB file can be downloaded which allows real time communication.

For any questions you can contact the author at federicomariaferrari@gmail.com .

The Simulink and Matlab files, together with a short guide, are all available at: https://bitbucket.org/ff95/bms.

Thanks for trying this out!

Federico

Diff: source/Simulink/EKF.cpp

Revision:
16:a92fd07112f7
Parent:
15:b39f568faa27
Child:
18:4a769322eb39
--- a/source/Simulink/EKF.cpp	Wed Feb 08 18:29:02 2017 +0000
+++ b/source/Simulink/EKF.cpp	Wed Feb 08 18:55:55 2017 +0000
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'EKF'.
 //
-// Model version                  : 1.191
+// Model version                  : 1.209
 // Simulink Coder version         : 8.11 (R2016b) 25-Aug-2016
-// C/C++ source code generated on : Wed Feb  8 18:18:38 2017
+// C/C++ source code generated on : Wed Feb  8 18:47:44 2017
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: STMicroelectronics->ST10/Super10
@@ -34,6 +34,7 @@
 
 // Forward declaration for local functions
 static int16_T EKF_bsearch(const real_T x[4], real_T xi);
+static real_T EKF_interp2(real_T varargin_4, real_T varargin_5);
 static real_T EKF_interp2_dispatch(const real_T V[76], real_T Xq, real_T Yq,
   real_T extrapval);
 
@@ -61,6 +62,108 @@
 }
 
 // Function for MATLAB Function: '<Root>/EKF_SLX'
+static real_T EKF_interp2(real_T varargin_4, real_T varargin_5)
+{
+  real_T Vq;
+  int32_T b_y1;
+  int32_T y2;
+  int16_T low_i;
+  int16_T low_ip1;
+  int16_T high_i;
+  int16_T mid_i;
+  real_T qx1;
+  real_T rx;
+  static const real_T b[16] = { 0.05, 0.1100000000000001, 0.17000000000000004,
+    0.22999999999999998, 0.29000000000000004, 0.35, 0.41000000000000003,
+    0.47000000000000003, 0.53, 0.59, 0.64999999999999991, 0.71, 0.77, 0.83, 0.89,
+    0.95 };
+
+  static const real_T V[64] = { 16576.9425034523, 20138.979107141495,
+    24421.239197254181, 28529.878780245781, 15374.498069286346,
+    21772.293746471405, 25693.107843399048, 32291.115447878838,
+    18555.804342031479, 20658.149644732475, 25824.466645717621,
+    31220.083832740784, 25359.847992658615, 27558.146119117737,
+    28088.040053844452, 32270.592749118805, 23069.943785667419,
+    32527.030557394028, 42899.709641933441, 47403.486669063568,
+    21542.409062385559, 27543.162256479263, 34497.639834880829,
+    38461.93790435791, 20651.820003986359, 25631.530284881592,
+    31862.854510545731, 33635.095953941345, 19963.297843933105,
+    24295.083433389664, 28222.531974315643, 30071.003437042236,
+    19400.93457698822, 23581.461608409882, 28570.601940155029,
+    29711.003303527832, 20319.71737742424, 24107.016921043396,
+    27998.240292072296, 30197.245478630066, 27554.730474948883,
+    30108.308792114258, 31208.517402410507, 34530.265778303146,
+    40256.447046995163, 47065.51194190979, 48255.230188369751,
+    42222.758531570435, 25791.702717542648, 26096.538305282593,
+    26316.764354705811, 28954.031467437744, 20904.739797115326,
+    22026.374936103821, 24867.414683103561, 29067.600816488266,
+    17860.728204250336, 21127.029955387115, 24830.964803695679,
+    28136.543035507202, 16835.392713546753, 21224.110424518585,
+    25584.730058908463, 28471.727073192596 };
+
+  real_T tmp[4];
+  if ((varargin_4 >= 0.05) && (varargin_4 <= 0.95) && (varargin_5 >= 15.0) &&
+      (varargin_5 <= 45.0)) {
+    low_i = 0;
+    low_ip1 = 2;
+    high_i = 16;
+    while (high_i > low_ip1) {
+      mid_i = ((low_i + high_i) + 1) >> 1;
+      if (varargin_4 >= b[mid_i - 1]) {
+        low_i = mid_i - 1;
+        low_ip1 = mid_i + 1;
+      } else {
+        high_i = mid_i;
+      }
+    }
+
+    tmp[0] = 15.0;
+    tmp[1] = 25.0;
+    tmp[2] = 35.0;
+    tmp[3] = 45.0;
+    low_ip1 = EKF_bsearch(tmp, varargin_5);
+    b_y1 = (low_ip1 - 1) * 10L + 15L;
+    y2 = 10L * low_ip1 + 15L;
+    if (varargin_4 == b[low_i]) {
+      qx1 = V[((low_i << 2) + low_ip1) - 1];
+      Vq = V[(low_i << 2) + low_ip1];
+    } else if (b[low_i + 1] == varargin_4) {
+      qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1];
+      Vq = V[((low_i + 1) << 2) + low_ip1];
+    } else {
+      rx = (varargin_4 - b[low_i]) / (b[low_i + 1] - b[low_i]);
+      if (V[(((low_i + 1) << 2) + low_ip1) - 1] == V[((low_i << 2) + low_ip1) -
+          1]) {
+        qx1 = V[((low_i << 2) + low_ip1) - 1];
+      } else {
+        qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1] * rx + V[((low_i << 2) +
+          low_ip1) - 1] * (1.0 - rx);
+      }
+
+      if (V[((low_i + 1) << 2) + low_ip1] == V[(low_i << 2) + low_ip1]) {
+        Vq = V[(low_i << 2) + low_ip1];
+      } else {
+        Vq = V[((low_i + 1) << 2) + low_ip1] * rx + V[(low_i << 2) + low_ip1] *
+          (1.0 - rx);
+      }
+    }
+
+    if ((varargin_5 == b_y1) || (qx1 == Vq)) {
+      Vq = qx1;
+    } else {
+      if (!(varargin_5 == y2)) {
+        rx = (varargin_5 - (real_T)b_y1) / (real_T)(y2 - b_y1);
+        Vq = (1.0 - rx) * qx1 + rx * Vq;
+      }
+    }
+  } else {
+    Vq = (rtNaN);
+  }
+
+  return Vq;
+}
+
+// Function for MATLAB Function: '<Root>/EKF_SLX'
 static real_T EKF_interp2_dispatch(const real_T V[76], real_T Xq, real_T Yq,
   real_T extrapval)
 {
@@ -142,6 +245,7 @@
 // Model step function
 void EKF_step(void)
 {
+  real_T C1;
   real_T Ahat[9];
   real_T Bhat[3];
   real_T C[3];
@@ -154,41 +258,38 @@
   real_T rx;
   real_T ry;
   int16_T b_mid_i;
-  real_T b_ry;
-  static const real_T d[16] = { 0.05, 0.1100000000000001, 0.17000000000000004,
-    0.22999999999999998, 0.29000000000000004, 0.35, 0.41000000000000003,
-    0.47000000000000003, 0.53, 0.59, 0.64999999999999991, 0.71, 0.77, 0.83, 0.89,
-    0.95 };
+  static const real_T d[76] = { 1.2199999999999978, 1.2699999999999967,
+    1.2900000000000034, 1.3100000000000012, 1.12, 1.169999999999999, 1.2,
+    1.2100000000000033, 1.04, 1.0900000000000034, 1.1199999999999957,
+    1.1199999999999957, 0.97999999999999776, 1.0300000000000011,
+    1.0499999999999989, 1.0499999999999989, 0.92999999999999883,
+    0.97999999999999776, 1.0, 1.0, 1.0299999999999967, 1.1100000000000012,
+    1.1399999999999979, 1.1199999999999957, 1.0499999999999989,
+    1.0599999999999978, 1.0599999999999978, 1.0499999999999989,
+    0.79000000000000115, 0.73999999999999777, 0.71000000000000107,
+    0.72999999999999887, 0.57999999999999785, 0.56, 0.55000000000000115, 0.56,
+    0.47000000000000108, 0.4499999999999989, 0.44, 0.44, 0.38000000000000222,
+    0.37000000000000333, 0.36, 0.36, 0.32, 0.32, 0.32999999999999891,
+    0.34000000000000224, 0.28999999999999893, 0.38999999999999668,
+    0.48999999999999888, 0.50999999999999668, 0.38999999999999668,
+    0.52999999999999892, 0.59000000000000119, 0.609999999999999,
+    0.51000000000000112, 0.51000000000000112, 0.51000000000000112,
+    0.51000000000000112, 0.48, 0.52, 0.569999999999999, 0.56, 0.52,
+    0.6599999999999977, 0.6699999999999966, 0.65999999999999781,
+    0.63000000000000111, 0.670000000000001, 1.0999999999999979,
+    1.0499999999999989, 1.3099999999999967, 2.8099999999999987,
+    4.1499999999999968, 4.3099999999999969 };
 
-  static const real_T V[64] = { 16576.9425034523, 20138.979107141495,
-    24421.239197254181, 28529.878780245781, 15374.498069286346,
-    21772.293746471405, 25693.107843399048, 32291.115447878838,
-    18555.804342031479, 20658.149644732475, 25824.466645717621,
-    31220.083832740784, 25359.847992658615, 27558.146119117737,
-    28088.040053844452, 32270.592749118805, 23069.943785667419,
-    32527.030557394028, 42899.709641933441, 47403.486669063568,
-    21542.409062385559, 27543.162256479263, 34497.639834880829,
-    38461.93790435791, 20651.820003986359, 25631.530284881592,
-    31862.854510545731, 33635.095953941345, 19963.297843933105,
-    24295.083433389664, 28222.531974315643, 30071.003437042236,
-    19400.93457698822, 23581.461608409882, 28570.601940155029,
-    29711.003303527832, 20319.71737742424, 24107.016921043396,
-    27998.240292072296, 30197.245478630066, 27554.730474948883,
-    30108.308792114258, 31208.517402410507, 34530.265778303146,
-    40256.447046995163, 47065.51194190979, 48255.230188369751,
-    42222.758531570435, 25791.702717542648, 26096.538305282593,
-    26316.764354705811, 28954.031467437744, 20904.739797115326,
-    22026.374936103821, 24867.414683103561, 29067.600816488266,
-    17860.728204250336, 21127.029955387115, 24830.964803695679,
-    28136.543035507202, 16835.392713546753, 21224.110424518585,
-    25584.730058908463, 28471.727073192596 };
+  static const real_T e[76] = { 3.493, 3.464, 3.407, 3.412, 3.526, 3.495, 3.482,
+    3.483, 3.556, 3.531, 3.517, 3.517, 3.578, 3.561, 3.549, 3.549, 3.604, 3.583,
+    3.574, 3.573, 3.629, 3.612, 3.6, 3.6, 3.643, 3.636, 3.633, 3.634, 3.658,
+    3.651, 3.649, 3.651, 3.675, 3.668, 3.666, 3.668, 3.696, 3.688, 3.685, 3.687,
+    3.722, 3.713, 3.71, 3.712, 3.754, 3.744, 3.74, 3.743, 3.801, 3.787, 3.781,
+    3.785, 3.859, 3.85, 3.846, 3.848, 3.904, 3.898, 3.895, 3.897, 3.952, 3.948,
+    3.946, 3.948, 4.002, 4.001, 4.0, 4.002, 4.056, 4.057, 4.058, 4.06, 4.114,
+    4.118, 4.12, 4.123 };
 
-  static const real_T e[17] = { 0.05, 0.10625000000000007, 0.16250000000000009,
-    0.21875, 0.275, 0.33125000000000004, 0.38749999999999996, 0.44375, 0.5,
-    0.55625, 0.6125, 0.66875, 0.725, 0.78125, 0.83749999999999991,
-    0.89374999999999993, 0.95 };
-
-  static const real_T b_V[68] = { 0.003318750942826959, 0.0023001508295625922,
+  static const real_T V[68] = { 0.003318750942826959, 0.0023001508295625922,
     0.001810091258767631, 0.0014328808446455602, 0.0030545289991703731,
     0.0021115342558727062, 0.0016216012369423278, 0.0012823414045409901,
     0.0027529509371346668, 0.002036583066188943, 0.001545886433903926,
@@ -212,40 +313,29 @@
     0.0012820029410655633, 0.0010935555639352886, 0.0020366598778004175,
     0.0014707546102500505, 0.0012066820015837712, 0.0010181379388362777 };
 
-  static const real_T h[76] = { 3.493, 3.464, 3.407, 3.412, 3.526, 3.495, 3.482,
-    3.483, 3.556, 3.531, 3.517, 3.517, 3.578, 3.561, 3.549, 3.549, 3.604, 3.583,
-    3.574, 3.573, 3.629, 3.612, 3.6, 3.6, 3.643, 3.636, 3.633, 3.634, 3.658,
-    3.651, 3.649, 3.651, 3.675, 3.668, 3.666, 3.668, 3.696, 3.688, 3.685, 3.687,
-    3.722, 3.713, 3.71, 3.712, 3.754, 3.744, 3.74, 3.743, 3.801, 3.787, 3.781,
-    3.785, 3.859, 3.85, 3.846, 3.848, 3.904, 3.898, 3.895, 3.897, 3.952, 3.948,
-    3.946, 3.948, 4.002, 4.001, 4.0, 4.002, 4.056, 4.057, 4.058, 4.06, 4.114,
-    4.118, 4.12, 4.123 };
+  static const real_T h[17] = { 0.05, 0.10625000000000007, 0.16250000000000009,
+    0.21875, 0.275, 0.33125000000000004, 0.38749999999999996, 0.44375, 0.5,
+    0.55625, 0.6125, 0.66875, 0.725, 0.78125, 0.83749999999999991,
+    0.89374999999999993, 0.95 };
 
-  static const real_T i[76] = { 1.2199999999999978, 1.2699999999999967,
-    1.2900000000000034, 1.3100000000000012, 1.12, 1.169999999999999, 1.2,
-    1.2100000000000033, 1.04, 1.0900000000000034, 1.1199999999999957,
-    1.1199999999999957, 0.97999999999999776, 1.0300000000000011,
-    1.0499999999999989, 1.0499999999999989, 0.92999999999999883,
-    0.97999999999999776, 1.0, 1.0, 1.0299999999999967, 1.1100000000000012,
-    1.1399999999999979, 1.1199999999999957, 1.0499999999999989,
-    1.0599999999999978, 1.0599999999999978, 1.0499999999999989,
-    0.79000000000000115, 0.73999999999999777, 0.71000000000000107,
-    0.72999999999999887, 0.57999999999999785, 0.56, 0.55000000000000115, 0.56,
-    0.47000000000000108, 0.4499999999999989, 0.44, 0.44, 0.38000000000000222,
-    0.37000000000000333, 0.36, 0.36, 0.32, 0.32, 0.32999999999999891,
-    0.34000000000000224, 0.28999999999999893, 0.38999999999999668,
-    0.48999999999999888, 0.50999999999999668, 0.38999999999999668,
-    0.52999999999999892, 0.59000000000000119, 0.609999999999999,
-    0.51000000000000112, 0.51000000000000112, 0.51000000000000112,
-    0.51000000000000112, 0.48, 0.52, 0.569999999999999, 0.56, 0.52,
-    0.6599999999999977, 0.6699999999999966, 0.65999999999999781,
-    0.63000000000000111, 0.670000000000001, 1.0999999999999979,
-    1.0499999999999989, 1.3099999999999967, 2.8099999999999987,
-    4.1499999999999968, 4.3099999999999969 };
+  static const real_T i[20] = { 0.0, 0.052631578947368474, 0.10526315789473684,
+    0.15789473684210531, 0.21052631578947367, 0.26315789473684215,
+    0.31578947368421051, 0.368421052631579, 0.42105263157894735,
+    0.47368421052631582, 0.52631578947368429, 0.57894736842105265,
+    0.631578947368421, 0.68421052631578949, 0.736842105263158,
+    0.78947368421052633, 0.84210526315789469, 0.89473684210526316,
+    0.94736842105263164, 1.0 };
+
+  static const real_T j[20] = { 7.5474530734005014E-7, -2.1136432924365931E-5,
+    -2.2263766747057817E-5, -2.151195469496965E-5, -2.2641466893070361E-5,
+    -2.6414980857523514E-5, -2.5285182664320518E-5, -2.490762556365612E-5,
+    -2.3775208552609869E-5, -2.4150473140180349E-5, -2.112638052174597E-5,
+    -2.3024208812905025E-5, -2.2650678687261015E-5, -2.4147027702574732E-5,
+    -2.3771107387690893E-5, -2.1508807517608091E-5, -2.1504094078701937E-5,
+    -2.2268593497227676E-5, -2.1892687685052847E-5, -2.0001423964217564E-5 };
 
   real_T tmp[4];
-  real_T tmp_0[4];
-  real_T tmp_1[3];
+  real_T tmp_0[3];
   real_T Ahat_0[3];
   real_T Ahat_1[9];
   real_T Ahat_2[9];
@@ -305,14 +395,17 @@
   // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   // INTERPOLATE PARAMETRES
   // '<S1>:1:51' C1=interp2(ParamData.C1d_x,ParamData.C1d_y,ParamData.C1_map,Xhat(1),TemperatureIn); 
+  C1 = EKF_interp2(EKF_DW.Xhat[0], EKF_U.TemperatureIn);
+
+  // '<S1>:1:52' R1=interp2(ParamData.R1d_x,ParamData.R1d_y,ParamData.R1_map,Xhat(1),TemperatureIn); 
   if ((EKF_DW.Xhat[0] >= 0.05) && (EKF_DW.Xhat[0] <= 0.95) &&
       ((EKF_U.TemperatureIn >= 15.0) && (EKF_U.TemperatureIn <= 45.0))) {
     low_i = 0;
     low_ip1 = 2;
-    high_i = 16;
+    high_i = 17;
     while (high_i > low_ip1) {
       b_mid_i = ((low_i + high_i) + 1) >> 1;
-      if (EKF_DW.Xhat[0] >= d[b_mid_i - 1]) {
+      if (EKF_DW.Xhat[0] >= h[b_mid_i - 1]) {
         low_i = b_mid_i - 1;
         low_ip1 = b_mid_i + 1;
       } else {
@@ -320,21 +413,21 @@
       }
     }
 
-    tmp_0[0] = 15.0;
-    tmp_0[1] = 25.0;
-    tmp_0[2] = 35.0;
-    tmp_0[3] = 45.0;
-    low_ip1 = EKF_bsearch(tmp_0, EKF_U.TemperatureIn);
+    tmp[0] = 15.0;
+    tmp[1] = 25.0;
+    tmp[2] = 35.0;
+    tmp[3] = 45.0;
+    low_ip1 = EKF_bsearch(tmp, EKF_U.TemperatureIn);
     b_y1 = (low_ip1 - 1) * 10L + 15L;
     y2 = 10L * low_ip1 + 15L;
-    if (EKF_DW.Xhat[0] == d[low_i]) {
+    if (EKF_DW.Xhat[0] == h[low_i]) {
       qx1 = V[((low_i << 2) + low_ip1) - 1];
       rx = V[(low_i << 2) + low_ip1];
-    } else if (d[low_i + 1] == EKF_DW.Xhat[0]) {
+    } else if (h[low_i + 1] == EKF_DW.Xhat[0]) {
       qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1];
       rx = V[((low_i + 1) << 2) + low_ip1];
     } else {
-      rx = (EKF_DW.Xhat[0] - d[low_i]) / (d[low_i + 1] - d[low_i]);
+      rx = (EKF_DW.Xhat[0] - h[low_i]) / (h[low_i + 1] - h[low_i]);
       if (V[(((low_i + 1) << 2) + low_ip1) - 1] == V[((low_i << 2) + low_ip1) -
           1]) {
         qx1 = V[((low_i << 2) + low_ip1) - 1];
@@ -363,65 +456,6 @@
     rx = (rtNaN);
   }
 
-  // '<S1>:1:52' R1=interp2(ParamData.R1d_x,ParamData.R1d_y,ParamData.R1_map,Xhat(1),TemperatureIn); 
-  if ((EKF_DW.Xhat[0] >= 0.05) && (EKF_DW.Xhat[0] <= 0.95) &&
-      ((EKF_U.TemperatureIn >= 15.0) && (EKF_U.TemperatureIn <= 45.0))) {
-    low_i = 0;
-    low_ip1 = 2;
-    high_i = 17;
-    while (high_i > low_ip1) {
-      b_mid_i = ((low_i + high_i) + 1) >> 1;
-      if (EKF_DW.Xhat[0] >= e[b_mid_i - 1]) {
-        low_i = b_mid_i - 1;
-        low_ip1 = b_mid_i + 1;
-      } else {
-        high_i = b_mid_i;
-      }
-    }
-
-    tmp[0] = 15.0;
-    tmp[1] = 25.0;
-    tmp[2] = 35.0;
-    tmp[3] = 45.0;
-    low_ip1 = EKF_bsearch(tmp, EKF_U.TemperatureIn);
-    b_y1 = (low_ip1 - 1) * 10L + 15L;
-    y2 = 10L * low_ip1 + 15L;
-    if (EKF_DW.Xhat[0] == e[low_i]) {
-      qx1 = b_V[((low_i << 2) + low_ip1) - 1];
-      ry = b_V[(low_i << 2) + low_ip1];
-    } else if (e[low_i + 1] == EKF_DW.Xhat[0]) {
-      qx1 = b_V[(((low_i + 1) << 2) + low_ip1) - 1];
-      ry = b_V[((low_i + 1) << 2) + low_ip1];
-    } else {
-      ry = (EKF_DW.Xhat[0] - e[low_i]) / (e[low_i + 1] - e[low_i]);
-      if (b_V[(((low_i + 1) << 2) + low_ip1) - 1] == b_V[((low_i << 2) + low_ip1)
-          - 1]) {
-        qx1 = b_V[((low_i << 2) + low_ip1) - 1];
-      } else {
-        qx1 = b_V[(((low_i + 1) << 2) + low_ip1) - 1] * ry + b_V[((low_i << 2) +
-          low_ip1) - 1] * (1.0 - ry);
-      }
-
-      if (b_V[((low_i + 1) << 2) + low_ip1] == b_V[(low_i << 2) + low_ip1]) {
-        ry = b_V[(low_i << 2) + low_ip1];
-      } else {
-        ry = b_V[((low_i + 1) << 2) + low_ip1] * ry + b_V[(low_i << 2) + low_ip1]
-          * (1.0 - ry);
-      }
-    }
-
-    if ((EKF_U.TemperatureIn == b_y1) || (qx1 == ry)) {
-      ry = qx1;
-    } else {
-      if (!(EKF_U.TemperatureIn == y2)) {
-        b_ry = (EKF_U.TemperatureIn - (real_T)b_y1) / (real_T)(y2 - b_y1);
-        ry = (1.0 - b_ry) * qx1 + b_ry * ry;
-      }
-    }
-  } else {
-    ry = (rtNaN);
-  }
-
   // '<S1>:1:53' tau=C1*R1;
   // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   // DISCRETE TIME SS MATRICES
@@ -429,7 +463,7 @@
   // '<S1>:1:58'     0 exp(-TimeStep/tau) 0;
   // '<S1>:1:59'     0     0        1;];
   Ahat[1] = 0.0;
-  Ahat[4] = std::exp(-EKF_U.TimeStep / (rx * ry));
+  Ahat[4] = std::exp(-EKF_U.TimeStep / (C1 * rx));
   Ahat[7] = 0.0;
   Ahat[0] = 1.0;
   Ahat[2] = 0.0;
@@ -442,7 +476,7 @@
   // '<S1>:1:63'     TimeStep/C1
   // '<S1>:1:64'     1];
   Bhat[0] = -EKF_U.TimeStep / (EKF_DW.Qhat * 3600.0);
-  Bhat[1] = EKF_U.TimeStep / rx;
+  Bhat[1] = EKF_U.TimeStep / C1;
   Bhat[2] = 1.0;
 
   // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -459,20 +493,20 @@
     EKF_DW.P0th += 0.1;
 
     // '<S1>:1:77' Cth=+Current_prev*TimeStep/(ThetaHat^2)+ CthPrev;
-    rx = EKF_DW.Current_prev * EKF_U.TimeStep / (EKF_DW.ThetaHat *
+    C1 = EKF_DW.Current_prev * EKF_U.TimeStep / (EKF_DW.ThetaHat *
       EKF_DW.ThetaHat) + EKF_DW.CthPrev;
 
     // '<S1>:1:78' KGainTh=P0th*(Cth')*(Cth*P0th*Cth'+eTh)^-1;
-    qx1 = 1.0 / (rx * EKF_DW.P0th * rx + 0.0001) * (EKF_DW.P0th * rx);
+    qx1 = 1.0 / (C1 * EKF_DW.P0th * C1 + 0.0001) * (EKF_DW.P0th * C1);
 
     // '<S1>:1:79' ThetaHat=ThetaHat+KGainTh*(Xhat(1)-XhatTh(1));
     EKF_DW.ThetaHat += (EKF_DW.Xhat[0] - EKF_DW.XhatTh) * qx1;
 
     // '<S1>:1:80' P0th=P0th-KGainTh*Cth*KGainTh';
-    EKF_DW.P0th -= qx1 * rx * qx1;
+    EKF_DW.P0th -= qx1 * C1 * qx1;
 
     // '<S1>:1:81' CthPrev=Cth;
-    EKF_DW.CthPrev = rx;
+    EKF_DW.CthPrev = C1;
 
     // '<S1>:1:82' Qhat=ThetaHat;
     EKF_DW.Qhat = EKF_DW.ThetaHat;
@@ -488,13 +522,13 @@
   // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   // PREDICTION FOR STATES
   // '<S1>:1:91' Xhat=Ahat*Xhat+Bhat.*[Current_prev Current_prev 0]';
-  tmp_1[0] = EKF_DW.Current_prev;
-  tmp_1[1] = EKF_DW.Current_prev;
-  tmp_1[2] = 0.0;
+  tmp_0[0] = EKF_DW.Current_prev;
+  tmp_0[1] = EKF_DW.Current_prev;
+  tmp_0[2] = 0.0;
   for (low_i = 0; low_i < 3; low_i++) {
     Ahat_0[low_i] = ((Ahat[low_i + 3] * EKF_DW.Xhat[1] + Ahat[low_i] *
                       EKF_DW.Xhat[0]) + Ahat[low_i + 6] * EKF_DW.Xhat[2]) +
-      Bhat[low_i] * tmp_1[low_i];
+      Bhat[low_i] * tmp_0[low_i];
   }
 
   // '<S1>:1:92' P0=Ahat*P0*Ahat' + Bhat*Q*Bhat';
@@ -526,71 +560,112 @@
   // '<S1>:1:101' dOCVatSOC=interp2(ParamData.dOCVdatad_x,ParamData.dOCVdatad_y,ParamData.dOCVdata_map,Xhat(1),TemperatureIn); 
   // '<S1>:1:104' Yhat=OCVatSOC -Xhat(2) -exp(Xhat(3))*Current;
   // '<S1>:1:105' C=[dOCVatSOC -1 -exp(Xhat(3))*Current];
-  qx1 = EKF_interp2_dispatch(i, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN));
-  C[0] = EKF_interp2_dispatch(i, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN));
+  rx = EKF_interp2_dispatch(d, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN));
+  C[0] = EKF_interp2_dispatch(d, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN));
   C[1] = -1.0;
   C[2] = -std::exp(EKF_DW.Xhat[2]) * EKF_U.Current;
 
   // '<S1>:1:106' KGain=P0*C'*(C*P0*C'+R)^-1;
-  ry = 0.0;
+  qx1 = 0.0;
   for (low_i = 0; low_i < 3; low_i++) {
     EKF_DW.P0[3 * low_i] = Ahat_2[3 * low_i] + Bhat_0[3 * low_i];
-    rx = EKF_DW.P0[3 * low_i] * qx1;
+    C1 = EKF_DW.P0[3 * low_i] * rx;
     EKF_DW.P0[1 + 3 * low_i] = Ahat_2[3 * low_i + 1] + Bhat_0[3 * low_i + 1];
-    rx += -EKF_DW.P0[3 * low_i + 1];
+    C1 += -EKF_DW.P0[3 * low_i + 1];
     EKF_DW.P0[2 + 3 * low_i] = Ahat_2[3 * low_i + 2] + Bhat_0[3 * low_i + 2];
-    rx += EKF_DW.P0[3 * low_i + 2] * C[2];
-    ry += rx * C[low_i];
+    C1 += EKF_DW.P0[3 * low_i + 2] * C[2];
+    qx1 += C1 * C[low_i];
   }
 
-  rx = 1.0 / (ry + 0.01);
+  C1 = 1.0 / (qx1 + 0.01);
   for (low_i = 0; low_i < 3; low_i++) {
     Bhat[low_i] = (EKF_DW.P0[low_i + 6] * C[2] + (-EKF_DW.P0[low_i + 3] +
-      EKF_DW.P0[low_i] * qx1)) * rx;
+      EKF_DW.P0[low_i] * rx)) * C1;
   }
 
   // '<S1>:1:107' Xhat=Xhat+KGain*(Ytrue-Yhat);
-  rx = EKF_U.Voltage - ((EKF_interp2_dispatch(h, EKF_DW.Xhat[0],
+  C1 = EKF_U.Voltage - ((EKF_interp2_dispatch(e, EKF_DW.Xhat[0],
     EKF_U.TemperatureIn, (rtNaN)) - EKF_DW.Xhat[1]) - std::exp(EKF_DW.Xhat[2]) *
                         EKF_U.Current);
 
   // '<S1>:1:108' P0=P0-KGain*(C*P0*C'+R)*KGain';
-  ry = 0.0;
+  qx1 = 0.0;
   for (low_i = 0; low_i < 3; low_i++) {
-    EKF_DW.Xhat[low_i] += Bhat[low_i] * rx;
-    ry += (EKF_DW.P0[3 * low_i + 2] * C[2] + (-EKF_DW.P0[3 * low_i + 1] +
-            EKF_DW.P0[3 * low_i] * qx1)) * C[low_i];
+    EKF_DW.Xhat[low_i] += Bhat[low_i] * C1;
+    qx1 += (EKF_DW.P0[3 * low_i + 2] * C[2] + (-EKF_DW.P0[3 * low_i + 1] +
+             EKF_DW.P0[3 * low_i] * rx)) * C[low_i];
   }
 
   for (low_i = 0; low_i < 3; low_i++) {
-    EKF_DW.P0[low_i] -= (ry + 0.01) * Bhat[low_i] * Bhat[0];
-    EKF_DW.P0[low_i + 3] -= (ry + 0.01) * Bhat[low_i] * Bhat[1];
-    EKF_DW.P0[low_i + 6] -= (ry + 0.01) * Bhat[low_i] * Bhat[2];
+    EKF_DW.P0[low_i] -= (qx1 + 0.01) * Bhat[low_i] * Bhat[0];
+    EKF_DW.P0[low_i + 3] -= (qx1 + 0.01) * Bhat[low_i] * Bhat[1];
+    EKF_DW.P0[low_i + 6] -= (qx1 + 0.01) * Bhat[low_i] * Bhat[2];
   }
 
-  // Outport: '<Root>/R0' incorporates:
-  //   MATLAB Function: '<Root>/EKF_SLX'
-
   // '<S1>:1:111' alpha=0.9995;
   // '<S1>:1:112' R0_filt=R0_filt_prev*alpha + (1-alpha)*exp(Xhat(3));
-  EKF_Y.R0 = EKF_DW.R0_filt_prev * 0.9995 + 0.00049999999999994493 * std::exp
+  C1 = EKF_DW.R0_filt_prev * 0.9995 + 0.00049999999999994493 * std::exp
     (EKF_DW.Xhat[2]);
 
-  // MATLAB Function: '<Root>/EKF_SLX'
   // '<S1>:1:113' R0_filt_prev=exp(Xhat(3));
   EKF_DW.R0_filt_prev = EKF_DW.Xhat[2];
   EKF_DW.R0_filt_prev = std::exp(EKF_DW.R0_filt_prev);
 
+  // '<S1>:1:116' [EstimatedT25R0,~,~]=InterpolateR0(Xhat(1),TemperatureIn, R0_filt,ParamData); 
+  // THIS SCRIPT INTERPOLATES THE DATA CORRESPONDING TO THE CORRECT SLOPE
+  // IT NORMALIZES THE RESULT BY DETERMINING THE CORRESPONDING VALUE AT 25°C AT
+  // THE CURRENT SOC VALUE
+  // IT ASSUMES IT KNOWS THE BEHAVIOUR OF THE DATA BUT NOT THE VALUE (I
+  // CALCULATE THE ESTIMATED R0 AND I CAN USE THE KNOWN PARAMETERS TO GET THE
+  // 25°C VALUE
+  // 'InterpolateR0:11' zData=fliplr(ParamData.zR0);
+  // 'InterpolateR0:12' SlopeData= fliplr(ParamData.SlopeR0);
+  // 'InterpolateR0:14' InterpSlope=interp1(zData,SlopeData,SOC);
+  qx1 = (rtNaN);
+  if ((!rtIsNaN(EKF_DW.Xhat[0])) && (!(EKF_DW.Xhat[0] > 1.0)) && (!(EKF_DW.Xhat
+        [0] < 0.0))) {
+    low_i = 0;
+    low_ip1 = 2;
+    high_i = 20;
+    while (high_i > low_ip1) {
+      b_mid_i = ((low_i + high_i) + 1) >> 1;
+      if (EKF_DW.Xhat[0] >= i[b_mid_i - 1]) {
+        low_i = b_mid_i - 1;
+        low_ip1 = b_mid_i + 1;
+      } else {
+        high_i = b_mid_i;
+      }
+    }
+
+    qx1 = (EKF_DW.Xhat[0] - i[low_i]) / (i[low_i + 1] - i[low_i]);
+    if (qx1 == 0.0) {
+      qx1 = j[low_i];
+    } else if (qx1 == 1.0) {
+      qx1 = j[low_i + 1];
+    } else if (j[low_i + 1] == j[low_i]) {
+      qx1 = j[low_i];
+    } else {
+      qx1 = (1.0 - qx1) * j[low_i] + j[low_i + 1] * qx1;
+    }
+  }
+
+  // 'InterpolateR0:16' ParamOffset=XhatParam-InterpSlope*MeasTemp;
+  // 'InterpolateR0:17' Param25Value=InterpSlope*25+ParamOffset;
+  // 'InterpolateR0:20' EstimatedT25R0=Param25Value;
+  // DETERMINE THE CORRECT R0 AT 25 VALUE
+  // 'InterpolateR0:24' TrueT25R0=interp2(ParamData.R0d_x,ParamData.R0d_y,ParamData.R0_map,SOC,25); 
+  // 'InterpolateR0:26' DegradationIndex=EstimatedT25R0/TrueT25R0;
+  // '<S1>:1:116' ~
   // MAKE SURE SOC IS WITHIN LIMITS
-  // '<S1>:1:117' if Xhat(1)>1
+  // '<S1>:1:120' if Xhat(1)>1
   if (EKF_DW.Xhat[0] > 1.0) {
-    // '<S1>:1:118' Xhat(1)=1;
+    // '<S1>:1:121' Xhat(1)=1;
     EKF_DW.Xhat[0] = 1.0;
   }
 
-  // '<S1>:1:121' if Xhat(1)<0
+  // '<S1>:1:124' if Xhat(1)<0
   if (EKF_DW.Xhat[0] < 0.0) {
-    // '<S1>:1:122' Xhat(1)=0;
+    // '<S1>:1:125' Xhat(1)=0;
     EKF_DW.Xhat[0] = 0.0;
   }
 
@@ -598,15 +673,22 @@
   //   MATLAB Function: '<Root>/EKF_SLX'
 
   // OUTPUT THE VALUES
-  // '<S1>:1:127' SOC=Xhat(1);
-  // '<S1>:1:128' R0=R0_filt;
-  // '<S1>:1:129' Q0=ThetaHat;
+  // '<S1>:1:130' SOC=Xhat(1);
+  // '<S1>:1:131' Q0=ThetaHat;
+  // '<S1>:1:132' R0_25C=EstimatedT25R0;
+  // TRUE VALUE
   EKF_Y.SOC = EKF_DW.Xhat[0];
 
   // Outport: '<Root>/Q0' incorporates:
   //   MATLAB Function: '<Root>/EKF_SLX'
 
   EKF_Y.Q0 = EKF_DW.ThetaHat;
+
+  // Outport: '<Root>/R0_25C' incorporates:
+  //   Inport: '<Root>/TemperatureIn'
+  //   MATLAB Function: '<Root>/EKF_SLX'
+
+  EKF_Y.R0_25C = (C1 - qx1 * EKF_U.TemperatureIn) + qx1 * 25.0;
 }
 
 // Model initialize function