Diff: source/Simulink/EKF.cpp

Revision:

14:4b5df635f248

Parent:

13:831eab218c33

Child:

15:b39f568faa27

--- a/source/Simulink/EKF.cpp	Mon Jan 30 19:31:17 2017 +0000
+++ b/source/Simulink/EKF.cpp	Tue Feb 07 12:20:57 2017 +0000
@@ -7,9 +7,9 @@
 //
 // Code generated for Simulink model 'EKF'.
 //
-// Model version                  : 1.116
+// Model version                  : 1.161
 // Simulink Coder version         : 8.11 (R2016b) 25-Aug-2016
-// C/C++ source code generated on : Mon Jan 16 12:41:30 2017
+// C/C++ source code generated on : Wed Feb  1 14:05:22 2017
 //
 // Target selection: ert.tlc
 // Embedded hardware selection: STMicroelectronics->ST10/Super10
@@ -33,524 +33,229 @@
 RT_MODEL_EKF_T *const EKF_M = &EKF_M_;
 
 // Forward declaration for local functions
-static int16_T EKF_ixamax(int16_T n, const real_T x[13], int16_T ix0);
-static void EKF_xswap(real_T x[273], int16_T ix0, int16_T iy0);
-static real_T EKF_xnrm2_i(int16_T n, const real_T x[273], int16_T ix0);
-static real_T EKF_xzlarfg(int16_T n, real_T *alpha1, real_T x[273], int16_T ix0);
-static void EKF_xzlarf(int16_T m, int16_T n, int16_T iv0, real_T tau, real_T C
-  [273], int16_T ic0, real_T work[13]);
-static real_T EKF_xnrm2_iz(int16_T n, const real_T x[273], int16_T ix0);
-static real_T EKF_xnrm2(const real_T x[273], int16_T ix0);
-static void EKF_xgeqp3(real_T A[273], real_T tau[13], int16_T jpvt[13]);
-static void EKF_qrsolve(const real_T A[273], const real_T B[21], real_T Y[13],
-  int16_T *rankR);
-static void EKF_polyfit(const real_T x[21], const real_T y[21], real_T p[13]);
-
-// Function for MATLAB Function: '<Root>/EKF_SLX'
-static int16_T EKF_ixamax(int16_T n, const real_T x[13], int16_T ix0)
-{
-  int16_T idxmax;
-  int16_T ix;
-  real_T smax;
-  real_T s;
-  int16_T k;
-  if (n < 1) {
-    idxmax = 0;
-  } else {
-    idxmax = 1;
-    if (n > 1) {
-      ix = ix0 - 1;
-      smax = std::abs(x[ix0 - 1]);
-      for (k = 2; k <= n; k++) {
-        ix++;
-        s = std::abs(x[ix]);
-        if (s > smax) {
-          idxmax = k;
-          smax = s;
-        }
-      }
-    }
-  }
-
-  return idxmax;
-}
+static int16_T EKF_bsearch(const real_T x[4], real_T xi);
+static real_T EKF_interp2_dispatch(const real_T V[76], real_T Xq, real_T Yq,
+  real_T extrapval);
 
 // Function for MATLAB Function: '<Root>/EKF_SLX'
