Diff: source/Simulink/EKF.cpp

Revision:

6:cb71171a7108

Child:

7:e0be546a9112

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/Simulink/EKF.cpp	Wed Dec 07 21:02:25 2016 +0000
@@ -0,0 +1,1413 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: EKF.cpp
+//
+// Code generated for Simulink model 'EKF'.
+//
+// Model version                  : 1.3
+// Simulink Coder version         : 8.11 (R2016b) 25-Aug-2016
+// C/C++ source code generated on : Wed Dec  7 22:00:15 2016
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: Intel->x86-64 (Windows64)
+// Code generation objectives: Unspecified
+// Validation result: Not run
+//
+#include "EKF.h"
+#include "EKF_private.h"
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+int32_T untitledModelClass::EKF_ixamax(int32_T n, const real_T x[13], int32_T
+  ix0)
+{
+  int32_T idxmax;
+  int32_T ix;
+  real_T smax;
+  real_T s;
+  int32_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;
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+void untitledModelClass::EKF_xswap(real_T x[273], int32_T ix0, int32_T iy0)
+{
+  int32_T ix;
+  int32_T iy;
+  real_T temp;
+  int32_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>/MATLAB Function1'
+real_T untitledModelClass::EKF_xnrm2_l(int32_T n, const real_T x[273], int32_T
+  ix0)
+{
+  real_T y;
+  real_T scale;
+  int32_T kend;
+  real_T absxk;
+  real_T t;
+  int32_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;
+}
+
+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;
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+real_T untitledModelClass::EKF_xzlarfg(int32_T n, real_T *alpha1, real_T x[273],
+  int32_T ix0)
+{
+  real_T tau;
+  real_T xnorm;
+  int32_T knt;
+  int32_T b_k;
+  int32_T c_k;
+  tau = 0.0;
+  if (!(n <= 0)) {
+    xnorm = EKF_xnrm2_l(n - 1, x, ix0);
+    if (xnorm != 0.0) {
+      xnorm = rt_hypotd_snf(*alpha1, xnorm);
+      if (*alpha1 >= 0.0) {
+        xnorm = -xnorm;
+      }
+
+      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_l(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;
+      } 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;
+      }
+    }
+  }
+
+  return tau;
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+void untitledModelClass::EKF_xzlarf(int32_T m, int32_T n, int32_T iv0, real_T
+  tau, real_T C[273], int32_T ic0, real_T work[13])
+{
+  int32_T lastv;
+  int32_T lastc;
+  int32_T coltop;
+  int32_T ix;
+  real_T c;
+  int32_T iac;
+  int32_T d;
+  int32_T b_ia;
+  int32_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 = 0;
+        if (jy <= (coltop + lastv) - 1) {
+          if (C[jy - 1] != 0.0) {
+            exitg1 = 1;
+          } else {
+            jy++;
+          }
+        } else {
+          lastc--;
+          exitg1 = 2;
+        }
+      } while (exitg1 == 0);
+
+      if (exitg1 == 1) {
+        exitg2 = true;
+      }
+    }
+  } 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>/MATLAB Function1'
+real_T untitledModelClass::EKF_xnrm2_lg(int32_T n, const real_T x[273], int32_T
+  ix0)
+{
+  real_T y;
+  real_T scale;
+  int32_T kend;
+  real_T absxk;
+  real_T t;
+  int32_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>/MATLAB Function1'
+real_T untitledModelClass::EKF_xnrm2(const real_T x[273], int32_T ix0)
+{
+  real_T y;
+  real_T scale;
+  real_T absxk;
+  real_T t;
+  int32_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;
+    }
+  }
+
+  return scale * std::sqrt(y);
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+void untitledModelClass::EKF_xgeqp3(real_T A[273], real_T tau[13], int32_T jpvt
+  [13])
+{
+  real_T work[13];
+  real_T vn1[13];
+  real_T vn2[13];
+  int32_T k;
+  int32_T pvt;
+  int32_T itemp;
+  real_T temp2;
+  real_T b_atmp;
+  int32_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_lg(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>/MATLAB Function1'
+void untitledModelClass::EKF_qrsolve(const real_T A[273], const real_T B[21],
+  real_T Y[13], int32_T *rankR)
+{
+  real_T b_A[273];
+  real_T tau[13];
+  int32_T jpvt[13];
+  real_T tol;
+  real_T b_B[21];
+  int32_T b_i;
+  int32_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>/MATLAB Function1'
+void untitledModelClass::EKF_polyfit(const real_T x[21], const real_T y[21],
+  real_T p[13])
+{
+  real_T V[273];
+  real_T p1[13];
+  int32_T k;
+  int32_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));
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+real_T untitledModelClass::EKF_eml_rand_shr3cong(uint32_T state[2])
+{
+  real_T r;
+  uint32_T icng;
+  uint32_T jsr;
+  int32_T j;
+  int32_T jp1;
+  real_T x;
+  real_T y;
+  real_T s;
+  uint32_T ui;
+  static const real_T b[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427,
+    0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948,
+    0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738,
+    1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008,
+    1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122,
+    1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503,
+    1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831,
+    1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713,
+    2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658,
+    2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 };
+
+  icng = 69069U * state[0] + 1234567U;
+  jsr = state[1] << 13 ^ state[1];
+  jsr ^= jsr >> 17;
+  jsr ^= jsr << 5;
+  ui = icng + jsr;
+  j = (int32_T)(ui & 63U);
+  jp1 = j + 1;
+  r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * b[jp1];
+  if (!(std::abs(r) <= b[j])) {
+    x = (std::abs(r) - b[j]) / (b[jp1] - b[j]);
+    icng = 69069U * icng + 1234567U;
+    jsr ^= jsr << 13;
+    jsr ^= jsr >> 17;
+    jsr ^= jsr << 5;
+    y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10 + 0.5;
+    s = x + y;
+    if (s > 1.301198) {
+      if (r < 0.0) {
+        r = 0.4878992 * x - 0.4878992;
+      } else {
+        r = 0.4878992 - 0.4878992 * x;
+      }
+    } else {
+      if (!(s <= 0.9689279)) {
+        x = 0.4878992 - 0.4878992 * x;
+        if (y > 12.67706 - std::exp(-0.5 * x * x) * 12.37586) {
+          if (r < 0.0) {
+            r = -x;
+          } else {
+            r = x;
+          }
+        } else {
+          if (!(std::exp(-0.5 * b[jp1] * b[jp1]) + y * 0.01958303 / b[jp1] <=
+                std::exp(-0.5 * r * r))) {
+            do {
+              icng = 69069U * icng + 1234567U;
+              jsr ^= jsr << 13;
+              jsr ^= jsr >> 17;
+              jsr ^= jsr << 5;
+              x = std::log((real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10
+                           + 0.5) / 2.776994;
+              icng = 69069U * icng + 1234567U;
+              jsr ^= jsr << 13;
+              jsr ^= jsr >> 17;
+              jsr ^= jsr << 5;
+            } while (!(std::log((real_T)(int32_T)(icng + jsr) *
+                                2.328306436538696E-10 + 0.5) * -2.0 > x * x));
+
+            if (r < 0.0) {
+              r = x - 2.776994;
+            } else {
+              r = 2.776994 - x;
+            }
+          }
+        }
+      }
+    }
+  }
+
+  state[0] = icng;
+  state[1] = jsr;
+  return r;
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+void untitledModelClass::EKF_genrandu(uint32_T s, uint32_T *state, real_T *r)
+{
+  int32_T hi;
+  uint32_T test1;
+  uint32_T test2;
+  hi = (int32_T)(s / 127773U);
+  test1 = (s - hi * 127773U) * 16807U;
+  test2 = 2836U * hi;
+  if (test1 < test2) {
+    *state = (test1 - test2) + 2147483647U;
+  } else {
+    *state = test1 - test2;
+  }
+
+  *r = (real_T)*state * 4.6566128752457969E-10;
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+void untitledModelClass::EKF_twister_state_vector(uint32_T mt[625], uint32_T
+  seed)
+{
+  uint32_T r;
+  int32_T mti;
+  r = seed;
+  mt[0] = seed;
+  for (mti = 0; mti < 623; mti++) {
+    r = ((r >> 30U ^ r) * 1812433253U + mti) + 1U;
+    mt[mti + 1] = r;
+  }
+
+  mt[624] = 624U;
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+void untitledModelClass::EKF_genrand_uint32_vector(uint32_T mt[625], uint32_T u
+  [2])
+{
+  uint32_T mti;
+  uint32_T y;
+  int32_T j;
+  int32_T kk;
+  for (j = 0; j < 2; j++) {
+    mti = mt[624] + 1U;
+    if (mti >= 625U) {
+      for (kk = 0; kk < 227; kk++) {
+        y = (mt[1 + kk] & 2147483647U) | (mt[kk] & 2147483648U);
+        if ((int32_T)(y & 1U) == 0) {
+          y >>= 1U;
+        } else {
+          y = y >> 1U ^ 2567483615U;
+        }
+
+        mt[kk] = mt[397 + kk] ^ y;
+      }
+
+      for (kk = 0; kk < 396; kk++) {
+        y = (mt[kk + 227] & 2147483648U) | (mt[228 + kk] & 2147483647U);
+        if ((int32_T)(y & 1U) == 0) {
+          y >>= 1U;
+        } else {
+          y = y >> 1U ^ 2567483615U;
+        }
+
+        mt[kk + 227] = mt[kk] ^ y;
+      }
+
+      y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U);
+      if ((int32_T)(y & 1U) == 0) {
+        y >>= 1U;
+      } else {
+        y = y >> 1U ^ 2567483615U;
+      }
+
+      mt[623] = mt[396] ^ y;
+      mti = 1U;
+    }
+
+    y = mt[(int32_T)mti - 1];
+    mt[624] = mti;
+    y ^= y >> 11U;
+    y ^= y << 7U & 2636928640U;
+    y ^= y << 15U & 4022730752U;
+    y ^= y >> 18U;
+    u[j] = y;
+  }
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+real_T untitledModelClass::EKF_genrandu_b(uint32_T mt[625])
+{
+  real_T r;
+  uint32_T u[2];
+  int32_T k;
+  boolean_T b_isvalid;
+  int32_T exitg1;
+  boolean_T exitg2;
+
+  // ========================= COPYRIGHT NOTICE ============================
+  //  This is a uniform (0,1) pseudorandom number generator based on:
+  //
+  //  A C-program for MT19937, with initialization improved 2002/1/26.
+  //  Coded by Takuji Nishimura and Makoto Matsumoto.
+  //
+  //  Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
+  //  All rights reserved.
+  //
+  //  Redistribution and use in source and binary forms, with or without
+  //  modification, are permitted provided that the following conditions
+  //  are met:
+  //
+  //    1. Redistributions of source code must retain the above copyright
+  //       notice, this list of conditions and the following disclaimer.
+  //
+  //    2. Redistributions in binary form must reproduce the above copyright
+  //       notice, this list of conditions and the following disclaimer
+  //       in the documentation and/or other materials provided with the
+  //       distribution.
+  //
+  //    3. The names of its contributors may not be used to endorse or
+  //       promote products derived from this software without specific
+  //       prior written permission.
+  //
+  //  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  //  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  //  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+  //  A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT
+  //  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  //  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  //  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+  //  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+  //  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  //  (INCLUDING  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  //  OF THIS  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  //
+  // =============================   END   =================================
+  do {
+    exitg1 = 0;
+    EKF_genrand_uint32_vector(mt, u);
+    r = ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)(u[1] >> 6U)) *
+      1.1102230246251565E-16;
+    if (r == 0.0) {
+      if ((mt[624] >= 1U) && (mt[624] < 625U)) {
+        b_isvalid = true;
+      } else {
+        b_isvalid = false;
+      }
+
+      if (b_isvalid) {
+        b_isvalid = false;
+        k = 1;
+        exitg2 = false;
+        while ((!exitg2) && (k < 625)) {
+          if (mt[k - 1] == 0U) {
+            k++;
+          } else {
+            b_isvalid = true;
+            exitg2 = true;
+          }
+        }
+      }
+
+      if (!b_isvalid) {
+        EKF_twister_state_vector(mt, 5489U);
+      }
+    } else {
+      exitg1 = 1;
+    }
+  } while (exitg1 == 0);
+
+  return r;
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+real_T untitledModelClass::EKF_eml_rand_mt19937ar(uint32_T state[625])
+{
+  real_T r;
+  int32_T i;
+  real_T x;
+  uint32_T u32[2];
+  real_T b_u;
+  static const real_T b[257] = { 0.0, 0.215241895984875, 0.286174591792068,
+    0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787,
+    0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557,
+    0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991,
+    0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419,
+    0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975,
+    0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107,
+    0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997,
+    0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153,
+    0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165,
+    0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959,
+    0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086,
+    0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062,
+    0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548,
+    1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093,
+    1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034,
+    1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357,
+    1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815,
+    1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083,
+    1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944,
+    1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653,
+    1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949,
+    1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382,
+    1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073,
+    1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936,
+    1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328,
+    1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998,
+    1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547,
+    1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872,
+    1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808,
+    1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049,
+    1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761,
+    1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417,
+    1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591,
+    1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609,
+    1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487,
+    1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818,
+    1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719,
+    1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782,
+    1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172,
+    1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452,
+    1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082,
+    1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281,
+    1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127,
+    1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043,
+    1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569,
+    1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366,
+    1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053,
+    2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753,
+    2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983,
+    2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952,
+    2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431,
+    2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416,
+    2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379,
+    2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137,
+    2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729,
+    2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715,
+    2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105,
+    2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184,
+    2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674,
+    2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999,
+    2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341,
+    2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214,
+    3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143,
+    3.65415288536101, 3.91075795952492 };
+
+  static const real_T c[257] = { 1.0, 0.977101701267673, 0.959879091800108,
+    0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
+    0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
+    0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
+    0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
+    0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
+    0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
+    0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
+    0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
+    0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
+    0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
+    0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
+    0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
+    0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
+    0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
+    0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
+    0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
+    0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
+    0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
+    0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
+    0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
+    0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
+    0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
+    0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
+    0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
+    0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
+    0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
+    0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
+    0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
+    0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
+    0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
+    0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
+    0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
+    0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
+    0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
+    0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
+    0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
+    0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
+    0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
+    0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
+    0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
+    0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
+    0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
+    0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
+    0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
+    0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
+    0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
+    0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
+    0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
+    0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
+    0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
+    0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
+    0.093365311912691, 0.0911136480663738, 0.0888735920682759,
+    0.0866451944505581, 0.0844285095703535, 0.082223595813203,
+    0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
+    0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
+    0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
+    0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
+    0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
+    0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
+    0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
+    0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
+    0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
+    0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
+    0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
+    0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
+    0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
+    0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
+    0.000477467764609386 };
+
+  int32_T exitg1;
+  do {
+    exitg1 = 0;
+    EKF_genrand_uint32_vector(state, u32);
+    i = (int32_T)((u32[1] >> 24U) + 1U);
+    r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] &
+           16777215)) * 2.2204460492503131E-16 - 1.0) * b[i];
+    if (std::abs(r) <= b[i - 1]) {
+      exitg1 = 1;
+    } else if (i < 256) {
+      x = EKF_genrandu_b(state);
+      if ((c[i - 1] - c[i]) * x + c[i] < std::exp(-0.5 * r * r)) {
+        exitg1 = 1;
+      }
+    } else {
+      do {
+        x = EKF_genrandu_b(state);
+        x = std::log(x) * 0.273661237329758;
+        b_u = EKF_genrandu_b(state);
+      } while (!(-2.0 * std::log(b_u) > x * x));
+
+      if (r < 0.0) {
+        r = x - 3.65415288536101;
+      } else {
+        r = 3.65415288536101 - x;
+      }
+
+      exitg1 = 1;
+    }
+  } while (exitg1 == 0);
+
+  return r;
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+void untitledModelClass::EKF_randn(real_T r[2])
+{
+  real_T t;
+  uint32_T b_state;
+  real_T c_r;
+  uint32_T c_state;
+  if (!EKF_DW.method_not_empty) {
+    EKF_DW.method = 0U;
+    EKF_DW.method_not_empty = true;
+    EKF_DW.state[0] = 362436069U;
+    EKF_DW.state[1] = 0U;
+    if (EKF_DW.state[1] == 0U) {
+      EKF_DW.state[1] = 521288629U;
+    }
+  }
+
+  if (EKF_DW.method == 0U) {
+    switch (EKF_DW.method_b) {
+     case 4U:
+      c_state = EKF_DW.state_c;
+      do {
+        EKF_genrandu(c_state, &b_state, &c_r);
+        EKF_genrandu(b_state, &c_state, &t);
+        c_r = 2.0 * c_r - 1.0;
+        t = 2.0 * t - 1.0;
+        t = t * t + c_r * c_r;
+      } while (!(t <= 1.0));
+
+      c_r *= std::sqrt(-2.0 * std::log(t) / t);
+      EKF_DW.state_c = c_state;
+      r[0] = c_r;
+      c_state = EKF_DW.state_c;
+      do {
+        EKF_genrandu(c_state, &b_state, &c_r);
+        EKF_genrandu(b_state, &c_state, &t);
+        c_r = 2.0 * c_r - 1.0;
+        t = 2.0 * t - 1.0;
+        t = t * t + c_r * c_r;
+      } while (!(t <= 1.0));
+
+      c_r *= std::sqrt(-2.0 * std::log(t) / t);
+      EKF_DW.state_c = c_state;
+      r[1] = c_r;
+      break;
+
+     case 5U:
+      t = EKF_eml_rand_shr3cong(EKF_DW.state_h);
+      r[0] = t;
+      t = EKF_eml_rand_shr3cong(EKF_DW.state_h);
+      r[1] = t;
+      break;
+
+     default:
+      if (!EKF_DW.state_not_empty) {
+        memset(&EKF_DW.state_p[0], 0, 625U * sizeof(uint32_T));
+        EKF_twister_state_vector(EKF_DW.state_p, 5489U);
+        EKF_DW.state_not_empty = true;
+      }
+
+      t = EKF_eml_rand_mt19937ar(EKF_DW.state_p);
+      r[0] = t;
+      t = EKF_eml_rand_mt19937ar(EKF_DW.state_p);
+      r[1] = t;
+      break;
+    }
+  } else if (EKF_DW.method == 4U) {
+    c_state = EKF_DW.state[0];
+    do {
+      EKF_genrandu(c_state, &b_state, &c_r);
+      EKF_genrandu(b_state, &c_state, &t);
+      c_r = 2.0 * c_r - 1.0;
+      t = 2.0 * t - 1.0;
+      t = t * t + c_r * c_r;
+    } while (!(t <= 1.0));
+
+    c_r *= std::sqrt(-2.0 * std::log(t) / t);
+    EKF_DW.state[0] = c_state;
+    r[0] = c_r;
+    c_state = EKF_DW.state[0];
+    do {
+      EKF_genrandu(c_state, &b_state, &c_r);
+      EKF_genrandu(b_state, &c_state, &t);
+      c_r = 2.0 * c_r - 1.0;
+      t = 2.0 * t - 1.0;
+      t = t * t + c_r * c_r;
+    } while (!(t <= 1.0));
+
+    c_r *= std::sqrt(-2.0 * std::log(t) / t);
+    EKF_DW.state[0] = c_state;
+    r[1] = c_r;
+  } else {
+    t = EKF_eml_rand_shr3cong(EKF_DW.state);
+    r[0] = t;
+    t = EKF_eml_rand_shr3cong(EKF_DW.state);
+    r[1] = t;
+  }
+}
+
+// Function for MATLAB Function: '<Root>/MATLAB Function1'
+real_T untitledModelClass::EKF_randn_c(void)
+{
+  real_T r;
+  real_T t;
+  uint32_T b_state;
+  real_T c_r;
+  uint32_T c_state;
+  if (!EKF_DW.method_not_empty) {
+    EKF_DW.method = 0U;
+    EKF_DW.method_not_empty = true;
+    EKF_DW.state[0] = 362436069U;
+    EKF_DW.state[1] = 0U;
+    if (EKF_DW.state[1] == 0U) {
+      EKF_DW.state[1] = 521288629U;
+    }
+  }
+
+  if (EKF_DW.method == 0U) {
+    switch (EKF_DW.method_b) {
+     case 4U:
+      c_state = EKF_DW.state_c;
+      do {
+        EKF_genrandu(c_state, &b_state, &c_r);
+        EKF_genrandu(b_state, &c_state, &t);
+        c_r = 2.0 * c_r - 1.0;
+        t = 2.0 * t - 1.0;
+        t = t * t + c_r * c_r;
+      } while (!(t <= 1.0));
+
+      r = std::sqrt(-2.0 * std::log(t) / t) * c_r;
+      EKF_DW.state_c = c_state;
+      break;
+
+     case 5U:
+      r = EKF_eml_rand_shr3cong(EKF_DW.state_h);
+      break;
+
+     default:
+      if (!EKF_DW.state_not_empty) {
+        memset(&EKF_DW.state_p[0], 0, 625U * sizeof(uint32_T));
+        EKF_twister_state_vector(EKF_DW.state_p, 5489U);
+        EKF_DW.state_not_empty = true;
+      }
+
+      r = EKF_eml_rand_mt19937ar(EKF_DW.state_p);
+      break;
+    }
+  } else if (EKF_DW.method == 4U) {
+    c_state = EKF_DW.state[0];
+    do {
+      EKF_genrandu(c_state, &b_state, &c_r);
+      EKF_genrandu(b_state, &c_state, &t);
+      c_r = 2.0 * c_r - 1.0;
+      t = 2.0 * t - 1.0;
+      t = t * t + c_r * c_r;
+    } while (!(t <= 1.0));
+
+    r = std::sqrt(-2.0 * std::log(t) / t) * c_r;
+    EKF_DW.state[0] = c_state;
+  } else {
+    r = EKF_eml_rand_shr3cong(EKF_DW.state);
+  }
+
+  return r;
+}
+
+// Model step function
+void untitledModelClass::step()
+{
+  real_T p[13];
+  real_T Ve;
+  real_T dOCV[21];
+  real_T work;
+  real_T b_y1[20];
+  int32_T ixLead;
+  int32_T iyLead;
+  int32_T high_i;
+  int32_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 };
+
+  static const real_T b_y[4] = { 2.7469025926365436E-24, -1.0638783181939197E-23,
+    -1.0638783181939197E-23, 4.1204121287633946E-23 };
+
+  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 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 a[4] = { 1.0, 0.0, 0.0, 0.9999985539602041 };
+
+  real_T a_0[2];
+  real_T a_1[4];
+  real_T b_y1_0[21];
+  real_T b_y1_1[21];
+  real_T C_idx_0;
+  real_T L_idx_0;
+  real_T unusedExpr[2];
+
+  // MATLAB Function: '<Root>/MATLAB Function1' incorporates:
+  //   Inport: '<Root>/Current'
+  //   Inport: '<Root>/Voltage'
+
+  // MATLAB Function 'MATLAB Function1': '<S1>:1'
+  // '<S1>:1:3' coder.extrinsic('display')
+  // '<S1>:1:5' dt=1e-4;
+  // '<S1>:1:7' I_new=Current;
+  // '<S1>:1:8' V_true=Voltage;
+  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  // DATA
+  // '<S1>:1:12' Q_total_t=[190800];
+  // Ampere*seconds
+  // '<S1>:1:13' R_0_t=0.0011622500;
+  // Ohm
+  // '<S1>:1:14' R1_t=0.00140375000;
+  // Ohm
+  // '<S1>:1:15' C1_t=49264;
+  // '<S1>:1:16' OCV_data=[3.2141
+  // '<S1>:1:17' 3.4640
+  // '<S1>:1:18' 3.4954
+  // '<S1>:1:19' 3.5308
+  // '<S1>:1:20' 3.5605
+  // '<S1>:1:21' 3.5831
+  // '<S1>:1:22' 3.6123
+  // '<S1>:1:23' 3.6364
+  // '<S1>:1:24' 3.6515
+  // '<S1>:1:25' 3.6681
+  // '<S1>:1:26' 3.6881
+  // '<S1>:1:27' 3.7133
+  // '<S1>:1:28' 3.7441
+  // '<S1>:1:29' 3.7868
+  // '<S1>:1:30' 3.8502
+  // '<S1>:1:31' 3.8981
+  // '<S1>:1:32' 3.9478
+  // '<S1>:1:33' 4.0008
+  // '<S1>:1:34' 4.0574
+  // '<S1>:1:35' 4.1180
+  // '<S1>:1:36' 4.1840]';
+  // '<S1>:1:37' z=0:0.05:1;
+  // '<S1>:1:38' Order=12;
+  // EKF ALLOWS FOR A NON LINEAR OCV FUNCTION
+  // '<S1>:1:39' [p]=polyfit(z,OCV_data,Order);
+  EKF_polyfit(SOC_spacing, OCV_data, p);
+
+  // '<S1>:1:40' Q_total=Q_total_t;
+  // '<S1>:1:41' R1=R1_t;
+  // '<S1>:1:42' C1=C1_t;
+  // '<S1>:1:43' R_0=R_0_t;
+  // '<S1>:1:44' t_RC=R1*C1;
+  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
+  // INITIALISE SS VECTORS AND COV. MATRICES
+  // '<S1>:1:52' if isempty(I_prev)
+  // '<S1>:1:57' if isempty(xhat)
+  // '<S1>:1:62' if isempty(P0)
+  // '<S1>:1:68' sigmaW=1e-5;
+  // COV. OF PROCESS NOISE (CURRENT) [Q]
+  // '<S1>:1:70' SigmaV=1e-5;
+  // COV. OF SENSOR NOISE (VOLTAGE) [R]
+  // EXTENDED KALMA FILTER MATRICES
+  // '<S1>:1:75' Ahat=[1 0
+  // '<S1>:1:76' 0 exp(-dt/t_RC)];
+  // '<S1>:1:78' Bhat_I=[-dt/Q_total
+  // '<S1>:1:79' dt/C1];
+  // '<S1>:1:81' Bhat_W=[-dt/Q_total
+  // '<S1>:1:82' dt/C1];
+  // '<S1>:1:85' xhat=Ahat*xhat+Bhat_I*I_prev;
+  a_0[0] = (0.0 * EKF_DW.xhat[1] + EKF_DW.xhat[0]) + -5.2410901467505246E-10 *
+    EKF_DW.I_prev;
+  a_0[1] = (0.0 * EKF_DW.xhat[0] + 0.9999985539602041 * EKF_DW.xhat[1]) +
+    2.0298798311139983E-9 * EKF_DW.I_prev;
+
+  // STATE PREDICTION TIME UPDATE
+  // '<S1>:1:87' P0= Ahat*P0*Ahat'+ Bhat_W*sigmaW*Bhat_W';
+  for (ixLead = 0; ixLead < 2; ixLead++) {
+    EKF_DW.xhat[ixLead] = a_0[ixLead];
+    a_1[ixLead] = 0.0;
+    a_1[ixLead] += a[ixLead] * EKF_DW.P0[0];
+    a_1[ixLead] += a[ixLead + 2] * EKF_DW.P0[1];
+    a_1[ixLead + 2] = 0.0;
+    a_1[ixLead + 2] += a[ixLead] * EKF_DW.P0[2];
+    a_1[ixLead + 2] += a[ixLead + 2] * EKF_DW.P0[3];
+  }
+
+  for (ixLead = 0; ixLead < 2; ixLead++) {
+    EKF_DW.P0[ixLead << 1] = (a[(ixLead << 1) + 1] * a_1[2] + a[ixLead << 1] *
+      a_1[0]) + b_y[ixLead << 1];
+    EKF_DW.P0[1 + (ixLead << 1)] = (a[(ixLead << 1) + 1] * a_1[3] + a[ixLead <<
+      1] * a_1[1]) + b_y[(ixLead << 1) + 1];
+  }
+
+  // ERROR COV. TIME UPDATE
+  // '<S1>:1:89' W=chol(sigmaW,'lower').*randn(2,1);
+  EKF_randn(unusedExpr);
+
+  // ADD PROCESS NOISE
+  // '<S1>:1:90' Ve=chol(SigmaV,'lower').*randn(1);
+  Ve = 0.0031622776601683794 * EKF_randn_c();
+
+  // ADD MEAS. NOISE
+  // '<S1>:1:92' ytrue=V_true+Ve;
+  // GET TRUE TERMINAL VOLTAGE READING
+  // '<S1>:1:94' yhat=OCV(p,xhat(1))-xhat(2)-R_0*I_new;
+  // '<S1>:1:120' OCV=polyval(p,SOC);
+  L_idx_0 = p[0];
+  for (ixLead = 0; ixLead < 12; ixLead++) {
+    L_idx_0 = EKF_DW.xhat[0] * L_idx_0 + p[ixLead + 1];
+  }
+
+  // ESTIMATE SYSTEM OUTPUT USING NON LINEAR OCV(Z)
+  // '<S1>:1:96' C=[dOCVatZ(z,OCV_data,xhat(1)) -1];
+  // FIND THE NUMERICAL DERIVATIVE AT A SPECIFIC POINT BY INTERPOLATING
+  // '<S1>:1:113' 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;
+  work = 3.2141;
+  for (high_i = 0; high_i < 20; high_i++) {
+    C_idx_0 = work;
+    work = d[ixLead];
+    C_idx_0 = d[ixLead] - C_idx_0;
+    ixLead++;
+    b_y1[iyLead] = C_idx_0;
+    iyLead++;
+  }
+
+  // FIND THE NUMERICAL DERIVATIVE
+  // 'dOCV_Num:9' dOCV_data=([dUdZ(1) dUdZ]+ [dUdZ dUdZ(end)])/2;
+  for (ixLead = 0; ixLead < 20; ixLead++) {
+    work = b_y1[ixLead] / 0.05;
+    b_y1_0[ixLead + 1] = work;
+    b_y1_1[ixLead] = work;
+    b_y1[ixLead] = work;
+  }
+
+  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
+  // '<S1>:1:115' dOCVz=interp1(SOC_spacing,dOCV,z);
+  work = (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;
+      }
+    }
+
+    work = (EKF_DW.xhat[0] - e[ixLead]) / (e[ixLead + 1] - e[ixLead]);
+    if (work == 0.0) {
+      work = dOCV[ixLead];
+    } else if (work == 1.0) {
+      work = dOCV[ixLead + 1];
+    } else if (dOCV[ixLead + 1] == dOCV[ixLead]) {
+      work = dOCV[ixLead];
+    } else {
+      work = (1.0 - work) * dOCV[ixLead] + dOCV[ixLead + 1] * work;
+    }
+  }
+
+  C_idx_0 = work;
+
+  // FIND C MATRIX FOR GAIN CALCULATION
+  // '<S1>:1:97' L=P0*C'*(C*P0*C'+SigmaV)^-1;
+  work = 1.0 / (((work * EKF_DW.P0[0] + -EKF_DW.P0[1]) * work + -(work *
+    EKF_DW.P0[2] + -EKF_DW.P0[3])) + 1.0E-5);
+
+  // KALMAN FILTER GAIN CALCULATION
+  // STATE ESTIMATE MEASURMENT UPDATE
+  // '<S1>:1:100' xhat=xhat+L*(ytrue - yhat);
+  Ve = (EKF_U.Voltage + Ve) - ((L_idx_0 - EKF_DW.xhat[1]) - 0.00116225 *
+    EKF_U.Current);
+
+  // '<S1>:1:102' P0=P0-L*(C*P0*C'+SigmaV)*L';
+  L_idx_0 = (EKF_DW.P0[0] * C_idx_0 + -EKF_DW.P0[2]) * work;
+  EKF_DW.xhat[0] += L_idx_0 * Ve;
+  work *= EKF_DW.P0[1] * C_idx_0 + -EKF_DW.P0[3];
+  EKF_DW.xhat[1] += work * Ve;
+  Ve = (C_idx_0 * EKF_DW.P0[0] + -EKF_DW.P0[1]) * C_idx_0 + -(C_idx_0 *
+    EKF_DW.P0[2] + -EKF_DW.P0[3]);
+  EKF_DW.P0[0] -= (Ve + 1.0E-5) * L_idx_0 * L_idx_0;
+  EKF_DW.P0[2] -= (Ve + 1.0E-5) * L_idx_0 * work;
+  EKF_DW.P0[1] -= (Ve + 1.0E-5) * work * L_idx_0;
+  EKF_DW.P0[3] -= (Ve + 1.0E-5) * work * work;
+
+  // '<S1>:1:105' I_prev=I_new;
+  EKF_DW.I_prev = EKF_U.Current;
+
+  // Outport: '<Root>/SOC' incorporates:
+  //   MATLAB Function: '<Root>/MATLAB Function1'
+
+  // '<S1>:1:108' SOC=xhat(1);
+  EKF_Y.SOC = EKF_DW.xhat[0];
+}
+
+// Model initialize function
+void untitledModelClass::initialize()
+{
+  // Registration code
+
+  // initialize non-finites
+  rt_InitInfAndNaN(sizeof(real_T));
+
+  // initialize error status
+  rtmSetErrorStatus((&EKF_M), (NULL));
+
+  // states (dwork)
+  (void) memset((void *)&EKF_DW, 0,
+                sizeof(DW_EKF_T));
+
+  // external inputs
+  (void)memset((void *)&EKF_U, 0, sizeof(ExtU_EKF_T));
+
+  // external outputs
+  EKF_Y.SOC = 0.0;
+
+  // SystemInitialize for MATLAB Function: '<Root>/MATLAB Function1'
+  EKF_DW.xhat[0] = 0.5;
+  EKF_DW.xhat[1] = 0.1;
+  EKF_DW.P0[0] = 0.1;
+  EKF_DW.P0[1] = 0.0;
+  EKF_DW.P0[2] = 0.0;
+  EKF_DW.P0[3] = 0.05;
+  EKF_DW.method_not_empty = false;
+  EKF_DW.state_not_empty = false;
+  EKF_DW.method_b = 7U;
+  EKF_DW.state_c = 1144108930U;
+  EKF_DW.state_h[0] = 362436069U;
+  EKF_DW.state_h[1] = 521288629U;
+
+  // '<S1>:1:53' I_prev=0;
+  EKF_DW.I_prev = 0.0;
+
+  // '<S1>:1:58' xhat=[0.5
+  // '<S1>:1:59' 0.1];
+  // '<S1>:1:63' P0=[.1 0
+  // '<S1>:1:64' 0 0.05 ];
+}
+
+// Model terminate function
+void untitledModelClass::terminate()
+{
+  // (no terminate code required)
+}
+
+// Constructor
+untitledModelClass::untitledModelClass()
+{
+}
+
+// Destructor
+untitledModelClass::~untitledModelClass()
+{
+  // Currently there is no destructor body generated.
+}
+
+// Real-Time Model get method
+RT_MODEL_EKF_T * untitledModelClass::getRTM()
+{
+  return (&EKF_M);
+}
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//