-static void EKF_xswap(real_T x[273], int16_T ix0, int16_T iy0)
-{
-  int16_T ix;
-  int16_T iy;
-  real_T temp;
-  int16_T k;
-  ix = ix0 - 1;
-  iy = iy0 - 1;
-  for (k = 0; k < 21; k++) {
-    temp = x[ix];
-    x[ix] = x[iy];
-    x[iy] = temp;
-    ix++;
-    iy++;
-  }
-}
-
-// Function for MATLAB Function: '<Root>/EKF_SLX'
-static real_T EKF_xnrm2_i(int16_T n, const real_T x[273], int16_T ix0)
+static int16_T EKF_bsearch(const real_T x[4], real_T xi)
 {
-  real_T y;
-  real_T scale;
-  int16_T kend;
-  real_T absxk;
-  real_T t;
-  int16_T k;
-  y = 0.0;
-  if (!(n < 1)) {
-    if (n == 1) {
-      y = std::abs(x[ix0 - 1]);
+  int16_T n;
+  int16_T low_ip1;
+  int16_T high_i;
+  int16_T mid_i;
+  n = 1;
+  low_ip1 = 2;
+  high_i = 4;
+  while (high_i > low_ip1) {
+    mid_i = (n + high_i) >> 1;
+    if (xi >= x[mid_i - 1]) {
+      n = mid_i;
+      low_ip1 = mid_i + 1;
     } else {
-      scale = 2.2250738585072014E-308;
-      kend = (ix0 + n) - 1;
-      for (k = ix0; k <= kend; k++) {
-        absxk = std::abs(x[k - 1]);
-        if (absxk > scale) {
-          t = scale / absxk;
-          y = y * t * t + 1.0;
-          scale = absxk;
-        } else {
-          t = absxk / scale;
-          y += t * t;
-        }
-      }
-
-      y = scale * std::sqrt(y);
+      high_i = mid_i;
     }
   }
 
-  return y;
-}
-
-real_T rt_hypotd_snf(real_T u0, real_T u1)
-{
-  real_T y;
-  real_T a;
-  a = std::abs(u0);
-  y = std::abs(u1);
-  if (a < y) {
-    a /= y;
-    y *= std::sqrt(a * a + 1.0);
-  } else if (a > y) {
-    y /= a;
-    y = std::sqrt(y * y + 1.0) * a;
-  } else {
-    if (!rtIsNaN(y)) {
-      y = a * 1.4142135623730951;
-    }
-  }
-
-  return y;
+  return n;
 }
 
 // Function for MATLAB Function: '<Root>/EKF_SLX'
-static real_T EKF_xzlarfg(int16_T n, real_T *alpha1, real_T x[273], int16_T ix0)
+static real_T EKF_interp2_dispatch(const real_T V[76], real_T Xq, real_T Yq,
+  real_T extrapval)
 {
-  real_T tau;
-  real_T xnorm;
-  int16_T knt;
-  int16_T b_k;
-  int16_T c_k;
-  tau = 0.0;
-  if (!(n <= 0)) {
-    xnorm = EKF_xnrm2_i(n - 1, x, ix0);
-    if (xnorm != 0.0) {
-      xnorm = rt_hypotd_snf(*alpha1, xnorm);
-      if (*alpha1 >= 0.0) {
-        xnorm = -xnorm;
+  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[19] = { 0.05, 0.099999999999999978, 0.15000000000000002,
+    0.20000000000000007, 0.25000000000000011, 0.30000000000000004, 0.35, 0.4,
+    0.44999999999999996, 0.5, 0.55, 0.60000000000000009, 0.64999999999999991,
+    0.7, 0.75, 0.79999999999999993, 0.85, 0.89999999999999991, 0.95 };
+
+  real_T tmp[4];
+  if ((Xq >= 0.05) && (Xq <= 0.95) && (Yq >= 15.0) && (Yq <= 45.0)) {
+    low_i = 0;
+    low_ip1 = 2;
+    high_i = 19;
+    while (high_i > low_ip1) {
+      mid_i = ((low_i + high_i) + 1) >> 1;
+      if (Xq >= 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, Yq);
+    b_y1 = (low_ip1 - 1) * 10L + 15L;
+    y2 = 10L * low_ip1 + 15L;
+    if (Xq == b[low_i]) {
+      qx1 = V[((low_i << 2) + low_ip1) - 1];
+      Vq = V[(low_i << 2) + low_ip1];
+    } else if (b[low_i + 1] == Xq) {
+      qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1];
+      Vq = V[((low_i + 1) << 2) + low_ip1];
+    } else {
+      rx = (Xq - 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 (std::abs(xnorm) < 1.0020841800044864E-292) {
-        knt = 0;
-        do {
-          knt++;
-          b_k = (ix0 + n) - 2;
-          for (c_k = ix0; c_k <= b_k; c_k++) {
-            x[c_k - 1] *= 9.9792015476736E+291;
-          }
-
-          xnorm *= 9.9792015476736E+291;
-          *alpha1 *= 9.9792015476736E+291;
-        } while (!(std::abs(xnorm) >= 1.0020841800044864E-292));
-
-        xnorm = rt_hypotd_snf(*alpha1, EKF_xnrm2_i(n - 1, x, ix0));
-        if (*alpha1 >= 0.0) {
-          xnorm = -xnorm;
-        }
-
-        tau = (xnorm - *alpha1) / xnorm;
-        *alpha1 = 1.0 / (*alpha1 - xnorm);
-        b_k = (ix0 + n) - 2;
-        for (c_k = ix0; c_k <= b_k; c_k++) {
-          x[c_k - 1] *= *alpha1;
-        }
-
-        for (b_k = 1; b_k <= knt; b_k++) {
-          xnorm *= 1.0020841800044864E-292;
-        }
-
-        *alpha1 = xnorm;
+      if (V[((low_i + 1) << 2) + low_ip1] == V[(low_i << 2) + low_ip1]) {
+        Vq = V[(low_i << 2) + low_ip1];
       } else {
-        tau = (xnorm - *alpha1) / xnorm;
-        *alpha1 = 1.0 / (*alpha1 - xnorm);
-        knt = (ix0 + n) - 2;
-        for (b_k = ix0; b_k <= knt; b_k++) {
-          x[b_k - 1] *= *alpha1;
-        }
-
-        *alpha1 = xnorm;
+        Vq = V[((low_i + 1) << 2) + low_ip1] * rx + V[(low_i << 2) + low_ip1] *
+          (1.0 - rx);
       }
     }
-  }
 
-  return tau;
-}
-
-// Function for MATLAB Function: '<Root>/EKF_SLX'
-static void EKF_xzlarf(int16_T m, int16_T n, int16_T iv0, real_T tau, real_T C
-  [273], int16_T ic0, real_T work[13])
-{
-  int16_T lastv;
-  int16_T lastc;
-  int16_T coltop;
-  int16_T ix;
-  real_T c;
-  int16_T iac;
-  int16_T d;
-  int16_T b_ia;
-  int16_T jy;
-  int32_T exitg1;
-  boolean_T exitg2;
-  if (tau != 0.0) {
-    lastv = m;
-    lastc = iv0 + m;
-    while ((lastv > 0) && (C[lastc - 2] == 0.0)) {
-      lastv--;
-      lastc--;
-    }
-
-    lastc = n;
-    exitg2 = false;
-    while ((!exitg2) && (lastc > 0)) {
-      coltop = (lastc - 1) * 21 + ic0;
-      jy = coltop;
-      do {
-        exitg1 = 0L;
-        if (jy <= (coltop + lastv) - 1) {
-          if (C[jy - 1] != 0.0) {
-            exitg1 = 1L;
-          } else {
-            jy++;
-          }
-        } else {
-          lastc--;
-          exitg1 = 2L;
-        }
-      } while (exitg1 == 0L);
-
-      if (exitg1 == 1L) {
-        exitg2 = true;
+    if ((Yq == b_y1) || (qx1 == Vq)) {
+      Vq = qx1;
+    } else {
+      if (!(Yq == y2)) {
+        rx = (Yq - (real_T)b_y1) / (real_T)(y2 - b_y1);
+        Vq = (1.0 - rx) * qx1 + rx * Vq;
       }
     }
   } else {
-    lastv = 0;
-    lastc = 0;
-  }
-
-  if (lastv > 0) {
-    if (lastc != 0) {
-      for (coltop = 1; coltop <= lastc; coltop++) {
-        work[coltop - 1] = 0.0;
-      }
-
-      coltop = 0;
-      jy = (lastc - 1) * 21 + ic0;
-      for (iac = ic0; iac <= jy; iac += 21) {
-        ix = iv0;
-        c = 0.0;
-        d = (iac + lastv) - 1;
-        for (b_ia = iac; b_ia <= d; b_ia++) {
-          c += C[b_ia - 1] * C[ix - 1];
-          ix++;
-        }
-
-        work[coltop] += c;
-        coltop++;
-      }
-    }
-
-    if (!(-tau == 0.0)) {
-      coltop = ic0 - 1;
-      jy = 0;
-      for (iac = 1; iac <= lastc; iac++) {
-        if (work[jy] != 0.0) {
-          c = work[jy] * -tau;
-          ix = iv0;
-          d = lastv + coltop;
-          for (b_ia = coltop; b_ia + 1 <= d; b_ia++) {
-            C[b_ia] += C[ix - 1] * c;
-            ix++;
-          }
-        }
-
-        jy++;
-        coltop += 21;
-      }
-    }
-  }
-}
-
-// Function for MATLAB Function: '<Root>/EKF_SLX'
-static real_T EKF_xnrm2_iz(int16_T n, const real_T x[273], int16_T ix0)
-{
-  real_T y;
-  real_T scale;
-  int16_T kend;
-  real_T absxk;
-  real_T t;
-  int16_T k;
-  y = 0.0;
-  if (!(n < 1)) {
-    if (n == 1) {
-      y = std::abs(x[ix0 - 1]);
-    } else {
-      scale = 2.2250738585072014E-308;
-      kend = (ix0 + n) - 1;
-      for (k = ix0; k <= kend; k++) {
-        absxk = std::abs(x[k - 1]);
-        if (absxk > scale) {
-          t = scale / absxk;
-          y = y * t * t + 1.0;
-          scale = absxk;
-        } else {
-          t = absxk / scale;
-          y += t * t;
-        }
-      }
-
-      y = scale * std::sqrt(y);
-    }
-  }
-
-  return y;
-}
-
-// Function for MATLAB Function: '<Root>/EKF_SLX'
-static real_T EKF_xnrm2(const real_T x[273], int16_T ix0)
-{
-  real_T y;
-  real_T scale;
-  real_T absxk;
-  real_T t;
-  int16_T k;
-  y = 0.0;
-  scale = 2.2250738585072014E-308;
-  for (k = ix0; k <= ix0 + 20; k++) {
-    absxk = std::abs(x[k - 1]);
-    if (absxk > scale) {
-      t = scale / absxk;
-      y = y * t * t + 1.0;
-      scale = absxk;
-    } else {
-      t = absxk / scale;
-      y += t * t;
-    }
+    Vq = extrapval;
   }
 
-  return scale * std::sqrt(y);
-}
-
-// Function for MATLAB Function: '<Root>/EKF_SLX'
-static void EKF_xgeqp3(real_T A[273], real_T tau[13], int16_T jpvt[13])
-{
-  real_T work[13];
-  real_T vn1[13];
-  real_T vn2[13];
-  int16_T k;
-  int16_T pvt;
-  int16_T itemp;
-  real_T temp2;
-  real_T b_atmp;
-  int16_T i_i;
-  k = 1;
-  for (i_i = 0; i_i < 13; i_i++) {
-    jpvt[i_i] = 1 + i_i;
-    work[i_i] = 0.0;
-    b_atmp = EKF_xnrm2(A, k);
-    vn2[i_i] = b_atmp;
-    k += 21;
-    vn1[i_i] = b_atmp;
-  }
-
-  for (k = 0; k < 13; k++) {
-    i_i = k * 21 + k;
-    pvt = (EKF_ixamax(13 - k, vn1, k + 1) + k) - 1;
-    if (pvt + 1 != k + 1) {
-      EKF_xswap(A, 1 + 21 * pvt, 1 + 21 * k);
-      itemp = jpvt[pvt];
-      jpvt[pvt] = jpvt[k];
-      jpvt[k] = itemp;
-      vn1[pvt] = vn1[k];
-      vn2[pvt] = vn2[k];
-    }
-
-    b_atmp = A[i_i];
-    temp2 = EKF_xzlarfg(21 - k, &b_atmp, A, i_i + 2);
-    tau[k] = temp2;
-    A[i_i] = b_atmp;
-    if (k + 1 < 13) {
-      b_atmp = A[i_i];
-      A[i_i] = 1.0;
-      EKF_xzlarf(21 - k, 12 - k, i_i + 1, tau[k], A, (k + (k + 1) * 21) + 1,
-                 work);
-      A[i_i] = b_atmp;
-    }
-
-    for (i_i = k + 1; i_i + 1 < 14; i_i++) {
-      if (vn1[i_i] != 0.0) {
-        b_atmp = std::abs(A[21 * i_i + k]) / vn1[i_i];
-        b_atmp = 1.0 - b_atmp * b_atmp;
-        if (b_atmp < 0.0) {
-          b_atmp = 0.0;
-        }
-
-        temp2 = vn1[i_i] / vn2[i_i];
-        temp2 = temp2 * temp2 * b_atmp;
-        if (temp2 <= 1.4901161193847656E-8) {
-          vn1[i_i] = EKF_xnrm2_iz(20 - k, A, (21 * i_i + k) + 2);
-          vn2[i_i] = vn1[i_i];
-        } else {
-          vn1[i_i] *= std::sqrt(b_atmp);
-        }
-      }
-    }
-  }
-}
-
-// Function for MATLAB Function: '<Root>/EKF_SLX'
-static void EKF_qrsolve(const real_T A[273], const real_T B[21], real_T Y[13],
-  int16_T *rankR)
-{
-  real_T b_A[273];
-  real_T tau[13];
-  int16_T jpvt[13];
-  real_T tol;
-  real_T b_B[21];
-  int16_T b_i;
-  int16_T b_j;
-  memcpy(&b_A[0], &A[0], 273U * sizeof(real_T));
-  EKF_xgeqp3(b_A, tau, jpvt);
-  *rankR = 0;
-  tol = 21.0 * std::abs(b_A[0]) * 2.2204460492503131E-16;
-  while ((*rankR < 13) && (std::abs(b_A[21 * *rankR + *rankR]) >= tol)) {
-    (*rankR)++;
-  }
-
-  memset(&Y[0], 0, 13U * sizeof(real_T));
-  memcpy(&b_B[0], &B[0], 21U * sizeof(real_T));
-  for (b_j = 0; b_j < 13; b_j++) {
-    if (tau[b_j] != 0.0) {
-      tol = b_B[b_j];
-      for (b_i = b_j + 1; b_i + 1 < 22; b_i++) {
-        tol += b_A[21 * b_j + b_i] * b_B[b_i];
-      }
-
-      tol *= tau[b_j];
-      if (tol != 0.0) {
-        b_B[b_j] -= tol;
-        for (b_i = b_j + 1; b_i + 1 < 22; b_i++) {
-          b_B[b_i] -= b_A[21 * b_j + b_i] * tol;
-        }
-      }
-    }
-  }
-
-  for (b_j = 0; b_j < 13; b_j++) {
-    Y[jpvt[b_j] - 1] = b_B[b_j];
-  }
-
-  for (b_j = 12; b_j >= 0; b_j += -1) {
-    Y[jpvt[b_j] - 1] /= b_A[21 * b_j + b_j];
-    for (b_i = 0; b_i + 1 <= b_j; b_i++) {
-      Y[jpvt[b_i] - 1] -= b_A[21 * b_j + b_i] * Y[jpvt[b_j] - 1];
-    }
-  }
-}
-
-// Function for MATLAB Function: '<Root>/EKF_SLX'
-static void EKF_polyfit(const real_T x[21], const real_T y[21], real_T p[13])
-{
-  real_T V[273];
-  real_T p1[13];
-  int16_T k;
-  int16_T c_k;
-  for (k = 0; k < 21; k++) {
-    V[252 + k] = 1.0;
-    V[231 + k] = x[k];
-  }
-
-  for (k = 0; k < 11; k++) {
-    for (c_k = 0; c_k < 21; c_k++) {
-      V[c_k + 21 * (10 - k)] = V[(11 - k) * 21 + c_k] * x[c_k];
-    }
-  }
-
-  EKF_qrsolve(V, y, p1, &k);
-  memcpy(&p[0], &p1[0], 13U * sizeof(real_T));
+  return Vq;
 }
 
 // Model step function
 void EKF_step(void)
 {
-  real_T CoeffOCV[13];
+  real_T Ahat[9];
   real_T Bhat[3];
-  real_T Cth;
-  real_T KGainTh;
   real_T C[3];
-  real_T dOCV[21];
-  real_T b_y1[20];
-  int16_T ixLead;
-  int16_T iyLead;
-  real_T tmp2;
+  int32_T b_y1;
+  int32_T y2;
+  int16_T low_i;
+  int16_T low_ip1;
   int16_T high_i;
-  int16_T mid_i;
-  static const real_T d[21] = { 3.2141, 3.464, 3.4954, 3.5308, 3.5605, 3.5831,
-    3.6123, 3.6364, 3.6515, 3.6681, 3.6881, 3.7133, 3.7441, 3.7868, 3.8502,
-    3.8981, 3.9478, 4.0008, 4.0574, 4.118, 4.184 };
+  real_T qx1;
+  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 OCV_data[21] = { 3.2141, 3.464, 3.4954, 3.5308, 3.5605,
-    3.5831, 3.6123, 3.6364, 3.6515, 3.6681, 3.6881, 3.7133, 3.7441, 3.7868,
-    3.8502, 3.8981, 3.9478, 4.0008, 4.0574, 4.118, 4.184 };
+  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[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 SOC_spacing[21] = { 0.0, 0.05, 0.1, 0.15000000000000002,
-    0.2, 0.25, 0.30000000000000004, 0.35000000000000003, 0.4, 0.45, 0.5, 0.55,
-    0.6, 0.64999999999999991, 0.7, 0.75, 0.8, 0.85, 0.9, 0.95, 1.0 };
-
-  static const real_T e[21] = { 0.0, 0.05, 0.1, 0.15000000000000002, 0.2, 0.25,
-    0.30000000000000004, 0.35000000000000003, 0.4, 0.45, 0.5, 0.55, 0.6,
-    0.64999999999999991, 0.7, 0.75, 0.8, 0.85, 0.9, 0.95, 1.0 };
+  static const real_T b_V[68] = { 0.003318750942826959, 0.0023001508295625922,
+    0.001810091258767631, 0.0014328808446455602, 0.0030545289991703731,
+    0.0021115342558727062, 0.0016216012369423278, 0.0012823414045409901,
+    0.0027529509371346668, 0.002036583066188943, 0.001545886433903926,
+    0.0012446724248481845, 0.0027524319432923596, 0.0017720469026882386,
+    0.0013951734539969804, 0.0011314777098891079, 0.0029413982954973922,
+    0.0018856539447880626, 0.0013197089099204458, 0.0010937617862261414,
+    0.0027527433161129741, 0.0020366598778004175, 0.001659312893615418,
+    0.0013955418096782679, 0.0025641025641025663, 0.0018852273584194189,
+    0.0015085800490288528, 0.0012819546037252021, 0.0023759239704329357,
+    0.0017728490060729568, 0.0014328268164850424, 0.0012065910033558322,
+    0.0022251555723175622, 0.0016591877521776855, 0.0012822446824558853,
+    0.0010559263868461751, 0.0021121713876211686, 0.0015462945502545701,
+    0.0012444846702115593, 0.0010182531301855535, 0.0019607843137254919,
+    0.0014709210228558553, 0.0011689731890342826, 0.00098024430704267071,
+    0.0022244844097575752, 0.0015838298514216688, 0.0012444846702115593,
+    0.0010559662090813104, 0.0027153416804947981, 0.0021879361726206297,
+    0.0018478711769808023, 0.0015460613145292029, 0.0025639092074504211,
+    0.001998717803673113, 0.0016216012369423445, 0.0013197089099204289,
+    0.002413455011690175, 0.001810023002375657, 0.0014330969980389297,
+    0.0011689291101055859, 0.0021869461935824378, 0.001659250320537012,
+    0.0012820029410655633, 0.0010935555639352886, 0.0020366598778004175,
+    0.0014707546102500505, 0.0012066820015837712, 0.0010181379388362777 };
 
-  static const real_T a[9] = { 1.0, 0.0, 0.0, 0.0, 0.998555004171887, 0.0, 0.0,
-    0.0, 1.0 };
+  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 };
 
-  real_T tmp[3];
-  real_T a_0[3];
-  real_T a_1[9];
-  real_T a_2[9];
+  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 };
+
+  real_T tmp[4];
+  real_T tmp_0[4];
+  real_T tmp_1[3];
+  real_T Ahat_0[3];
+  real_T Ahat_1[9];
+  real_T Ahat_2[9];
   real_T Bhat_0[9];
-  real_T b_y1_0[21];
-  real_T b_y1_1[21];
-  real_T Bhat_1;
 
   // MATLAB Function: '<Root>/EKF_SLX' incorporates:
   //   Inport: '<Root>/Current'
   //   Inport: '<Root>/MacroTime'
+  //   Inport: '<Root>/TemperatureIn'
+  //   Inport: '<Root>/TimeStep'
   //   Inport: '<Root>/Voltage'
 
   // DECIDE WHAT CAPACITY TO USE IN BATTERY SIMULATION AND WHAT IN ESTIMATION
@@ -561,276 +266,343 @@
   // '<S1>:1:9' R=1e-2;
   // SENSOR NOISE (VOLTAGE) THIS NEEDS TO BE FAIRLY LARGE TO ALLOW TO CONVERGE
   // '<S1>:1:12' InitialSOC=0.8;
-  // '<S1>:1:14' DataOCV=[3.2141
-  // '<S1>:1:15'     3.4640
-  // '<S1>:1:16'     3.4954
-  // '<S1>:1:17'     3.5308
-  // '<S1>:1:18'     3.5605
-  // '<S1>:1:19'     3.5831
-  // '<S1>:1:20'     3.6123
-  // '<S1>:1:21'     3.6364
-  // '<S1>:1:22'     3.6515
-  // '<S1>:1:23'     3.6681
-  // '<S1>:1:24'     3.6881
-  // '<S1>:1:25'     3.7133
-  // '<S1>:1:26'     3.7441
-  // '<S1>:1:27'     3.7868
-  // '<S1>:1:28'     3.8502
-  // '<S1>:1:29'     3.8981
-  // '<S1>:1:30'     3.9478
-  // '<S1>:1:31'     4.0008
-  // '<S1>:1:32'     4.0574
-  // '<S1>:1:33'     4.1180
-  // '<S1>:1:34'     4.1840]';
-  // '<S1>:1:35' R1=0.00140375000;
-  // '<S1>:1:36' C1=49264;
-  // '<S1>:1:37' tau=C1*R1;
-  // '<S1>:1:38' SpacingOCV=0:0.05:1;
-  // '<S1>:1:40' CoeffOCV=polyfit(SpacingOCV,DataOCV,12);
-  EKF_polyfit(SOC_spacing, OCV_data, CoeffOCV);
-
-  // INITIALISE ESTIMATION VECTORS AND P0 MATRIX
-  // '<S1>:1:45' if isempty(Count)
+  // '<S1>:1:13' LoadedData=coder.load('ParamData.mat');
+  // '<S1>:1:15' ParamData=LoadedData.ParamData;
+  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  // INITIALISE ESTIMATION VECTORS AND P0 MATRICES
+  // '<S1>:1:21' if isempty(Count)
   if (!EKF_DW.Count_not_empty) {
-    // '<S1>:1:46' Count=0;
+    // '<S1>:1:22' Count=0;
     EKF_DW.Count = 0.0;
     EKF_DW.Count_not_empty = true;
 
-    // '<S1>:1:47' Xhat=[0.5 0.1 -5]';
-    // '<S1>:1:48' P0=[0.1 0 0;
-    // '<S1>:1:49'         0 0.05 0;
-    // '<S1>:1:50'         0 0 .1];
-    // '<S1>:1:51' Qhat=52;
-    // '<S1>:1:52' ThetaHat=Qhat;
+    // '<S1>:1:23' Xhat=[0.5 0.1 -5]';
+    // '<S1>:1:24' P0=[0.1 0 0;
+    // '<S1>:1:25'         0 0.05 0;
+    // '<S1>:1:26'         0 0 .1];
+    // '<S1>:1:27' Qhat=52;
+    // '<S1>:1:28' ThetaHat=Qhat;
     EKF_DW.ThetaHat = EKF_DW.Qhat;
 
-    // '<S1>:1:53' P0th=2;
-    // '<S1>:1:54' XhatTh=InitialSOC;
-    // '<S1>:1:55' CthPrev=0;
-    // '<S1>:1:56' Current_prev=0;
-    // '<S1>:1:57' iQ=1;
+    // '<S1>:1:29' P0th=2;
+    // '<S1>:1:30' XhatTh=InitialSOC;
+    // '<S1>:1:31' CthPrev=0;
+    // '<S1>:1:32' R0_filt_prev=0;
+    // '<S1>:1:33' Current_prev=0;
+    // '<S1>:1:34' iQ=1;
   } else {
-    // '<S1>:1:59' else
-    // '<S1>:1:60' Count=Count+1;
+    // '<S1>:1:36' else
+    // '<S1>:1:37' Count=Count+1;
     EKF_DW.Count++;
   }
 
+  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   // INITIALISE VARIABLE FOR CAPACITY ESTIMATION
-  // '<S1>:1:66' MacroTime=Time;
-  // '<S1>:1:67' UpdateMacro=60*10;
+  // '<S1>:1:43' UpdateMacro=60*10;
   // IN SECONDS
-  // '<S1>:1:69' tR=0.1;
-  // '<S1>:1:70' eTh=1e-4;
-  // INITIALISE VECTORS FOR C(THETA)
-  // '<S1>:1:73' dt=0.1;
+  // '<S1>:1:44' tR=0.1;
+  // '<S1>:1:45' eTh=1e-4;
+  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  // INTERPOLATE PARAMETRES
+  // '<S1>:1:49' C1=interp2(ParamData.C1d_x,ParamData.C1d_y,ParamData.C1_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;
+    while (high_i > low_ip1) {
+      b_mid_i = ((low_i + high_i) + 1) >> 1;
+      if (EKF_DW.Xhat[0] >= d[b_mid_i - 1]) {
+        low_i = b_mid_i - 1;
+        low_ip1 = b_mid_i + 1;
+      } else {
+        high_i = b_mid_i;
+      }
+    }
+
+    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);
+    b_y1 = (low_ip1 - 1) * 10L + 15L;
+    y2 = 10L * low_ip1 + 15L;
+    if (EKF_DW.Xhat[0] == d[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]) {
+      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]);
+      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]) {
+        rx = V[(low_i << 2) + low_ip1];
+      } else {
+        rx = V[((low_i + 1) << 2) + low_ip1] * rx + V[(low_i << 2) + low_ip1] *
+          (1.0 - rx);
+      }
+    }
+
+    if ((EKF_U.TemperatureIn == b_y1) || (qx1 == rx)) {
+      rx = qx1;
+    } else {
+      if (!(EKF_U.TemperatureIn == y2)) {
+        ry = (EKF_U.TemperatureIn - (real_T)b_y1) / (real_T)(y2 - b_y1);
+        rx = (1.0 - ry) * qx1 + ry * rx;
+      }
+    }
+  } else {
+    rx = (rtNaN);
+  }
+
+  // '<S1>:1:50' 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:51' tau=C1*R1;
+  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   // DISCRETE TIME SS MATRICES
-  // '<S1>:1:76' Ahat=[1     0        0;
-  // '<S1>:1:77'     0 exp(-dt/tau) 0;
-  // '<S1>:1:78'     0     0        1;];
-  // '<S1>:1:80' Bhat=[-dt/(Qhat*3600)
-  // '<S1>:1:81'     dt/C1
-  // '<S1>:1:82'     1];
-  Bhat[0] = -0.1 / (EKF_DW.Qhat * 3600.0);
-  Bhat[1] = 2.029879831113998E-6;
+  // '<S1>:1:55' Ahat=[1     0        0;
+  // '<S1>:1:56'     0 exp(-TimeStep/tau) 0;
+  // '<S1>:1:57'     0     0        1;];
+  Ahat[1] = 0.0;
+  Ahat[4] = std::exp(-EKF_U.TimeStep / (rx * ry));
+  Ahat[7] = 0.0;
+  Ahat[0] = 1.0;
+  Ahat[2] = 0.0;
+  Ahat[3] = 0.0;
+  Ahat[5] = 0.0;
+  Ahat[6] = 0.0;
+  Ahat[8] = 1.0;
+
+  // '<S1>:1:60' Bhat=[-TimeStep/(Qhat*3600)
+  // '<S1>:1:61'     TimeStep/C1
+  // '<S1>:1:62'     1];
+  Bhat[0] = -EKF_U.TimeStep / (EKF_DW.Qhat * 3600.0);
+  Bhat[1] = EKF_U.TimeStep / rx;
   Bhat[2] = 1.0;
 
+  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   // PREDICTION STAGE
+  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   // SOC ESTIMATION FOR CAPACITY ESTIMATION
-  // '<S1>:1:88' XhatTh=XhatTh-dt/(Qhat*3600)*Current_prev;
-  EKF_DW.XhatTh -= 0.1 / (EKF_DW.Qhat * 3600.0) * EKF_DW.Current_prev;
+  // '<S1>:1:69' XhatTh=XhatTh-TimeStep/(Qhat*3600)*Current_prev;
+  EKF_DW.XhatTh -= EKF_U.TimeStep / (EKF_DW.Qhat * 3600.0) * EKF_DW.Current_prev;
 
-  // '<S1>:1:90' if MacroTime>UpdateMacro*iQ
+  // '<S1>:1:71' if MacroTime>UpdateMacro*iQ
   if (EKF_U.MacroTime > 600.0 * EKF_DW.iQ) {
     // MACROSCALE EKF FOR CAPACITY ESTIMATION
-    // '<S1>:1:94' P0th=P0th+tR;
+    // '<S1>:1:74' P0th=P0th+tR;
     EKF_DW.P0th += 0.1;
 
-    // '<S1>:1:96' Cth=+Current_prev*dt/(ThetaHat^2)+ CthPrev;
-    Cth = EKF_DW.Current_prev * 0.1 / (EKF_DW.ThetaHat * EKF_DW.ThetaHat) +
-      EKF_DW.CthPrev;
+    // '<S1>:1:75' Cth=+Current_prev*TimeStep/(ThetaHat^2)+ CthPrev;
+    rx = EKF_DW.Current_prev * EKF_U.TimeStep / (EKF_DW.ThetaHat *
+      EKF_DW.ThetaHat) + EKF_DW.CthPrev;
 
-    // '<S1>:1:98' KGainTh=P0th*(Cth')*(Cth*P0th*Cth'+eTh)^-1;
-    KGainTh = 1.0 / (Cth * EKF_DW.P0th * Cth + 0.0001) * (EKF_DW.P0th * Cth);
+    // '<S1>:1:76' KGainTh=P0th*(Cth')*(Cth*P0th*Cth'+eTh)^-1;
+    qx1 = 1.0 / (rx * EKF_DW.P0th * rx + 0.0001) * (EKF_DW.P0th * rx);
 
-    // '<S1>:1:100' ThetaHat=ThetaHat+KGainTh*(Xhat(1)-XhatTh(1));
-    EKF_DW.ThetaHat += (EKF_DW.Xhat[0] - EKF_DW.XhatTh) * KGainTh;
+    // '<S1>:1:77' ThetaHat=ThetaHat+KGainTh*(Xhat(1)-XhatTh(1));
+    EKF_DW.ThetaHat += (EKF_DW.Xhat[0] - EKF_DW.XhatTh) * qx1;
 
-    // '<S1>:1:102' P0th=P0th-KGainTh*Cth*KGainTh';
-    EKF_DW.P0th -= KGainTh * Cth * KGainTh;
+    // '<S1>:1:78' P0th=P0th-KGainTh*Cth*KGainTh';
+    EKF_DW.P0th -= qx1 * rx * qx1;
 
-    // '<S1>:1:104' CthPrev=Cth;
-    EKF_DW.CthPrev = Cth;
+    // '<S1>:1:79' CthPrev=Cth;
+    EKF_DW.CthPrev = rx;
 
-    // '<S1>:1:105' Qhat=ThetaHat;
+    // '<S1>:1:80' Qhat=ThetaHat;
     EKF_DW.Qhat = EKF_DW.ThetaHat;
 
     // THIS HAS TO BE DONE TO AVOID OVERSHOOTING
-    // '<S1>:1:108' XhatTh=Xhat(1);
+    // '<S1>:1:82' XhatTh=Xhat(1);
     EKF_DW.XhatTh = EKF_DW.Xhat[0];
 
-    // '<S1>:1:109' iQ=iQ+1;
+    // '<S1>:1:83' iQ=iQ+1;
     EKF_DW.iQ++;
   }
 
-  // '<S1>:1:114' Xhat=Ahat*Xhat+Bhat.*[Current_prev Current_prev 0]';
-  tmp[0] = EKF_DW.Current_prev;
-  tmp[1] = EKF_DW.Current_prev;
-  tmp[2] = 0.0;
-  for (ixLead = 0; ixLead < 3; ixLead++) {
-    a_0[ixLead] = ((a[ixLead + 3] * EKF_DW.Xhat[1] + a[ixLead] * EKF_DW.Xhat[0])
-                   + a[ixLead + 6] * EKF_DW.Xhat[2]) + Bhat[ixLead] * tmp[ixLead];
+  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  // PREDICTION FOR STATES
+  // '<S1>:1:89' 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;
+  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];
   }
 
-  // '<S1>:1:115' P0=Ahat*P0*Ahat' + Bhat*Q*Bhat';
-  for (ixLead = 0; ixLead < 3; ixLead++) {
-    EKF_DW.Xhat[ixLead] = a_0[ixLead];
-    for (iyLead = 0; iyLead < 3; iyLead++) {
-      a_1[ixLead + 3 * iyLead] = 0.0;
-      a_1[ixLead + 3 * iyLead] += EKF_DW.P0[3 * iyLead] * a[ixLead];
-      a_1[ixLead + 3 * iyLead] += EKF_DW.P0[3 * iyLead + 1] * a[ixLead + 3];
-      a_1[ixLead + 3 * iyLead] += EKF_DW.P0[3 * iyLead + 2] * a[ixLead + 6];
+  // '<S1>:1:90' P0=Ahat*P0*Ahat' + Bhat*Q*Bhat';
+  for (low_i = 0; low_i < 3; low_i++) {
+    EKF_DW.Xhat[low_i] = Ahat_0[low_i];
+    for (low_ip1 = 0; low_ip1 < 3; low_ip1++) {
+      Ahat_1[low_i + 3 * low_ip1] = 0.0;
+      Ahat_1[low_i + 3 * low_ip1] += EKF_DW.P0[3 * low_ip1] * Ahat[low_i];
+      Ahat_1[low_i + 3 * low_ip1] += EKF_DW.P0[3 * low_ip1 + 1] * Ahat[low_i + 3];
+      Ahat_1[low_i + 3 * low_ip1] += EKF_DW.P0[3 * low_ip1 + 2] * Ahat[low_i + 6];
     }
 
-    for (iyLead = 0; iyLead < 3; iyLead++) {
-      a_2[ixLead + 3 * iyLead] = 0.0;
-      a_2[ixLead + 3 * iyLead] += a[3 * iyLead] * a_1[ixLead];
-      a_2[ixLead + 3 * iyLead] += a[3 * iyLead + 1] * a_1[ixLead + 3];
-      a_2[ixLead + 3 * iyLead] += a[3 * iyLead + 2] * a_1[ixLead + 6];
-      Bhat_0[ixLead + 3 * iyLead] = Bhat[ixLead] * 0.0001 * Bhat[iyLead];
+    for (low_ip1 = 0; low_ip1 < 3; low_ip1++) {
+      Ahat_2[low_i + 3 * low_ip1] = 0.0;
+      Ahat_2[low_i + 3 * low_ip1] += Ahat_1[low_i] * Ahat[low_ip1];
+      Ahat_2[low_i + 3 * low_ip1] += Ahat_1[low_i + 3] * Ahat[low_ip1 + 3];
+      Ahat_2[low_i + 3 * low_ip1] += Ahat_1[low_i + 6] * Ahat[low_ip1 + 6];
+      Bhat_0[low_i + 3 * low_ip1] = Bhat[low_i] * 0.0001 * Bhat[low_ip1];
     }
   }
 
-  for (ixLead = 0; ixLead < 3; ixLead++) {
-    EKF_DW.P0[3 * ixLead] = a_2[3 * ixLead] + Bhat_0[3 * ixLead];
-    EKF_DW.P0[1 + 3 * ixLead] = a_2[3 * ixLead + 1] + Bhat_0[3 * ixLead + 1];
-    EKF_DW.P0[2 + 3 * ixLead] = a_2[3 * ixLead + 2] + Bhat_0[3 * ixLead + 2];
-  }
-
-  // '<S1>:1:117' Current_prev=Current;
+  // '<S1>:1:92' Current_prev=Current;
   EKF_DW.Current_prev = EKF_U.Current;
 
-  // ADD NOISE TO MEASUREMENT
-  // '<S1>:1:119' Ytrue=Voltage;
+  // '<S1>:1:93' Ytrue=Voltage;
   // ESTIMATION STAGE
-  // '<S1>:1:123' Yhat=OCVfun(CoeffOCV,Xhat(1)) -Xhat(2) -exp(Xhat(3))*Current;
-  // 'OCVfun:4' OCV=polyval(p,SOC);
-  Cth = CoeffOCV[0];
-  for (ixLead = 0; ixLead < 12; ixLead++) {
-    Cth = EKF_DW.Xhat[0] * Cth + CoeffOCV[ixLead + 1];
-  }
-
-  // '<S1>:1:125' C=[dOCVatZ(SpacingOCV,DataOCV,Xhat(1)) -1 -exp(Xhat(3))*Current]; 
-  // FIND THE NUMERICAL DERIVATIVE AT A SPECIFIC POINT BY INTERPOLATING
-  // 'dOCVatZ:4' dOCV=dOCV_Num(SOC_spacing,OCV_data);
-  // THIS FUNCTION RETURNS THE NUMERICAL DERIVATIVE OF SOME DATA Y (OCV DATA)
-  // AND ITS SPACING Z
-  // 'dOCV_Num:5' OCV=OCV_data;
-  // 'dOCV_Num:6' dz=z(2)-z(1);
-  // 'dOCV_Num:7' dUdZ=diff(OCV)/dz;
-  ixLead = 1;
-  iyLead = 0;
-  KGainTh = 3.2141;
-  for (high_i = 0; high_i < 20; high_i++) {
-    tmp2 = KGainTh;
-    KGainTh = d[ixLead];
-    tmp2 = d[ixLead] - tmp2;
-    ixLead++;
-    b_y1[iyLead] = tmp2;
-    iyLead++;
-  }
-
-  // FIND THE NUMERICAL DERIVATIVE
-  // 'dOCV_Num:9' dOCV_data=([dUdZ(1) dUdZ]+ [dUdZ dUdZ(end)])/2;
-  for (ixLead = 0; ixLead < 20; ixLead++) {
-    KGainTh = b_y1[ixLead] / 0.05;
-    b_y1_0[ixLead + 1] = KGainTh;
-    b_y1_1[ixLead] = KGainTh;
-    b_y1[ixLead] = KGainTh;
-  }
-
-  b_y1_0[0] = b_y1[0];
-  b_y1_1[20] = b_y1[19];
-  for (ixLead = 0; ixLead < 21; ixLead++) {
-    dOCV[ixLead] = (b_y1_0[ixLead] + b_y1_1[ixLead]) / 2.0;
-  }
-
-  // GET THE WHOLE VECTOR OF THE DERIVATIVE
-  // 'dOCVatZ:6' dOCVz=interp1(SOC_spacing,dOCV,z);
-  KGainTh = (rtNaN);
-  if ((!rtIsNaN(EKF_DW.Xhat[0])) && (!(EKF_DW.Xhat[0] > 1.0)) && (!(EKF_DW.Xhat
-        [0] < 0.0))) {
-    ixLead = 0;
-    iyLead = 2;
-    high_i = 21;
-    while (high_i > iyLead) {
-      mid_i = ((ixLead + high_i) + 1) >> 1;
-      if (EKF_DW.Xhat[0] >= e[mid_i - 1]) {
-        ixLead = mid_i - 1;
-        iyLead = mid_i + 1;
-      } else {
-        high_i = mid_i;
-      }
-    }
-
-    KGainTh = (EKF_DW.Xhat[0] - e[ixLead]) / (e[ixLead + 1] - e[ixLead]);
-    if (KGainTh == 0.0) {
-      KGainTh = dOCV[ixLead];
-    } else if (KGainTh == 1.0) {
-      KGainTh = dOCV[ixLead + 1];
-    } else if (dOCV[ixLead + 1] == dOCV[ixLead]) {
-      KGainTh = dOCV[ixLead];
-    } else {
-      KGainTh = (1.0 - KGainTh) * dOCV[ixLead] + dOCV[ixLead + 1] * KGainTh;
-    }
-  }
-
-  C[0] = KGainTh;
+  // INTERPOLATE CORRECT VALUES
+  // '<S1>:1:98' OCVatSOC=interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,Xhat(1),TemperatureIn); 
+  // '<S1>:1:99' dOCVatSOC=interp2(ParamData.dOCVdatad_x,ParamData.dOCVdatad_y,ParamData.dOCVdata_map,Xhat(1),TemperatureIn); 
+  // '<S1>:1:102' Yhat=OCVatSOC -Xhat(2) -exp(Xhat(3))*Current;
+  // '<S1>:1:103' 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));
   C[1] = -1.0;
   C[2] = -std::exp(EKF_DW.Xhat[2]) * EKF_U.Current;
 
-  // '<S1>:1:127' KGain=P0*C'*(C*P0*C'+R)^-1;
-  tmp2 = 0.0;
-  for (ixLead = 0; ixLead < 3; ixLead++) {
-    tmp2 += (EKF_DW.P0[3 * ixLead + 2] * C[2] + (-EKF_DW.P0[3 * ixLead + 1] +
-              EKF_DW.P0[3 * ixLead] * KGainTh)) * C[ixLead];
+  // '<S1>:1:104' KGain=P0*C'*(C*P0*C'+R)^-1;
+  ry = 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;
+    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];
+    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];
+  }
+
+  rx = 1.0 / (ry + 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;
+  }
+
+  // '<S1>:1:105' Xhat=Xhat+KGain*(Ytrue-Yhat);
+  rx = EKF_U.Voltage - ((EKF_interp2_dispatch(h, EKF_DW.Xhat[0],
+    EKF_U.TemperatureIn, (rtNaN)) - EKF_DW.Xhat[1]) - std::exp(EKF_DW.Xhat[2]) *
+                        EKF_U.Current);
+
+  // '<S1>:1:106' P0=P0-KGain*(C*P0*C'+R)*KGain';
+  ry = 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];
   }
 
-  KGainTh = 1.0 / (tmp2 + 0.01);
-
-  // '<S1>:1:129' Xhat=Xhat+KGain*(Ytrue-Yhat);
-  Cth = EKF_U.Voltage - ((Cth - EKF_DW.Xhat[1]) - std::exp(EKF_DW.Xhat[2]) *
-    EKF_U.Current);
-
-  // '<S1>:1:131' P0=P0-KGain*(C*P0*C'+R)*KGain';
-  tmp2 = 0.0;
-  for (ixLead = 0; ixLead < 3; ixLead++) {
-    Bhat_1 = (EKF_DW.P0[ixLead + 6] * C[2] + (-EKF_DW.P0[ixLead + 3] +
-               EKF_DW.P0[ixLead] * C[0])) * KGainTh;
-    EKF_DW.Xhat[ixLead] += Bhat_1 * Cth;
-    tmp2 += (EKF_DW.P0[3 * ixLead + 2] * C[2] + (-EKF_DW.P0[3 * ixLead + 1] +
-              EKF_DW.P0[3 * ixLead] * C[0])) * C[ixLead];
-    Bhat[ixLead] = Bhat_1;
+  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];
   }
 
-  for (ixLead = 0; ixLead < 3; ixLead++) {
-    EKF_DW.P0[ixLead] -= (tmp2 + 0.01) * Bhat[ixLead] * Bhat[0];
-    EKF_DW.P0[ixLead + 3] -= (tmp2 + 0.01) * Bhat[ixLead] * Bhat[1];
-    EKF_DW.P0[ixLead + 6] -= (tmp2 + 0.01) * Bhat[ixLead] * Bhat[2];
+  // Outport: '<Root>/R0' incorporates:
+  //   MATLAB Function: '<Root>/EKF_SLX'
+
+  // '<S1>:1:109' alpha=0.9995;
+  // '<S1>:1:110' 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
+    (EKF_DW.Xhat[2]);
+
+  // MATLAB Function: '<Root>/EKF_SLX'
+  // '<S1>:1:111' 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);
+
+  // MAKE SURE SOC IS WITHIN LIMITS
+  // '<S1>:1:115' if Xhat(1)>1
+  if (EKF_DW.Xhat[0] > 1.0) {
+    // '<S1>:1:116' Xhat(1)=1;
+    EKF_DW.Xhat[0] = 1.0;
+  }
+
+  // '<S1>:1:119' if Xhat(1)<0
+  if (EKF_DW.Xhat[0] < 0.0) {
+    // '<S1>:1:120' Xhat(1)=0;
+    EKF_DW.Xhat[0] = 0.0;
   }
 
   // Outport: '<Root>/SOC' incorporates:
   //   MATLAB Function: '<Root>/EKF_SLX'
 
-  // '<S1>:1:133' SOC=Xhat(1);
-  // '<S1>:1:134' R0=exp(Xhat(3));
-  // '<S1>:1:135' Q0=ThetaHat;
+  // OUTPUT THE VALUES
+  // '<S1>:1:125' SOC=Xhat(1);
+  // '<S1>:1:126' R0=R0_filt;
+  // '<S1>:1:127' Q0=ThetaHat;
   EKF_Y.SOC = EKF_DW.Xhat[0];
 
-  // Outport: '<Root>/R0' incorporates:
-  //   MATLAB Function: '<Root>/EKF_SLX'
-
-  EKF_Y.R0 = std::exp(EKF_DW.Xhat[2]);
-
   // Outport: '<Root>/Q0' incorporates:
   //   MATLAB Function: '<Root>/EKF_SLX'
 
@@ -873,6 +645,7 @@
     EKF_DW.P0th = 2.0;
     EKF_DW.XhatTh = 0.8;
     EKF_DW.CthPrev = 0.0;
+    EKF_DW.R0_filt_prev = 0.0;
     EKF_DW.Current_prev = 0.0;
     EKF_DW.iQ = 1.0;
   }