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

Committer:
fmferrari
Date:
Fri Dec 09 17:23:26 2016 +0000
Revision:
9:17c258c67c33
Parent:
8:339dd9885af3
Child:
11:e39c490420d2
A (what seems) working version. It prints SOC, V and Current

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fmferrari 6:cb71171a7108 1 //
fmferrari 6:cb71171a7108 2 // Academic License - for use in teaching, academic research, and meeting
fmferrari 6:cb71171a7108 3 // course requirements at degree granting institutions only. Not for
fmferrari 6:cb71171a7108 4 // government, commercial, or other organizational use.
fmferrari 6:cb71171a7108 5 //
fmferrari 6:cb71171a7108 6 // File: EKF.cpp
fmferrari 6:cb71171a7108 7 //
fmferrari 6:cb71171a7108 8 // Code generated for Simulink model 'EKF'.
fmferrari 6:cb71171a7108 9 //
fmferrari 9:17c258c67c33 10 // Model version : 1.12
fmferrari 6:cb71171a7108 11 // Simulink Coder version : 8.11 (R2016b) 25-Aug-2016
fmferrari 9:17c258c67c33 12 // C/C++ source code generated on : Fri Dec 9 18:08:17 2016
fmferrari 6:cb71171a7108 13 //
fmferrari 6:cb71171a7108 14 // Target selection: ert.tlc
fmferrari 7:e0be546a9112 15 // Embedded hardware selection: STMicroelectronics->ST10/Super10
fmferrari 6:cb71171a7108 16 // Code generation objectives: Unspecified
fmferrari 6:cb71171a7108 17 // Validation result: Not run
fmferrari 6:cb71171a7108 18 //
fmferrari 6:cb71171a7108 19 #include "EKF.h"
fmferrari 6:cb71171a7108 20 #include "EKF_private.h"
fmferrari 6:cb71171a7108 21
fmferrari 8:339dd9885af3 22 // Block states (auto storage)
fmferrari 8:339dd9885af3 23 DW_EKF_T EKF_DW;
fmferrari 8:339dd9885af3 24
fmferrari 7:e0be546a9112 25 // External inputs (root inport signals with auto storage)
fmferrari 7:e0be546a9112 26 ExtU_EKF_T EKF_U;
fmferrari 6:cb71171a7108 27
fmferrari 7:e0be546a9112 28 // External outputs (root outports fed by signals with auto storage)
fmferrari 7:e0be546a9112 29 ExtY_EKF_T EKF_Y;
fmferrari 6:cb71171a7108 30
fmferrari 7:e0be546a9112 31 // Real-time model
fmferrari 7:e0be546a9112 32 RT_MODEL_EKF_T EKF_M_;
fmferrari 7:e0be546a9112 33 RT_MODEL_EKF_T *const EKF_M = &EKF_M_;
fmferrari 6:cb71171a7108 34
fmferrari 8:339dd9885af3 35 // Forward declaration for local functions
fmferrari 8:339dd9885af3 36 static int16_T EKF_ixamax(int16_T n, const real_T x[13], int16_T ix0);
fmferrari 8:339dd9885af3 37 static void EKF_xswap(real_T x[273], int16_T ix0, int16_T iy0);
fmferrari 8:339dd9885af3 38 static real_T EKF_xnrm2_i(int16_T n, const real_T x[273], int16_T ix0);
fmferrari 8:339dd9885af3 39 static real_T EKF_xzlarfg(int16_T n, real_T *alpha1, real_T x[273], int16_T ix0);
fmferrari 8:339dd9885af3 40 static void EKF_xzlarf(int16_T m, int16_T n, int16_T iv0, real_T tau, real_T C
fmferrari 8:339dd9885af3 41 [273], int16_T ic0, real_T work[13]);
fmferrari 8:339dd9885af3 42 static real_T EKF_xnrm2_iz(int16_T n, const real_T x[273], int16_T ix0);
fmferrari 8:339dd9885af3 43 static real_T EKF_xnrm2(const real_T x[273], int16_T ix0);
fmferrari 8:339dd9885af3 44 static void EKF_xgeqp3(real_T A[273], real_T tau[13], int16_T jpvt[13]);
fmferrari 8:339dd9885af3 45 static void EKF_qrsolve(const real_T A[273], const real_T B[21], real_T Y[13],
fmferrari 8:339dd9885af3 46 int16_T *rankR);
fmferrari 8:339dd9885af3 47 static void EKF_polyfit(const real_T x[21], const real_T y[21], real_T p[13]);
fmferrari 8:339dd9885af3 48 static real_T EKF_eml_rand_shr3cong(uint32_T state[2]);
fmferrari 8:339dd9885af3 49 static void EKF_genrandu(uint32_T s, uint32_T *state, real_T *r);
fmferrari 8:339dd9885af3 50 static void EKF_twister_state_vector(uint32_T mt[625], uint32_T seed);
fmferrari 8:339dd9885af3 51 static void EKF_genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]);
fmferrari 8:339dd9885af3 52 static real_T EKF_genrandu_k(uint32_T mt[625]);
fmferrari 8:339dd9885af3 53 static real_T EKF_eml_rand_mt19937ar(uint32_T state[625]);
fmferrari 8:339dd9885af3 54 static void EKF_randn(real_T r[2]);
fmferrari 8:339dd9885af3 55 static real_T EKF_randn_g(void);
fmferrari 8:339dd9885af3 56
fmferrari 8:339dd9885af3 57 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 58 static int16_T EKF_ixamax(int16_T n, const real_T x[13], int16_T ix0)
fmferrari 8:339dd9885af3 59 {
fmferrari 8:339dd9885af3 60 int16_T idxmax;
fmferrari 8:339dd9885af3 61 int16_T ix;
fmferrari 8:339dd9885af3 62 real_T smax;
fmferrari 8:339dd9885af3 63 real_T s;
fmferrari 8:339dd9885af3 64 int16_T k;
fmferrari 8:339dd9885af3 65 if (n < 1) {
fmferrari 8:339dd9885af3 66 idxmax = 0;
fmferrari 8:339dd9885af3 67 } else {
fmferrari 8:339dd9885af3 68 idxmax = 1;
fmferrari 8:339dd9885af3 69 if (n > 1) {
fmferrari 8:339dd9885af3 70 ix = ix0 - 1;
fmferrari 8:339dd9885af3 71 smax = std::abs(x[ix0 - 1]);
fmferrari 8:339dd9885af3 72 for (k = 2; k <= n; k++) {
fmferrari 8:339dd9885af3 73 ix++;
fmferrari 8:339dd9885af3 74 s = std::abs(x[ix]);
fmferrari 8:339dd9885af3 75 if (s > smax) {
fmferrari 8:339dd9885af3 76 idxmax = k;
fmferrari 8:339dd9885af3 77 smax = s;
fmferrari 8:339dd9885af3 78 }
fmferrari 8:339dd9885af3 79 }
fmferrari 8:339dd9885af3 80 }
fmferrari 8:339dd9885af3 81 }
fmferrari 8:339dd9885af3 82
fmferrari 8:339dd9885af3 83 return idxmax;
fmferrari 8:339dd9885af3 84 }
fmferrari 8:339dd9885af3 85
fmferrari 8:339dd9885af3 86 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 87 static void EKF_xswap(real_T x[273], int16_T ix0, int16_T iy0)
fmferrari 8:339dd9885af3 88 {
fmferrari 8:339dd9885af3 89 int16_T ix;
fmferrari 8:339dd9885af3 90 int16_T iy;
fmferrari 8:339dd9885af3 91 real_T temp;
fmferrari 8:339dd9885af3 92 int16_T k;
fmferrari 8:339dd9885af3 93 ix = ix0 - 1;
fmferrari 8:339dd9885af3 94 iy = iy0 - 1;
fmferrari 8:339dd9885af3 95 for (k = 0; k < 21; k++) {
fmferrari 8:339dd9885af3 96 temp = x[ix];
fmferrari 8:339dd9885af3 97 x[ix] = x[iy];
fmferrari 8:339dd9885af3 98 x[iy] = temp;
fmferrari 8:339dd9885af3 99 ix++;
fmferrari 8:339dd9885af3 100 iy++;
fmferrari 8:339dd9885af3 101 }
fmferrari 8:339dd9885af3 102 }
fmferrari 8:339dd9885af3 103
fmferrari 8:339dd9885af3 104 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 105 static real_T EKF_xnrm2_i(int16_T n, const real_T x[273], int16_T ix0)
fmferrari 8:339dd9885af3 106 {
fmferrari 8:339dd9885af3 107 real_T y;
fmferrari 8:339dd9885af3 108 real_T scale;
fmferrari 8:339dd9885af3 109 int16_T kend;
fmferrari 8:339dd9885af3 110 real_T absxk;
fmferrari 8:339dd9885af3 111 real_T t;
fmferrari 8:339dd9885af3 112 int16_T k;
fmferrari 8:339dd9885af3 113 y = 0.0;
fmferrari 8:339dd9885af3 114 if (!(n < 1)) {
fmferrari 8:339dd9885af3 115 if (n == 1) {
fmferrari 8:339dd9885af3 116 y = std::abs(x[ix0 - 1]);
fmferrari 8:339dd9885af3 117 } else {
fmferrari 8:339dd9885af3 118 scale = 2.2250738585072014E-308;
fmferrari 8:339dd9885af3 119 kend = (ix0 + n) - 1;
fmferrari 8:339dd9885af3 120 for (k = ix0; k <= kend; k++) {
fmferrari 8:339dd9885af3 121 absxk = std::abs(x[k - 1]);
fmferrari 8:339dd9885af3 122 if (absxk > scale) {
fmferrari 8:339dd9885af3 123 t = scale / absxk;
fmferrari 8:339dd9885af3 124 y = y * t * t + 1.0;
fmferrari 8:339dd9885af3 125 scale = absxk;
fmferrari 8:339dd9885af3 126 } else {
fmferrari 8:339dd9885af3 127 t = absxk / scale;
fmferrari 8:339dd9885af3 128 y += t * t;
fmferrari 8:339dd9885af3 129 }
fmferrari 8:339dd9885af3 130 }
fmferrari 8:339dd9885af3 131
fmferrari 8:339dd9885af3 132 y = scale * std::sqrt(y);
fmferrari 8:339dd9885af3 133 }
fmferrari 8:339dd9885af3 134 }
fmferrari 8:339dd9885af3 135
fmferrari 8:339dd9885af3 136 return y;
fmferrari 8:339dd9885af3 137 }
fmferrari 8:339dd9885af3 138
fmferrari 8:339dd9885af3 139 real_T rt_hypotd_snf(real_T u0, real_T u1)
fmferrari 8:339dd9885af3 140 {
fmferrari 8:339dd9885af3 141 real_T y;
fmferrari 8:339dd9885af3 142 real_T a;
fmferrari 8:339dd9885af3 143 a = std::abs(u0);
fmferrari 8:339dd9885af3 144 y = std::abs(u1);
fmferrari 8:339dd9885af3 145 if (a < y) {
fmferrari 8:339dd9885af3 146 a /= y;
fmferrari 8:339dd9885af3 147 y *= std::sqrt(a * a + 1.0);
fmferrari 8:339dd9885af3 148 } else if (a > y) {
fmferrari 8:339dd9885af3 149 y /= a;
fmferrari 8:339dd9885af3 150 y = std::sqrt(y * y + 1.0) * a;
fmferrari 8:339dd9885af3 151 } else {
fmferrari 8:339dd9885af3 152 if (!rtIsNaN(y)) {
fmferrari 8:339dd9885af3 153 y = a * 1.4142135623730951;
fmferrari 8:339dd9885af3 154 }
fmferrari 8:339dd9885af3 155 }
fmferrari 8:339dd9885af3 156
fmferrari 8:339dd9885af3 157 return y;
fmferrari 8:339dd9885af3 158 }
fmferrari 8:339dd9885af3 159
fmferrari 8:339dd9885af3 160 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 161 static real_T EKF_xzlarfg(int16_T n, real_T *alpha1, real_T x[273], int16_T ix0)
fmferrari 8:339dd9885af3 162 {
fmferrari 8:339dd9885af3 163 real_T tau;
fmferrari 8:339dd9885af3 164 real_T xnorm;
fmferrari 8:339dd9885af3 165 int16_T knt;
fmferrari 8:339dd9885af3 166 int16_T b_k;
fmferrari 8:339dd9885af3 167 int16_T c_k;
fmferrari 8:339dd9885af3 168 tau = 0.0;
fmferrari 8:339dd9885af3 169 if (!(n <= 0)) {
fmferrari 8:339dd9885af3 170 xnorm = EKF_xnrm2_i(n - 1, x, ix0);
fmferrari 8:339dd9885af3 171 if (xnorm != 0.0) {
fmferrari 8:339dd9885af3 172 xnorm = rt_hypotd_snf(*alpha1, xnorm);
fmferrari 8:339dd9885af3 173 if (*alpha1 >= 0.0) {
fmferrari 8:339dd9885af3 174 xnorm = -xnorm;
fmferrari 8:339dd9885af3 175 }
fmferrari 8:339dd9885af3 176
fmferrari 8:339dd9885af3 177 if (std::abs(xnorm) < 1.0020841800044864E-292) {
fmferrari 8:339dd9885af3 178 knt = 0;
fmferrari 8:339dd9885af3 179 do {
fmferrari 8:339dd9885af3 180 knt++;
fmferrari 8:339dd9885af3 181 b_k = (ix0 + n) - 2;
fmferrari 8:339dd9885af3 182 for (c_k = ix0; c_k <= b_k; c_k++) {
fmferrari 8:339dd9885af3 183 x[c_k - 1] *= 9.9792015476736E+291;
fmferrari 8:339dd9885af3 184 }
fmferrari 8:339dd9885af3 185
fmferrari 8:339dd9885af3 186 xnorm *= 9.9792015476736E+291;
fmferrari 8:339dd9885af3 187 *alpha1 *= 9.9792015476736E+291;
fmferrari 8:339dd9885af3 188 } while (!(std::abs(xnorm) >= 1.0020841800044864E-292));
fmferrari 8:339dd9885af3 189
fmferrari 8:339dd9885af3 190 xnorm = rt_hypotd_snf(*alpha1, EKF_xnrm2_i(n - 1, x, ix0));
fmferrari 8:339dd9885af3 191 if (*alpha1 >= 0.0) {
fmferrari 8:339dd9885af3 192 xnorm = -xnorm;
fmferrari 8:339dd9885af3 193 }
fmferrari 8:339dd9885af3 194
fmferrari 8:339dd9885af3 195 tau = (xnorm - *alpha1) / xnorm;
fmferrari 8:339dd9885af3 196 *alpha1 = 1.0 / (*alpha1 - xnorm);
fmferrari 8:339dd9885af3 197 b_k = (ix0 + n) - 2;
fmferrari 8:339dd9885af3 198 for (c_k = ix0; c_k <= b_k; c_k++) {
fmferrari 8:339dd9885af3 199 x[c_k - 1] *= *alpha1;
fmferrari 8:339dd9885af3 200 }
fmferrari 8:339dd9885af3 201
fmferrari 8:339dd9885af3 202 for (b_k = 1; b_k <= knt; b_k++) {
fmferrari 8:339dd9885af3 203 xnorm *= 1.0020841800044864E-292;
fmferrari 8:339dd9885af3 204 }
fmferrari 8:339dd9885af3 205
fmferrari 8:339dd9885af3 206 *alpha1 = xnorm;
fmferrari 8:339dd9885af3 207 } else {
fmferrari 8:339dd9885af3 208 tau = (xnorm - *alpha1) / xnorm;
fmferrari 8:339dd9885af3 209 *alpha1 = 1.0 / (*alpha1 - xnorm);
fmferrari 8:339dd9885af3 210 knt = (ix0 + n) - 2;
fmferrari 8:339dd9885af3 211 for (b_k = ix0; b_k <= knt; b_k++) {
fmferrari 8:339dd9885af3 212 x[b_k - 1] *= *alpha1;
fmferrari 8:339dd9885af3 213 }
fmferrari 8:339dd9885af3 214
fmferrari 8:339dd9885af3 215 *alpha1 = xnorm;
fmferrari 8:339dd9885af3 216 }
fmferrari 8:339dd9885af3 217 }
fmferrari 8:339dd9885af3 218 }
fmferrari 8:339dd9885af3 219
fmferrari 8:339dd9885af3 220 return tau;
fmferrari 8:339dd9885af3 221 }
fmferrari 8:339dd9885af3 222
fmferrari 8:339dd9885af3 223 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 224 static void EKF_xzlarf(int16_T m, int16_T n, int16_T iv0, real_T tau, real_T C
fmferrari 8:339dd9885af3 225 [273], int16_T ic0, real_T work[13])
fmferrari 8:339dd9885af3 226 {
fmferrari 8:339dd9885af3 227 int16_T lastv;
fmferrari 8:339dd9885af3 228 int16_T lastc;
fmferrari 8:339dd9885af3 229 int16_T coltop;
fmferrari 8:339dd9885af3 230 int16_T ix;
fmferrari 8:339dd9885af3 231 real_T c;
fmferrari 8:339dd9885af3 232 int16_T iac;
fmferrari 8:339dd9885af3 233 int16_T d;
fmferrari 8:339dd9885af3 234 int16_T b_ia;
fmferrari 8:339dd9885af3 235 int16_T jy;
fmferrari 8:339dd9885af3 236 int32_T exitg1;
fmferrari 8:339dd9885af3 237 boolean_T exitg2;
fmferrari 8:339dd9885af3 238 if (tau != 0.0) {
fmferrari 8:339dd9885af3 239 lastv = m;
fmferrari 8:339dd9885af3 240 lastc = iv0 + m;
fmferrari 8:339dd9885af3 241 while ((lastv > 0) && (C[lastc - 2] == 0.0)) {
fmferrari 8:339dd9885af3 242 lastv--;
fmferrari 8:339dd9885af3 243 lastc--;
fmferrari 8:339dd9885af3 244 }
fmferrari 8:339dd9885af3 245
fmferrari 8:339dd9885af3 246 lastc = n;
fmferrari 8:339dd9885af3 247 exitg2 = false;
fmferrari 8:339dd9885af3 248 while ((!exitg2) && (lastc > 0)) {
fmferrari 8:339dd9885af3 249 coltop = (lastc - 1) * 21 + ic0;
fmferrari 8:339dd9885af3 250 jy = coltop;
fmferrari 8:339dd9885af3 251 do {
fmferrari 8:339dd9885af3 252 exitg1 = 0L;
fmferrari 8:339dd9885af3 253 if (jy <= (coltop + lastv) - 1) {
fmferrari 8:339dd9885af3 254 if (C[jy - 1] != 0.0) {
fmferrari 8:339dd9885af3 255 exitg1 = 1L;
fmferrari 8:339dd9885af3 256 } else {
fmferrari 8:339dd9885af3 257 jy++;
fmferrari 8:339dd9885af3 258 }
fmferrari 8:339dd9885af3 259 } else {
fmferrari 8:339dd9885af3 260 lastc--;
fmferrari 8:339dd9885af3 261 exitg1 = 2L;
fmferrari 8:339dd9885af3 262 }
fmferrari 8:339dd9885af3 263 } while (exitg1 == 0L);
fmferrari 8:339dd9885af3 264
fmferrari 8:339dd9885af3 265 if (exitg1 == 1L) {
fmferrari 8:339dd9885af3 266 exitg2 = true;
fmferrari 8:339dd9885af3 267 }
fmferrari 8:339dd9885af3 268 }
fmferrari 8:339dd9885af3 269 } else {
fmferrari 8:339dd9885af3 270 lastv = 0;
fmferrari 8:339dd9885af3 271 lastc = 0;
fmferrari 8:339dd9885af3 272 }
fmferrari 8:339dd9885af3 273
fmferrari 8:339dd9885af3 274 if (lastv > 0) {
fmferrari 8:339dd9885af3 275 if (lastc != 0) {
fmferrari 8:339dd9885af3 276 for (coltop = 1; coltop <= lastc; coltop++) {
fmferrari 8:339dd9885af3 277 work[coltop - 1] = 0.0;
fmferrari 8:339dd9885af3 278 }
fmferrari 8:339dd9885af3 279
fmferrari 8:339dd9885af3 280 coltop = 0;
fmferrari 8:339dd9885af3 281 jy = (lastc - 1) * 21 + ic0;
fmferrari 8:339dd9885af3 282 for (iac = ic0; iac <= jy; iac += 21) {
fmferrari 8:339dd9885af3 283 ix = iv0;
fmferrari 8:339dd9885af3 284 c = 0.0;
fmferrari 8:339dd9885af3 285 d = (iac + lastv) - 1;
fmferrari 8:339dd9885af3 286 for (b_ia = iac; b_ia <= d; b_ia++) {
fmferrari 8:339dd9885af3 287 c += C[b_ia - 1] * C[ix - 1];
fmferrari 8:339dd9885af3 288 ix++;
fmferrari 8:339dd9885af3 289 }
fmferrari 8:339dd9885af3 290
fmferrari 8:339dd9885af3 291 work[coltop] += c;
fmferrari 8:339dd9885af3 292 coltop++;
fmferrari 8:339dd9885af3 293 }
fmferrari 8:339dd9885af3 294 }
fmferrari 8:339dd9885af3 295
fmferrari 8:339dd9885af3 296 if (!(-tau == 0.0)) {
fmferrari 8:339dd9885af3 297 coltop = ic0 - 1;
fmferrari 8:339dd9885af3 298 jy = 0;
fmferrari 8:339dd9885af3 299 for (iac = 1; iac <= lastc; iac++) {
fmferrari 8:339dd9885af3 300 if (work[jy] != 0.0) {
fmferrari 8:339dd9885af3 301 c = work[jy] * -tau;
fmferrari 8:339dd9885af3 302 ix = iv0;
fmferrari 8:339dd9885af3 303 d = lastv + coltop;
fmferrari 8:339dd9885af3 304 for (b_ia = coltop; b_ia + 1 <= d; b_ia++) {
fmferrari 8:339dd9885af3 305 C[b_ia] += C[ix - 1] * c;
fmferrari 8:339dd9885af3 306 ix++;
fmferrari 8:339dd9885af3 307 }
fmferrari 8:339dd9885af3 308 }
fmferrari 8:339dd9885af3 309
fmferrari 8:339dd9885af3 310 jy++;
fmferrari 8:339dd9885af3 311 coltop += 21;
fmferrari 8:339dd9885af3 312 }
fmferrari 8:339dd9885af3 313 }
fmferrari 8:339dd9885af3 314 }
fmferrari 8:339dd9885af3 315 }
fmferrari 8:339dd9885af3 316
fmferrari 8:339dd9885af3 317 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 318 static real_T EKF_xnrm2_iz(int16_T n, const real_T x[273], int16_T ix0)
fmferrari 8:339dd9885af3 319 {
fmferrari 8:339dd9885af3 320 real_T y;
fmferrari 8:339dd9885af3 321 real_T scale;
fmferrari 8:339dd9885af3 322 int16_T kend;
fmferrari 8:339dd9885af3 323 real_T absxk;
fmferrari 8:339dd9885af3 324 real_T t;
fmferrari 8:339dd9885af3 325 int16_T k;
fmferrari 8:339dd9885af3 326 y = 0.0;
fmferrari 8:339dd9885af3 327 if (!(n < 1)) {
fmferrari 8:339dd9885af3 328 if (n == 1) {
fmferrari 8:339dd9885af3 329 y = std::abs(x[ix0 - 1]);
fmferrari 8:339dd9885af3 330 } else {
fmferrari 8:339dd9885af3 331 scale = 2.2250738585072014E-308;
fmferrari 8:339dd9885af3 332 kend = (ix0 + n) - 1;
fmferrari 8:339dd9885af3 333 for (k = ix0; k <= kend; k++) {
fmferrari 8:339dd9885af3 334 absxk = std::abs(x[k - 1]);
fmferrari 8:339dd9885af3 335 if (absxk > scale) {
fmferrari 8:339dd9885af3 336 t = scale / absxk;
fmferrari 8:339dd9885af3 337 y = y * t * t + 1.0;
fmferrari 8:339dd9885af3 338 scale = absxk;
fmferrari 8:339dd9885af3 339 } else {
fmferrari 8:339dd9885af3 340 t = absxk / scale;
fmferrari 8:339dd9885af3 341 y += t * t;
fmferrari 8:339dd9885af3 342 }
fmferrari 8:339dd9885af3 343 }
fmferrari 8:339dd9885af3 344
fmferrari 8:339dd9885af3 345 y = scale * std::sqrt(y);
fmferrari 8:339dd9885af3 346 }
fmferrari 8:339dd9885af3 347 }
fmferrari 8:339dd9885af3 348
fmferrari 8:339dd9885af3 349 return y;
fmferrari 8:339dd9885af3 350 }
fmferrari 8:339dd9885af3 351
fmferrari 8:339dd9885af3 352 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 353 static real_T EKF_xnrm2(const real_T x[273], int16_T ix0)
fmferrari 8:339dd9885af3 354 {
fmferrari 8:339dd9885af3 355 real_T y;
fmferrari 8:339dd9885af3 356 real_T scale;
fmferrari 8:339dd9885af3 357 real_T absxk;
fmferrari 8:339dd9885af3 358 real_T t;
fmferrari 8:339dd9885af3 359 int16_T k;
fmferrari 8:339dd9885af3 360 y = 0.0;
fmferrari 8:339dd9885af3 361 scale = 2.2250738585072014E-308;
fmferrari 8:339dd9885af3 362 for (k = ix0; k <= ix0 + 20; k++) {
fmferrari 8:339dd9885af3 363 absxk = std::abs(x[k - 1]);
fmferrari 8:339dd9885af3 364 if (absxk > scale) {
fmferrari 8:339dd9885af3 365 t = scale / absxk;
fmferrari 8:339dd9885af3 366 y = y * t * t + 1.0;
fmferrari 8:339dd9885af3 367 scale = absxk;
fmferrari 8:339dd9885af3 368 } else {
fmferrari 8:339dd9885af3 369 t = absxk / scale;
fmferrari 8:339dd9885af3 370 y += t * t;
fmferrari 8:339dd9885af3 371 }
fmferrari 8:339dd9885af3 372 }
fmferrari 8:339dd9885af3 373
fmferrari 8:339dd9885af3 374 return scale * std::sqrt(y);
fmferrari 8:339dd9885af3 375 }
fmferrari 8:339dd9885af3 376
fmferrari 8:339dd9885af3 377 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 378 static void EKF_xgeqp3(real_T A[273], real_T tau[13], int16_T jpvt[13])
fmferrari 8:339dd9885af3 379 {
fmferrari 8:339dd9885af3 380 real_T work[13];
fmferrari 8:339dd9885af3 381 real_T vn1[13];
fmferrari 8:339dd9885af3 382 real_T vn2[13];
fmferrari 8:339dd9885af3 383 int16_T k;
fmferrari 8:339dd9885af3 384 int16_T pvt;
fmferrari 8:339dd9885af3 385 int16_T itemp;
fmferrari 8:339dd9885af3 386 real_T temp2;
fmferrari 8:339dd9885af3 387 real_T b_atmp;
fmferrari 8:339dd9885af3 388 int16_T i_i;
fmferrari 8:339dd9885af3 389 k = 1;
fmferrari 8:339dd9885af3 390 for (i_i = 0; i_i < 13; i_i++) {
fmferrari 8:339dd9885af3 391 jpvt[i_i] = 1 + i_i;
fmferrari 8:339dd9885af3 392 work[i_i] = 0.0;
fmferrari 8:339dd9885af3 393 b_atmp = EKF_xnrm2(A, k);
fmferrari 8:339dd9885af3 394 vn2[i_i] = b_atmp;
fmferrari 8:339dd9885af3 395 k += 21;
fmferrari 8:339dd9885af3 396 vn1[i_i] = b_atmp;
fmferrari 8:339dd9885af3 397 }
fmferrari 8:339dd9885af3 398
fmferrari 8:339dd9885af3 399 for (k = 0; k < 13; k++) {
fmferrari 8:339dd9885af3 400 i_i = k * 21 + k;
fmferrari 8:339dd9885af3 401 pvt = (EKF_ixamax(13 - k, vn1, k + 1) + k) - 1;
fmferrari 8:339dd9885af3 402 if (pvt + 1 != k + 1) {
fmferrari 8:339dd9885af3 403 EKF_xswap(A, 1 + 21 * pvt, 1 + 21 * k);
fmferrari 8:339dd9885af3 404 itemp = jpvt[pvt];
fmferrari 8:339dd9885af3 405 jpvt[pvt] = jpvt[k];
fmferrari 8:339dd9885af3 406 jpvt[k] = itemp;
fmferrari 8:339dd9885af3 407 vn1[pvt] = vn1[k];
fmferrari 8:339dd9885af3 408 vn2[pvt] = vn2[k];
fmferrari 8:339dd9885af3 409 }
fmferrari 8:339dd9885af3 410
fmferrari 8:339dd9885af3 411 b_atmp = A[i_i];
fmferrari 8:339dd9885af3 412 temp2 = EKF_xzlarfg(21 - k, &b_atmp, A, i_i + 2);
fmferrari 8:339dd9885af3 413 tau[k] = temp2;
fmferrari 8:339dd9885af3 414 A[i_i] = b_atmp;
fmferrari 8:339dd9885af3 415 if (k + 1 < 13) {
fmferrari 8:339dd9885af3 416 b_atmp = A[i_i];
fmferrari 8:339dd9885af3 417 A[i_i] = 1.0;
fmferrari 8:339dd9885af3 418 EKF_xzlarf(21 - k, 12 - k, i_i + 1, tau[k], A, (k + (k + 1) * 21) + 1,
fmferrari 8:339dd9885af3 419 work);
fmferrari 8:339dd9885af3 420 A[i_i] = b_atmp;
fmferrari 8:339dd9885af3 421 }
fmferrari 8:339dd9885af3 422
fmferrari 8:339dd9885af3 423 for (i_i = k + 1; i_i + 1 < 14; i_i++) {
fmferrari 8:339dd9885af3 424 if (vn1[i_i] != 0.0) {
fmferrari 8:339dd9885af3 425 b_atmp = std::abs(A[21 * i_i + k]) / vn1[i_i];
fmferrari 8:339dd9885af3 426 b_atmp = 1.0 - b_atmp * b_atmp;
fmferrari 8:339dd9885af3 427 if (b_atmp < 0.0) {
fmferrari 8:339dd9885af3 428 b_atmp = 0.0;
fmferrari 8:339dd9885af3 429 }
fmferrari 8:339dd9885af3 430
fmferrari 8:339dd9885af3 431 temp2 = vn1[i_i] / vn2[i_i];
fmferrari 8:339dd9885af3 432 temp2 = temp2 * temp2 * b_atmp;
fmferrari 8:339dd9885af3 433 if (temp2 <= 1.4901161193847656E-8) {
fmferrari 8:339dd9885af3 434 vn1[i_i] = EKF_xnrm2_iz(20 - k, A, (21 * i_i + k) + 2);
fmferrari 8:339dd9885af3 435 vn2[i_i] = vn1[i_i];
fmferrari 8:339dd9885af3 436 } else {
fmferrari 8:339dd9885af3 437 vn1[i_i] *= std::sqrt(b_atmp);
fmferrari 8:339dd9885af3 438 }
fmferrari 8:339dd9885af3 439 }
fmferrari 8:339dd9885af3 440 }
fmferrari 8:339dd9885af3 441 }
fmferrari 8:339dd9885af3 442 }
fmferrari 8:339dd9885af3 443
fmferrari 8:339dd9885af3 444 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 445 static void EKF_qrsolve(const real_T A[273], const real_T B[21], real_T Y[13],
fmferrari 8:339dd9885af3 446 int16_T *rankR)
fmferrari 8:339dd9885af3 447 {
fmferrari 8:339dd9885af3 448 real_T b_A[273];
fmferrari 8:339dd9885af3 449 real_T tau[13];
fmferrari 8:339dd9885af3 450 int16_T jpvt[13];
fmferrari 8:339dd9885af3 451 real_T tol;
fmferrari 8:339dd9885af3 452 real_T b_B[21];
fmferrari 8:339dd9885af3 453 int16_T b_i;
fmferrari 8:339dd9885af3 454 int16_T b_j;
fmferrari 8:339dd9885af3 455 memcpy(&b_A[0], &A[0], 273U * sizeof(real_T));
fmferrari 8:339dd9885af3 456 EKF_xgeqp3(b_A, tau, jpvt);
fmferrari 8:339dd9885af3 457 *rankR = 0;
fmferrari 8:339dd9885af3 458 tol = 21.0 * std::abs(b_A[0]) * 2.2204460492503131E-16;
fmferrari 8:339dd9885af3 459 while ((*rankR < 13) && (std::abs(b_A[21 * *rankR + *rankR]) >= tol)) {
fmferrari 8:339dd9885af3 460 (*rankR)++;
fmferrari 8:339dd9885af3 461 }
fmferrari 8:339dd9885af3 462
fmferrari 8:339dd9885af3 463 memset(&Y[0], 0, 13U * sizeof(real_T));
fmferrari 8:339dd9885af3 464 memcpy(&b_B[0], &B[0], 21U * sizeof(real_T));
fmferrari 8:339dd9885af3 465 for (b_j = 0; b_j < 13; b_j++) {
fmferrari 8:339dd9885af3 466 if (tau[b_j] != 0.0) {
fmferrari 8:339dd9885af3 467 tol = b_B[b_j];
fmferrari 8:339dd9885af3 468 for (b_i = b_j + 1; b_i + 1 < 22; b_i++) {
fmferrari 8:339dd9885af3 469 tol += b_A[21 * b_j + b_i] * b_B[b_i];
fmferrari 8:339dd9885af3 470 }
fmferrari 8:339dd9885af3 471
fmferrari 8:339dd9885af3 472 tol *= tau[b_j];
fmferrari 8:339dd9885af3 473 if (tol != 0.0) {
fmferrari 8:339dd9885af3 474 b_B[b_j] -= tol;
fmferrari 8:339dd9885af3 475 for (b_i = b_j + 1; b_i + 1 < 22; b_i++) {
fmferrari 8:339dd9885af3 476 b_B[b_i] -= b_A[21 * b_j + b_i] * tol;
fmferrari 8:339dd9885af3 477 }
fmferrari 8:339dd9885af3 478 }
fmferrari 8:339dd9885af3 479 }
fmferrari 8:339dd9885af3 480 }
fmferrari 8:339dd9885af3 481
fmferrari 8:339dd9885af3 482 for (b_j = 0; b_j < 13; b_j++) {
fmferrari 8:339dd9885af3 483 Y[jpvt[b_j] - 1] = b_B[b_j];
fmferrari 8:339dd9885af3 484 }
fmferrari 8:339dd9885af3 485
fmferrari 8:339dd9885af3 486 for (b_j = 12; b_j >= 0; b_j += -1) {
fmferrari 8:339dd9885af3 487 Y[jpvt[b_j] - 1] /= b_A[21 * b_j + b_j];
fmferrari 8:339dd9885af3 488 for (b_i = 0; b_i + 1 <= b_j; b_i++) {
fmferrari 8:339dd9885af3 489 Y[jpvt[b_i] - 1] -= b_A[21 * b_j + b_i] * Y[jpvt[b_j] - 1];
fmferrari 8:339dd9885af3 490 }
fmferrari 8:339dd9885af3 491 }
fmferrari 8:339dd9885af3 492 }
fmferrari 8:339dd9885af3 493
fmferrari 8:339dd9885af3 494 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 495 static void EKF_polyfit(const real_T x[21], const real_T y[21], real_T p[13])
fmferrari 8:339dd9885af3 496 {
fmferrari 8:339dd9885af3 497 real_T V[273];
fmferrari 8:339dd9885af3 498 real_T p1[13];
fmferrari 8:339dd9885af3 499 int16_T k;
fmferrari 8:339dd9885af3 500 int16_T c_k;
fmferrari 8:339dd9885af3 501 for (k = 0; k < 21; k++) {
fmferrari 8:339dd9885af3 502 V[252 + k] = 1.0;
fmferrari 8:339dd9885af3 503 V[231 + k] = x[k];
fmferrari 8:339dd9885af3 504 }
fmferrari 8:339dd9885af3 505
fmferrari 8:339dd9885af3 506 for (k = 0; k < 11; k++) {
fmferrari 8:339dd9885af3 507 for (c_k = 0; c_k < 21; c_k++) {
fmferrari 8:339dd9885af3 508 V[c_k + 21 * (10 - k)] = V[(11 - k) * 21 + c_k] * x[c_k];
fmferrari 8:339dd9885af3 509 }
fmferrari 8:339dd9885af3 510 }
fmferrari 8:339dd9885af3 511
fmferrari 8:339dd9885af3 512 EKF_qrsolve(V, y, p1, &k);
fmferrari 8:339dd9885af3 513 memcpy(&p[0], &p1[0], 13U * sizeof(real_T));
fmferrari 8:339dd9885af3 514 }
fmferrari 8:339dd9885af3 515
fmferrari 8:339dd9885af3 516 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 517 static real_T EKF_eml_rand_shr3cong(uint32_T state[2])
fmferrari 8:339dd9885af3 518 {
fmferrari 8:339dd9885af3 519 real_T r;
fmferrari 8:339dd9885af3 520 uint32_T icng;
fmferrari 8:339dd9885af3 521 uint32_T jsr;
fmferrari 8:339dd9885af3 522 int16_T j;
fmferrari 8:339dd9885af3 523 int16_T jp1;
fmferrari 8:339dd9885af3 524 real_T x;
fmferrari 8:339dd9885af3 525 real_T y;
fmferrari 8:339dd9885af3 526 real_T s;
fmferrari 8:339dd9885af3 527 uint32_T ui;
fmferrari 8:339dd9885af3 528 static const real_T b[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427,
fmferrari 8:339dd9885af3 529 0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948,
fmferrari 8:339dd9885af3 530 0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738,
fmferrari 8:339dd9885af3 531 1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008,
fmferrari 8:339dd9885af3 532 1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122,
fmferrari 8:339dd9885af3 533 1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503,
fmferrari 8:339dd9885af3 534 1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831,
fmferrari 8:339dd9885af3 535 1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713,
fmferrari 8:339dd9885af3 536 2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658,
fmferrari 8:339dd9885af3 537 2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 };
fmferrari 8:339dd9885af3 538
fmferrari 8:339dd9885af3 539 icng = 69069UL * state[0] + 1234567UL;
fmferrari 8:339dd9885af3 540 jsr = state[1] << 13 ^ state[1];
fmferrari 8:339dd9885af3 541 jsr ^= jsr >> 17;
fmferrari 8:339dd9885af3 542 jsr ^= jsr << 5;
fmferrari 8:339dd9885af3 543 ui = icng + jsr;
fmferrari 8:339dd9885af3 544 j = (int16_T)ui & 63;
fmferrari 8:339dd9885af3 545 jp1 = j + 1;
fmferrari 8:339dd9885af3 546 r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * b[jp1];
fmferrari 8:339dd9885af3 547 if (!(std::abs(r) <= b[j])) {
fmferrari 8:339dd9885af3 548 x = (std::abs(r) - b[j]) / (b[jp1] - b[j]);
fmferrari 8:339dd9885af3 549 icng = 69069UL * icng + 1234567UL;
fmferrari 8:339dd9885af3 550 jsr ^= jsr << 13;
fmferrari 8:339dd9885af3 551 jsr ^= jsr >> 17;
fmferrari 8:339dd9885af3 552 jsr ^= jsr << 5;
fmferrari 8:339dd9885af3 553 y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10 + 0.5;
fmferrari 8:339dd9885af3 554 s = x + y;
fmferrari 8:339dd9885af3 555 if (s > 1.301198) {
fmferrari 8:339dd9885af3 556 if (r < 0.0) {
fmferrari 8:339dd9885af3 557 r = 0.4878992 * x - 0.4878992;
fmferrari 8:339dd9885af3 558 } else {
fmferrari 8:339dd9885af3 559 r = 0.4878992 - 0.4878992 * x;
fmferrari 8:339dd9885af3 560 }
fmferrari 8:339dd9885af3 561 } else {
fmferrari 8:339dd9885af3 562 if (!(s <= 0.9689279)) {
fmferrari 8:339dd9885af3 563 x = 0.4878992 - 0.4878992 * x;
fmferrari 8:339dd9885af3 564 if (y > 12.67706 - std::exp(-0.5 * x * x) * 12.37586) {
fmferrari 8:339dd9885af3 565 if (r < 0.0) {
fmferrari 8:339dd9885af3 566 r = -x;
fmferrari 8:339dd9885af3 567 } else {
fmferrari 8:339dd9885af3 568 r = x;
fmferrari 8:339dd9885af3 569 }
fmferrari 8:339dd9885af3 570 } else {
fmferrari 8:339dd9885af3 571 if (!(std::exp(-0.5 * b[jp1] * b[jp1]) + y * 0.01958303 / b[jp1] <=
fmferrari 8:339dd9885af3 572 std::exp(-0.5 * r * r))) {
fmferrari 8:339dd9885af3 573 do {
fmferrari 8:339dd9885af3 574 icng = 69069UL * icng + 1234567UL;
fmferrari 8:339dd9885af3 575 jsr ^= jsr << 13;
fmferrari 8:339dd9885af3 576 jsr ^= jsr >> 17;
fmferrari 8:339dd9885af3 577 jsr ^= jsr << 5;
fmferrari 8:339dd9885af3 578 x = std::log((real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10
fmferrari 8:339dd9885af3 579 + 0.5) / 2.776994;
fmferrari 8:339dd9885af3 580 icng = 69069UL * icng + 1234567UL;
fmferrari 8:339dd9885af3 581 jsr ^= jsr << 13;
fmferrari 8:339dd9885af3 582 jsr ^= jsr >> 17;
fmferrari 8:339dd9885af3 583 jsr ^= jsr << 5;
fmferrari 8:339dd9885af3 584 } while (!(std::log((real_T)(int32_T)(icng + jsr) *
fmferrari 8:339dd9885af3 585 2.328306436538696E-10 + 0.5) * -2.0 > x * x));
fmferrari 8:339dd9885af3 586
fmferrari 8:339dd9885af3 587 if (r < 0.0) {
fmferrari 8:339dd9885af3 588 r = x - 2.776994;
fmferrari 8:339dd9885af3 589 } else {
fmferrari 8:339dd9885af3 590 r = 2.776994 - x;
fmferrari 8:339dd9885af3 591 }
fmferrari 8:339dd9885af3 592 }
fmferrari 8:339dd9885af3 593 }
fmferrari 8:339dd9885af3 594 }
fmferrari 8:339dd9885af3 595 }
fmferrari 8:339dd9885af3 596 }
fmferrari 8:339dd9885af3 597
fmferrari 8:339dd9885af3 598 state[0] = icng;
fmferrari 8:339dd9885af3 599 state[1] = jsr;
fmferrari 8:339dd9885af3 600 return r;
fmferrari 8:339dd9885af3 601 }
fmferrari 8:339dd9885af3 602
fmferrari 8:339dd9885af3 603 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 604 static void EKF_genrandu(uint32_T s, uint32_T *state, real_T *r)
fmferrari 8:339dd9885af3 605 {
fmferrari 8:339dd9885af3 606 uint16_T hi;
fmferrari 8:339dd9885af3 607 uint32_T test1;
fmferrari 8:339dd9885af3 608 uint32_T test2;
fmferrari 8:339dd9885af3 609 hi = (uint16_T)(s / 127773UL);
fmferrari 8:339dd9885af3 610 test1 = (s - hi * 127773UL) * 16807UL;
fmferrari 8:339dd9885af3 611 test2 = 2836UL * hi;
fmferrari 8:339dd9885af3 612 if (test1 < test2) {
fmferrari 8:339dd9885af3 613 *state = (test1 - test2) + 2147483647UL;
fmferrari 8:339dd9885af3 614 } else {
fmferrari 8:339dd9885af3 615 *state = test1 - test2;
fmferrari 8:339dd9885af3 616 }
fmferrari 8:339dd9885af3 617
fmferrari 8:339dd9885af3 618 *r = (real_T)*state * 4.6566128752457969E-10;
fmferrari 8:339dd9885af3 619 }
fmferrari 8:339dd9885af3 620
fmferrari 8:339dd9885af3 621 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 622 static void EKF_twister_state_vector(uint32_T mt[625], uint32_T seed)
fmferrari 8:339dd9885af3 623 {
fmferrari 8:339dd9885af3 624 uint32_T r;
fmferrari 8:339dd9885af3 625 int16_T mti;
fmferrari 8:339dd9885af3 626 r = seed;
fmferrari 8:339dd9885af3 627 mt[0] = seed;
fmferrari 8:339dd9885af3 628 for (mti = 0; mti < 623; mti++) {
fmferrari 8:339dd9885af3 629 r = (r >> 30 ^ r) * 1812433253UL + (1 + mti);
fmferrari 8:339dd9885af3 630 mt[mti + 1] = r;
fmferrari 8:339dd9885af3 631 }
fmferrari 8:339dd9885af3 632
fmferrari 8:339dd9885af3 633 mt[624] = 624UL;
fmferrari 8:339dd9885af3 634 }
fmferrari 8:339dd9885af3 635
fmferrari 8:339dd9885af3 636 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 637 static void EKF_genrand_uint32_vector(uint32_T mt[625], uint32_T u[2])
fmferrari 8:339dd9885af3 638 {
fmferrari 8:339dd9885af3 639 uint32_T mti;
fmferrari 8:339dd9885af3 640 uint32_T y;
fmferrari 8:339dd9885af3 641 int16_T j;
fmferrari 8:339dd9885af3 642 int16_T kk;
fmferrari 8:339dd9885af3 643 for (j = 0; j < 2; j++) {
fmferrari 8:339dd9885af3 644 mti = mt[624] + 1UL;
fmferrari 8:339dd9885af3 645 if (mti >= 625UL) {
fmferrari 8:339dd9885af3 646 for (kk = 0; kk < 227; kk++) {
fmferrari 8:339dd9885af3 647 y = (mt[1 + kk] & 2147483647UL) | (mt[kk] & 2147483648UL);
fmferrari 8:339dd9885af3 648 if (((int16_T)y & 1) == 0) {
fmferrari 8:339dd9885af3 649 y >>= 1;
fmferrari 8:339dd9885af3 650 } else {
fmferrari 8:339dd9885af3 651 y = y >> 1 ^ 2567483615UL;
fmferrari 8:339dd9885af3 652 }
fmferrari 8:339dd9885af3 653
fmferrari 8:339dd9885af3 654 mt[kk] = mt[397 + kk] ^ y;
fmferrari 8:339dd9885af3 655 }
fmferrari 8:339dd9885af3 656
fmferrari 8:339dd9885af3 657 for (kk = 0; kk < 396; kk++) {
fmferrari 8:339dd9885af3 658 y = (mt[kk + 227] & 2147483648UL) | (mt[228 + kk] & 2147483647UL);
fmferrari 8:339dd9885af3 659 if (((int16_T)y & 1) == 0) {
fmferrari 8:339dd9885af3 660 y >>= 1;
fmferrari 8:339dd9885af3 661 } else {
fmferrari 8:339dd9885af3 662 y = y >> 1 ^ 2567483615UL;
fmferrari 8:339dd9885af3 663 }
fmferrari 8:339dd9885af3 664
fmferrari 8:339dd9885af3 665 mt[kk + 227] = mt[kk] ^ y;
fmferrari 8:339dd9885af3 666 }
fmferrari 8:339dd9885af3 667
fmferrari 8:339dd9885af3 668 y = (mt[623] & 2147483648UL) | (mt[0] & 2147483647UL);
fmferrari 8:339dd9885af3 669 if (((int16_T)y & 1) == 0) {
fmferrari 8:339dd9885af3 670 y >>= 1;
fmferrari 8:339dd9885af3 671 } else {
fmferrari 8:339dd9885af3 672 y = y >> 1 ^ 2567483615UL;
fmferrari 8:339dd9885af3 673 }
fmferrari 8:339dd9885af3 674
fmferrari 8:339dd9885af3 675 mt[623] = mt[396] ^ y;
fmferrari 8:339dd9885af3 676 mti = 1UL;
fmferrari 8:339dd9885af3 677 }
fmferrari 8:339dd9885af3 678
fmferrari 8:339dd9885af3 679 y = mt[(int16_T)mti - 1];
fmferrari 8:339dd9885af3 680 mt[624] = mti;
fmferrari 8:339dd9885af3 681 y ^= y >> 11;
fmferrari 8:339dd9885af3 682 y ^= y << 7UL & 2636928640UL;
fmferrari 8:339dd9885af3 683 y ^= y << 15UL & 4022730752UL;
fmferrari 8:339dd9885af3 684 y ^= y >> 18;
fmferrari 8:339dd9885af3 685 u[j] = y;
fmferrari 8:339dd9885af3 686 }
fmferrari 8:339dd9885af3 687 }
fmferrari 8:339dd9885af3 688
fmferrari 8:339dd9885af3 689 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 690 static real_T EKF_genrandu_k(uint32_T mt[625])
fmferrari 8:339dd9885af3 691 {
fmferrari 8:339dd9885af3 692 real_T r;
fmferrari 8:339dd9885af3 693 uint32_T u[2];
fmferrari 8:339dd9885af3 694 int16_T k;
fmferrari 8:339dd9885af3 695 boolean_T b_isvalid;
fmferrari 8:339dd9885af3 696 int32_T exitg1;
fmferrari 8:339dd9885af3 697 boolean_T exitg2;
fmferrari 8:339dd9885af3 698
fmferrari 8:339dd9885af3 699 // ========================= COPYRIGHT NOTICE ============================
fmferrari 8:339dd9885af3 700 // This is a uniform (0,1) pseudorandom number generator based on:
fmferrari 8:339dd9885af3 701 //
fmferrari 8:339dd9885af3 702 // A C-program for MT19937, with initialization improved 2002/1/26.
fmferrari 8:339dd9885af3 703 // Coded by Takuji Nishimura and Makoto Matsumoto.
fmferrari 8:339dd9885af3 704 //
fmferrari 8:339dd9885af3 705 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
fmferrari 8:339dd9885af3 706 // All rights reserved.
fmferrari 8:339dd9885af3 707 //
fmferrari 8:339dd9885af3 708 // Redistribution and use in source and binary forms, with or without
fmferrari 8:339dd9885af3 709 // modification, are permitted provided that the following conditions
fmferrari 8:339dd9885af3 710 // are met:
fmferrari 8:339dd9885af3 711 //
fmferrari 8:339dd9885af3 712 // 1. Redistributions of source code must retain the above copyright
fmferrari 8:339dd9885af3 713 // notice, this list of conditions and the following disclaimer.
fmferrari 8:339dd9885af3 714 //
fmferrari 8:339dd9885af3 715 // 2. Redistributions in binary form must reproduce the above copyright
fmferrari 8:339dd9885af3 716 // notice, this list of conditions and the following disclaimer
fmferrari 8:339dd9885af3 717 // in the documentation and/or other materials provided with the
fmferrari 8:339dd9885af3 718 // distribution.
fmferrari 8:339dd9885af3 719 //
fmferrari 8:339dd9885af3 720 // 3. The names of its contributors may not be used to endorse or
fmferrari 8:339dd9885af3 721 // promote products derived from this software without specific
fmferrari 8:339dd9885af3 722 // prior written permission.
fmferrari 8:339dd9885af3 723 //
fmferrari 8:339dd9885af3 724 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
fmferrari 8:339dd9885af3 725 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
fmferrari 8:339dd9885af3 726 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
fmferrari 8:339dd9885af3 727 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
fmferrari 8:339dd9885af3 728 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
fmferrari 8:339dd9885af3 729 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
fmferrari 8:339dd9885af3 730 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
fmferrari 8:339dd9885af3 731 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
fmferrari 8:339dd9885af3 732 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
fmferrari 8:339dd9885af3 733 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
fmferrari 8:339dd9885af3 734 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
fmferrari 8:339dd9885af3 735 //
fmferrari 8:339dd9885af3 736 // ============================= END =================================
fmferrari 8:339dd9885af3 737 do {
fmferrari 8:339dd9885af3 738 exitg1 = 0L;
fmferrari 8:339dd9885af3 739 EKF_genrand_uint32_vector(mt, u);
fmferrari 8:339dd9885af3 740 r = ((real_T)(u[0] >> 5) * 6.7108864E+7 + (real_T)(u[1] >> 6)) *
fmferrari 8:339dd9885af3 741 1.1102230246251565E-16;
fmferrari 8:339dd9885af3 742 if (r == 0.0) {
fmferrari 8:339dd9885af3 743 if ((mt[624] >= 1UL) && (mt[624] < 625UL)) {
fmferrari 8:339dd9885af3 744 b_isvalid = true;
fmferrari 8:339dd9885af3 745 } else {
fmferrari 8:339dd9885af3 746 b_isvalid = false;
fmferrari 8:339dd9885af3 747 }
fmferrari 8:339dd9885af3 748
fmferrari 8:339dd9885af3 749 if (b_isvalid) {
fmferrari 8:339dd9885af3 750 b_isvalid = false;
fmferrari 8:339dd9885af3 751 k = 1;
fmferrari 8:339dd9885af3 752 exitg2 = false;
fmferrari 8:339dd9885af3 753 while ((!exitg2) && (k < 625)) {
fmferrari 8:339dd9885af3 754 if (mt[k - 1] == 0UL) {
fmferrari 8:339dd9885af3 755 k++;
fmferrari 8:339dd9885af3 756 } else {
fmferrari 8:339dd9885af3 757 b_isvalid = true;
fmferrari 8:339dd9885af3 758 exitg2 = true;
fmferrari 8:339dd9885af3 759 }
fmferrari 8:339dd9885af3 760 }
fmferrari 8:339dd9885af3 761 }
fmferrari 8:339dd9885af3 762
fmferrari 8:339dd9885af3 763 if (!b_isvalid) {
fmferrari 8:339dd9885af3 764 EKF_twister_state_vector(mt, 5489UL);
fmferrari 8:339dd9885af3 765 }
fmferrari 8:339dd9885af3 766 } else {
fmferrari 8:339dd9885af3 767 exitg1 = 1L;
fmferrari 8:339dd9885af3 768 }
fmferrari 8:339dd9885af3 769 } while (exitg1 == 0L);
fmferrari 8:339dd9885af3 770
fmferrari 8:339dd9885af3 771 return r;
fmferrari 8:339dd9885af3 772 }
fmferrari 8:339dd9885af3 773
fmferrari 8:339dd9885af3 774 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 775 static real_T EKF_eml_rand_mt19937ar(uint32_T state[625])
fmferrari 8:339dd9885af3 776 {
fmferrari 8:339dd9885af3 777 real_T r;
fmferrari 8:339dd9885af3 778 int16_T i;
fmferrari 8:339dd9885af3 779 int16_T ip1;
fmferrari 8:339dd9885af3 780 real_T x;
fmferrari 8:339dd9885af3 781 uint32_T u32[2];
fmferrari 8:339dd9885af3 782 real_T b_u;
fmferrari 8:339dd9885af3 783 static const real_T b[257] = { 0.0, 0.215241895984875, 0.286174591792068,
fmferrari 8:339dd9885af3 784 0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787,
fmferrari 8:339dd9885af3 785 0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557,
fmferrari 8:339dd9885af3 786 0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991,
fmferrari 8:339dd9885af3 787 0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419,
fmferrari 8:339dd9885af3 788 0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975,
fmferrari 8:339dd9885af3 789 0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107,
fmferrari 8:339dd9885af3 790 0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997,
fmferrari 8:339dd9885af3 791 0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153,
fmferrari 8:339dd9885af3 792 0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165,
fmferrari 8:339dd9885af3 793 0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959,
fmferrari 8:339dd9885af3 794 0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086,
fmferrari 8:339dd9885af3 795 0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062,
fmferrari 8:339dd9885af3 796 0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548,
fmferrari 8:339dd9885af3 797 1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093,
fmferrari 8:339dd9885af3 798 1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034,
fmferrari 8:339dd9885af3 799 1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357,
fmferrari 8:339dd9885af3 800 1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815,
fmferrari 8:339dd9885af3 801 1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083,
fmferrari 8:339dd9885af3 802 1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944,
fmferrari 8:339dd9885af3 803 1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653,
fmferrari 8:339dd9885af3 804 1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949,
fmferrari 8:339dd9885af3 805 1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382,
fmferrari 8:339dd9885af3 806 1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073,
fmferrari 8:339dd9885af3 807 1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936,
fmferrari 8:339dd9885af3 808 1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328,
fmferrari 8:339dd9885af3 809 1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998,
fmferrari 8:339dd9885af3 810 1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547,
fmferrari 8:339dd9885af3 811 1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872,
fmferrari 8:339dd9885af3 812 1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808,
fmferrari 8:339dd9885af3 813 1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049,
fmferrari 8:339dd9885af3 814 1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761,
fmferrari 8:339dd9885af3 815 1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417,
fmferrari 8:339dd9885af3 816 1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591,
fmferrari 8:339dd9885af3 817 1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609,
fmferrari 8:339dd9885af3 818 1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487,
fmferrari 8:339dd9885af3 819 1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818,
fmferrari 8:339dd9885af3 820 1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719,
fmferrari 8:339dd9885af3 821 1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782,
fmferrari 8:339dd9885af3 822 1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172,
fmferrari 8:339dd9885af3 823 1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452,
fmferrari 8:339dd9885af3 824 1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082,
fmferrari 8:339dd9885af3 825 1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281,
fmferrari 8:339dd9885af3 826 1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127,
fmferrari 8:339dd9885af3 827 1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043,
fmferrari 8:339dd9885af3 828 1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569,
fmferrari 8:339dd9885af3 829 1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366,
fmferrari 8:339dd9885af3 830 1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053,
fmferrari 8:339dd9885af3 831 2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753,
fmferrari 8:339dd9885af3 832 2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983,
fmferrari 8:339dd9885af3 833 2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952,
fmferrari 8:339dd9885af3 834 2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431,
fmferrari 8:339dd9885af3 835 2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416,
fmferrari 8:339dd9885af3 836 2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379,
fmferrari 8:339dd9885af3 837 2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137,
fmferrari 8:339dd9885af3 838 2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729,
fmferrari 8:339dd9885af3 839 2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715,
fmferrari 8:339dd9885af3 840 2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105,
fmferrari 8:339dd9885af3 841 2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184,
fmferrari 8:339dd9885af3 842 2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674,
fmferrari 8:339dd9885af3 843 2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999,
fmferrari 8:339dd9885af3 844 2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341,
fmferrari 8:339dd9885af3 845 2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214,
fmferrari 8:339dd9885af3 846 3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143,
fmferrari 8:339dd9885af3 847 3.65415288536101, 3.91075795952492 };
fmferrari 8:339dd9885af3 848
fmferrari 8:339dd9885af3 849 static const real_T c[257] = { 1.0, 0.977101701267673, 0.959879091800108,
fmferrari 8:339dd9885af3 850 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
fmferrari 8:339dd9885af3 851 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
fmferrari 8:339dd9885af3 852 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
fmferrari 8:339dd9885af3 853 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
fmferrari 8:339dd9885af3 854 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
fmferrari 8:339dd9885af3 855 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
fmferrari 8:339dd9885af3 856 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
fmferrari 8:339dd9885af3 857 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
fmferrari 8:339dd9885af3 858 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
fmferrari 8:339dd9885af3 859 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
fmferrari 8:339dd9885af3 860 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
fmferrari 8:339dd9885af3 861 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
fmferrari 8:339dd9885af3 862 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
fmferrari 8:339dd9885af3 863 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
fmferrari 8:339dd9885af3 864 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
fmferrari 8:339dd9885af3 865 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
fmferrari 8:339dd9885af3 866 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
fmferrari 8:339dd9885af3 867 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
fmferrari 8:339dd9885af3 868 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
fmferrari 8:339dd9885af3 869 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
fmferrari 8:339dd9885af3 870 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
fmferrari 8:339dd9885af3 871 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
fmferrari 8:339dd9885af3 872 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
fmferrari 8:339dd9885af3 873 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
fmferrari 8:339dd9885af3 874 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
fmferrari 8:339dd9885af3 875 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
fmferrari 8:339dd9885af3 876 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
fmferrari 8:339dd9885af3 877 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
fmferrari 8:339dd9885af3 878 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
fmferrari 8:339dd9885af3 879 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
fmferrari 8:339dd9885af3 880 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
fmferrari 8:339dd9885af3 881 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
fmferrari 8:339dd9885af3 882 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
fmferrari 8:339dd9885af3 883 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
fmferrari 8:339dd9885af3 884 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
fmferrari 8:339dd9885af3 885 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
fmferrari 8:339dd9885af3 886 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
fmferrari 8:339dd9885af3 887 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
fmferrari 8:339dd9885af3 888 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
fmferrari 8:339dd9885af3 889 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
fmferrari 8:339dd9885af3 890 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
fmferrari 8:339dd9885af3 891 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
fmferrari 8:339dd9885af3 892 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
fmferrari 8:339dd9885af3 893 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
fmferrari 8:339dd9885af3 894 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
fmferrari 8:339dd9885af3 895 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
fmferrari 8:339dd9885af3 896 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
fmferrari 8:339dd9885af3 897 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
fmferrari 8:339dd9885af3 898 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
fmferrari 8:339dd9885af3 899 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
fmferrari 8:339dd9885af3 900 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
fmferrari 8:339dd9885af3 901 0.093365311912691, 0.0911136480663738, 0.0888735920682759,
fmferrari 8:339dd9885af3 902 0.0866451944505581, 0.0844285095703535, 0.082223595813203,
fmferrari 8:339dd9885af3 903 0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
fmferrari 8:339dd9885af3 904 0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
fmferrari 8:339dd9885af3 905 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
fmferrari 8:339dd9885af3 906 0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
fmferrari 8:339dd9885af3 907 0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
fmferrari 8:339dd9885af3 908 0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
fmferrari 8:339dd9885af3 909 0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
fmferrari 8:339dd9885af3 910 0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
fmferrari 8:339dd9885af3 911 0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
fmferrari 8:339dd9885af3 912 0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
fmferrari 8:339dd9885af3 913 0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
fmferrari 8:339dd9885af3 914 0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
fmferrari 8:339dd9885af3 915 0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
fmferrari 8:339dd9885af3 916 0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
fmferrari 8:339dd9885af3 917 0.000477467764609386 };
fmferrari 8:339dd9885af3 918
fmferrari 8:339dd9885af3 919 int32_T exitg1;
fmferrari 8:339dd9885af3 920 do {
fmferrari 8:339dd9885af3 921 exitg1 = 0L;
fmferrari 8:339dd9885af3 922 EKF_genrand_uint32_vector(state, u32);
fmferrari 8:339dd9885af3 923 i = (int16_T)(u32[1] >> 24);
fmferrari 8:339dd9885af3 924 ip1 = i + 1;
fmferrari 8:339dd9885af3 925 r = (((real_T)(u32[0] >> 3) * 1.6777216E+7 + (real_T)((int32_T)u32[1] &
fmferrari 8:339dd9885af3 926 16777215L)) * 2.2204460492503131E-16 - 1.0) * b[ip1];
fmferrari 8:339dd9885af3 927 if (std::abs(r) <= b[i]) {
fmferrari 8:339dd9885af3 928 exitg1 = 1L;
fmferrari 8:339dd9885af3 929 } else if (i + 1 < 256) {
fmferrari 8:339dd9885af3 930 x = EKF_genrandu_k(state);
fmferrari 8:339dd9885af3 931 if ((c[i] - c[ip1]) * x + c[ip1] < std::exp(-0.5 * r * r)) {
fmferrari 8:339dd9885af3 932 exitg1 = 1L;
fmferrari 8:339dd9885af3 933 }
fmferrari 8:339dd9885af3 934 } else {
fmferrari 8:339dd9885af3 935 do {
fmferrari 8:339dd9885af3 936 x = EKF_genrandu_k(state);
fmferrari 8:339dd9885af3 937 x = std::log(x) * 0.273661237329758;
fmferrari 8:339dd9885af3 938 b_u = EKF_genrandu_k(state);
fmferrari 8:339dd9885af3 939 } while (!(-2.0 * std::log(b_u) > x * x));
fmferrari 8:339dd9885af3 940
fmferrari 8:339dd9885af3 941 if (r < 0.0) {
fmferrari 8:339dd9885af3 942 r = x - 3.65415288536101;
fmferrari 8:339dd9885af3 943 } else {
fmferrari 8:339dd9885af3 944 r = 3.65415288536101 - x;
fmferrari 8:339dd9885af3 945 }
fmferrari 8:339dd9885af3 946
fmferrari 8:339dd9885af3 947 exitg1 = 1L;
fmferrari 8:339dd9885af3 948 }
fmferrari 8:339dd9885af3 949 } while (exitg1 == 0L);
fmferrari 8:339dd9885af3 950
fmferrari 8:339dd9885af3 951 return r;
fmferrari 8:339dd9885af3 952 }
fmferrari 8:339dd9885af3 953
fmferrari 8:339dd9885af3 954 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 955 static void EKF_randn(real_T r[2])
fmferrari 8:339dd9885af3 956 {
fmferrari 8:339dd9885af3 957 real_T t;
fmferrari 8:339dd9885af3 958 uint32_T b_state;
fmferrari 8:339dd9885af3 959 real_T c_r;
fmferrari 8:339dd9885af3 960 uint32_T c_state;
fmferrari 8:339dd9885af3 961 if (!EKF_DW.method_not_empty) {
fmferrari 8:339dd9885af3 962 EKF_DW.method = 0UL;
fmferrari 8:339dd9885af3 963 EKF_DW.method_not_empty = true;
fmferrari 8:339dd9885af3 964 EKF_DW.state[0] = 362436069UL;
fmferrari 8:339dd9885af3 965 EKF_DW.state[1] = 0UL;
fmferrari 8:339dd9885af3 966 if (EKF_DW.state[1] == 0UL) {
fmferrari 8:339dd9885af3 967 EKF_DW.state[1] = 521288629UL;
fmferrari 8:339dd9885af3 968 }
fmferrari 8:339dd9885af3 969 }
fmferrari 8:339dd9885af3 970
fmferrari 8:339dd9885af3 971 if (EKF_DW.method == 0UL) {
fmferrari 8:339dd9885af3 972 switch (EKF_DW.method_c) {
fmferrari 8:339dd9885af3 973 case 4UL:
fmferrari 8:339dd9885af3 974 c_state = EKF_DW.state_p;
fmferrari 8:339dd9885af3 975 do {
fmferrari 8:339dd9885af3 976 EKF_genrandu(c_state, &b_state, &c_r);
fmferrari 8:339dd9885af3 977 EKF_genrandu(b_state, &c_state, &t);
fmferrari 8:339dd9885af3 978 c_r = 2.0 * c_r - 1.0;
fmferrari 8:339dd9885af3 979 t = 2.0 * t - 1.0;
fmferrari 8:339dd9885af3 980 t = t * t + c_r * c_r;
fmferrari 8:339dd9885af3 981 } while (!(t <= 1.0));
fmferrari 8:339dd9885af3 982
fmferrari 8:339dd9885af3 983 c_r *= std::sqrt(-2.0 * std::log(t) / t);
fmferrari 8:339dd9885af3 984 EKF_DW.state_p = c_state;
fmferrari 8:339dd9885af3 985 r[0] = c_r;
fmferrari 8:339dd9885af3 986 c_state = EKF_DW.state_p;
fmferrari 8:339dd9885af3 987 do {
fmferrari 8:339dd9885af3 988 EKF_genrandu(c_state, &b_state, &c_r);
fmferrari 8:339dd9885af3 989 EKF_genrandu(b_state, &c_state, &t);
fmferrari 8:339dd9885af3 990 c_r = 2.0 * c_r - 1.0;
fmferrari 8:339dd9885af3 991 t = 2.0 * t - 1.0;
fmferrari 8:339dd9885af3 992 t = t * t + c_r * c_r;
fmferrari 8:339dd9885af3 993 } while (!(t <= 1.0));
fmferrari 8:339dd9885af3 994
fmferrari 8:339dd9885af3 995 c_r *= std::sqrt(-2.0 * std::log(t) / t);
fmferrari 8:339dd9885af3 996 EKF_DW.state_p = c_state;
fmferrari 8:339dd9885af3 997 r[1] = c_r;
fmferrari 8:339dd9885af3 998 break;
fmferrari 8:339dd9885af3 999
fmferrari 8:339dd9885af3 1000 case 5UL:
fmferrari 8:339dd9885af3 1001 t = EKF_eml_rand_shr3cong(EKF_DW.state_g);
fmferrari 8:339dd9885af3 1002 r[0] = t;
fmferrari 8:339dd9885af3 1003 t = EKF_eml_rand_shr3cong(EKF_DW.state_g);
fmferrari 8:339dd9885af3 1004 r[1] = t;
fmferrari 8:339dd9885af3 1005 break;
fmferrari 8:339dd9885af3 1006
fmferrari 8:339dd9885af3 1007 default:
fmferrari 8:339dd9885af3 1008 if (!EKF_DW.state_not_empty) {
fmferrari 8:339dd9885af3 1009 memset(&EKF_DW.state_pv[0], 0, 625U * sizeof(uint32_T));
fmferrari 8:339dd9885af3 1010 EKF_twister_state_vector(EKF_DW.state_pv, 5489UL);
fmferrari 8:339dd9885af3 1011 EKF_DW.state_not_empty = true;
fmferrari 8:339dd9885af3 1012 }
fmferrari 8:339dd9885af3 1013
fmferrari 8:339dd9885af3 1014 t = EKF_eml_rand_mt19937ar(EKF_DW.state_pv);
fmferrari 8:339dd9885af3 1015 r[0] = t;
fmferrari 8:339dd9885af3 1016 t = EKF_eml_rand_mt19937ar(EKF_DW.state_pv);
fmferrari 8:339dd9885af3 1017 r[1] = t;
fmferrari 8:339dd9885af3 1018 break;
fmferrari 8:339dd9885af3 1019 }
fmferrari 8:339dd9885af3 1020 } else if (EKF_DW.method == 4UL) {
fmferrari 8:339dd9885af3 1021 c_state = EKF_DW.state[0];
fmferrari 8:339dd9885af3 1022 do {
fmferrari 8:339dd9885af3 1023 EKF_genrandu(c_state, &b_state, &c_r);
fmferrari 8:339dd9885af3 1024 EKF_genrandu(b_state, &c_state, &t);
fmferrari 8:339dd9885af3 1025 c_r = 2.0 * c_r - 1.0;
fmferrari 8:339dd9885af3 1026 t = 2.0 * t - 1.0;
fmferrari 8:339dd9885af3 1027 t = t * t + c_r * c_r;
fmferrari 8:339dd9885af3 1028 } while (!(t <= 1.0));
fmferrari 8:339dd9885af3 1029
fmferrari 8:339dd9885af3 1030 c_r *= std::sqrt(-2.0 * std::log(t) / t);
fmferrari 8:339dd9885af3 1031 EKF_DW.state[0] = c_state;
fmferrari 8:339dd9885af3 1032 r[0] = c_r;
fmferrari 8:339dd9885af3 1033 c_state = EKF_DW.state[0];
fmferrari 8:339dd9885af3 1034 do {
fmferrari 8:339dd9885af3 1035 EKF_genrandu(c_state, &b_state, &c_r);
fmferrari 8:339dd9885af3 1036 EKF_genrandu(b_state, &c_state, &t);
fmferrari 8:339dd9885af3 1037 c_r = 2.0 * c_r - 1.0;
fmferrari 8:339dd9885af3 1038 t = 2.0 * t - 1.0;
fmferrari 8:339dd9885af3 1039 t = t * t + c_r * c_r;
fmferrari 8:339dd9885af3 1040 } while (!(t <= 1.0));
fmferrari 8:339dd9885af3 1041
fmferrari 8:339dd9885af3 1042 c_r *= std::sqrt(-2.0 * std::log(t) / t);
fmferrari 8:339dd9885af3 1043 EKF_DW.state[0] = c_state;
fmferrari 8:339dd9885af3 1044 r[1] = c_r;
fmferrari 8:339dd9885af3 1045 } else {
fmferrari 8:339dd9885af3 1046 t = EKF_eml_rand_shr3cong(EKF_DW.state);
fmferrari 8:339dd9885af3 1047 r[0] = t;
fmferrari 8:339dd9885af3 1048 t = EKF_eml_rand_shr3cong(EKF_DW.state);
fmferrari 8:339dd9885af3 1049 r[1] = t;
fmferrari 8:339dd9885af3 1050 }
fmferrari 8:339dd9885af3 1051 }
fmferrari 8:339dd9885af3 1052
fmferrari 8:339dd9885af3 1053 // Function for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 8:339dd9885af3 1054 static real_T EKF_randn_g(void)
fmferrari 8:339dd9885af3 1055 {
fmferrari 8:339dd9885af3 1056 real_T r;
fmferrari 8:339dd9885af3 1057 real_T t;
fmferrari 8:339dd9885af3 1058 uint32_T b_state;
fmferrari 8:339dd9885af3 1059 real_T c_r;
fmferrari 8:339dd9885af3 1060 uint32_T c_state;
fmferrari 8:339dd9885af3 1061 if (!EKF_DW.method_not_empty) {
fmferrari 8:339dd9885af3 1062 EKF_DW.method = 0UL;
fmferrari 8:339dd9885af3 1063 EKF_DW.method_not_empty = true;
fmferrari 8:339dd9885af3 1064 EKF_DW.state[0] = 362436069UL;
fmferrari 8:339dd9885af3 1065 EKF_DW.state[1] = 0UL;
fmferrari 8:339dd9885af3 1066 if (EKF_DW.state[1] == 0UL) {
fmferrari 8:339dd9885af3 1067 EKF_DW.state[1] = 521288629UL;
fmferrari 8:339dd9885af3 1068 }
fmferrari 8:339dd9885af3 1069 }
fmferrari 8:339dd9885af3 1070
fmferrari 8:339dd9885af3 1071 if (EKF_DW.method == 0UL) {
fmferrari 8:339dd9885af3 1072 switch (EKF_DW.method_c) {
fmferrari 8:339dd9885af3 1073 case 4UL:
fmferrari 8:339dd9885af3 1074 c_state = EKF_DW.state_p;
fmferrari 8:339dd9885af3 1075 do {
fmferrari 8:339dd9885af3 1076 EKF_genrandu(c_state, &b_state, &c_r);
fmferrari 8:339dd9885af3 1077 EKF_genrandu(b_state, &c_state, &t);
fmferrari 8:339dd9885af3 1078 c_r = 2.0 * c_r - 1.0;
fmferrari 8:339dd9885af3 1079 t = 2.0 * t - 1.0;
fmferrari 8:339dd9885af3 1080 t = t * t + c_r * c_r;
fmferrari 8:339dd9885af3 1081 } while (!(t <= 1.0));
fmferrari 8:339dd9885af3 1082
fmferrari 8:339dd9885af3 1083 r = std::sqrt(-2.0 * std::log(t) / t) * c_r;
fmferrari 8:339dd9885af3 1084 EKF_DW.state_p = c_state;
fmferrari 8:339dd9885af3 1085 break;
fmferrari 8:339dd9885af3 1086
fmferrari 8:339dd9885af3 1087 case 5UL:
fmferrari 8:339dd9885af3 1088 r = EKF_eml_rand_shr3cong(EKF_DW.state_g);
fmferrari 8:339dd9885af3 1089 break;
fmferrari 8:339dd9885af3 1090
fmferrari 8:339dd9885af3 1091 default:
fmferrari 8:339dd9885af3 1092 if (!EKF_DW.state_not_empty) {
fmferrari 8:339dd9885af3 1093 memset(&EKF_DW.state_pv[0], 0, 625U * sizeof(uint32_T));
fmferrari 8:339dd9885af3 1094 EKF_twister_state_vector(EKF_DW.state_pv, 5489UL);
fmferrari 8:339dd9885af3 1095 EKF_DW.state_not_empty = true;
fmferrari 8:339dd9885af3 1096 }
fmferrari 8:339dd9885af3 1097
fmferrari 8:339dd9885af3 1098 r = EKF_eml_rand_mt19937ar(EKF_DW.state_pv);
fmferrari 8:339dd9885af3 1099 break;
fmferrari 8:339dd9885af3 1100 }
fmferrari 8:339dd9885af3 1101 } else if (EKF_DW.method == 4UL) {
fmferrari 8:339dd9885af3 1102 c_state = EKF_DW.state[0];
fmferrari 8:339dd9885af3 1103 do {
fmferrari 8:339dd9885af3 1104 EKF_genrandu(c_state, &b_state, &c_r);
fmferrari 8:339dd9885af3 1105 EKF_genrandu(b_state, &c_state, &t);
fmferrari 8:339dd9885af3 1106 c_r = 2.0 * c_r - 1.0;
fmferrari 8:339dd9885af3 1107 t = 2.0 * t - 1.0;
fmferrari 8:339dd9885af3 1108 t = t * t + c_r * c_r;
fmferrari 8:339dd9885af3 1109 } while (!(t <= 1.0));
fmferrari 8:339dd9885af3 1110
fmferrari 8:339dd9885af3 1111 r = std::sqrt(-2.0 * std::log(t) / t) * c_r;
fmferrari 8:339dd9885af3 1112 EKF_DW.state[0] = c_state;
fmferrari 8:339dd9885af3 1113 } else {
fmferrari 8:339dd9885af3 1114 r = EKF_eml_rand_shr3cong(EKF_DW.state);
fmferrari 8:339dd9885af3 1115 }
fmferrari 8:339dd9885af3 1116
fmferrari 8:339dd9885af3 1117 return r;
fmferrari 8:339dd9885af3 1118 }
fmferrari 8:339dd9885af3 1119
fmferrari 6:cb71171a7108 1120 // Model step function
fmferrari 7:e0be546a9112 1121 void EKF_step(void)
fmferrari 6:cb71171a7108 1122 {
fmferrari 8:339dd9885af3 1123 real_T p[13];
fmferrari 8:339dd9885af3 1124 real_T Ve;
fmferrari 9:17c258c67c33 1125 real_T A;
fmferrari 8:339dd9885af3 1126 real_T dOCV[21];
fmferrari 8:339dd9885af3 1127 real_T work;
fmferrari 8:339dd9885af3 1128 real_T b_y1[20];
fmferrari 8:339dd9885af3 1129 int16_T ixLead;
fmferrari 8:339dd9885af3 1130 int16_T iyLead;
fmferrari 8:339dd9885af3 1131 int16_T high_i;
fmferrari 8:339dd9885af3 1132 int16_T mid_i;
fmferrari 9:17c258c67c33 1133 static const real_T d[21] = { 0.21410000000000018, 0.46399999999999997,
fmferrari 9:17c258c67c33 1134 0.49540000000000006, 0.53080000000000016, 0.56050000000000022, 0.5831,
fmferrari 9:17c258c67c33 1135 0.61229999999999984, 0.63640000000000008, 0.6515, 0.66809999999999992,
fmferrari 9:17c258c67c33 1136 0.68809999999999993, 0.71329999999999982, 0.7441, 0.78679999999999994,
fmferrari 9:17c258c67c33 1137 0.85020000000000007, 0.8980999999999999, 0.9478, 1.0008, 1.0574000000000003,
fmferrari 9:17c258c67c33 1138 1.1180000000000003, 1.1840000000000002 };
fmferrari 8:339dd9885af3 1139
fmferrari 8:339dd9885af3 1140 static const real_T b_y[4] = { 2.7469025926365436E-24, -1.0638783181939197E-23,
fmferrari 8:339dd9885af3 1141 -1.0638783181939197E-23, 4.1204121287633946E-23 };
fmferrari 8:339dd9885af3 1142
fmferrari 9:17c258c67c33 1143 static const real_T OCV_data[21] = { 0.21410000000000018, 0.46399999999999997,
fmferrari 9:17c258c67c33 1144 0.49540000000000006, 0.53080000000000016, 0.56050000000000022, 0.5831,
fmferrari 9:17c258c67c33 1145 0.61229999999999984, 0.63640000000000008, 0.6515, 0.66809999999999992,
fmferrari 9:17c258c67c33 1146 0.68809999999999993, 0.71329999999999982, 0.7441, 0.78679999999999994,
fmferrari 9:17c258c67c33 1147 0.85020000000000007, 0.8980999999999999, 0.9478, 1.0008, 1.0574000000000003,
fmferrari 9:17c258c67c33 1148 1.1180000000000003, 1.1840000000000002 };
fmferrari 8:339dd9885af3 1149
fmferrari 8:339dd9885af3 1150 static const real_T SOC_spacing[21] = { 0.0, 0.05, 0.1, 0.15000000000000002,
fmferrari 8:339dd9885af3 1151 0.2, 0.25, 0.30000000000000004, 0.35000000000000003, 0.4, 0.45, 0.5, 0.55,
fmferrari 8:339dd9885af3 1152 0.6, 0.64999999999999991, 0.7, 0.75, 0.8, 0.85, 0.9, 0.95, 1.0 };
fmferrari 8:339dd9885af3 1153
fmferrari 8:339dd9885af3 1154 static const real_T e[21] = { 0.0, 0.05, 0.1, 0.15000000000000002, 0.2, 0.25,
fmferrari 8:339dd9885af3 1155 0.30000000000000004, 0.35000000000000003, 0.4, 0.45, 0.5, 0.55, 0.6,
fmferrari 8:339dd9885af3 1156 0.64999999999999991, 0.7, 0.75, 0.8, 0.85, 0.9, 0.95, 1.0 };
fmferrari 8:339dd9885af3 1157
fmferrari 8:339dd9885af3 1158 static const real_T a[4] = { 1.0, 0.0, 0.0, 0.9999985539602041 };
fmferrari 8:339dd9885af3 1159
fmferrari 8:339dd9885af3 1160 real_T a_0[2];
fmferrari 8:339dd9885af3 1161 real_T a_1[4];
fmferrari 8:339dd9885af3 1162 real_T b_y1_0[21];
fmferrari 8:339dd9885af3 1163 real_T b_y1_1[21];
fmferrari 8:339dd9885af3 1164 real_T C_idx_0;
fmferrari 9:17c258c67c33 1165 real_T KGain_idx_0;
fmferrari 8:339dd9885af3 1166 real_T unusedExpr[2];
fmferrari 8:339dd9885af3 1167
fmferrari 8:339dd9885af3 1168 // MATLAB Function: '<Root>/MATLAB Function' incorporates:
fmferrari 6:cb71171a7108 1169 // Inport: '<Root>/Current'
fmferrari 6:cb71171a7108 1170 // Inport: '<Root>/Voltage'
fmferrari 6:cb71171a7108 1171
fmferrari 7:e0be546a9112 1172 // MATLAB Function 'MATLAB Function': '<S1>:1'
fmferrari 8:339dd9885af3 1173 // '<S1>:1:3' coder.extrinsic('display')
fmferrari 9:17c258c67c33 1174 // CONVERT VOLTAGE_CURRENT MEASURMENT TO ACTUAL CURRENT
fmferrari 9:17c258c67c33 1175 // '<S1>:1:6' dSPACE_range=1;
fmferrari 9:17c258c67c33 1176 // [V]
fmferrari 9:17c258c67c33 1177 // '<S1>:1:7' I_range=50;
fmferrari 9:17c258c67c33 1178 // [A]
fmferrari 9:17c258c67c33 1179 // '<S1>:1:9' Current=V_Current*I_range/dSPACE_range;
fmferrari 9:17c258c67c33 1180 A = EKF_U.Current * 50.0;
fmferrari 9:17c258c67c33 1181
fmferrari 9:17c258c67c33 1182 // '<S1>:1:11' Voltage_new=Voltage;
fmferrari 8:339dd9885af3 1183 // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fmferrari 8:339dd9885af3 1184 // DATA
fmferrari 9:17c258c67c33 1185 // '<S1>:1:16' dt=1e-4;
fmferrari 9:17c258c67c33 1186 // '<S1>:1:18' Q_total_t=[190800];
fmferrari 8:339dd9885af3 1187 // Ampere*seconds
fmferrari 9:17c258c67c33 1188 // '<S1>:1:19' R_0_t=0.0011622500;
fmferrari 8:339dd9885af3 1189 // Ohm
fmferrari 9:17c258c67c33 1190 // '<S1>:1:20' R1_t=0.00140375000;
fmferrari 8:339dd9885af3 1191 // Ohm
fmferrari 9:17c258c67c33 1192 // '<S1>:1:21' C1_t=49264;
fmferrari 9:17c258c67c33 1193 // '<S1>:1:22' OCV_data=[3.2141
fmferrari 9:17c258c67c33 1194 // '<S1>:1:23' 3.4640
fmferrari 9:17c258c67c33 1195 // '<S1>:1:24' 3.4954
fmferrari 9:17c258c67c33 1196 // '<S1>:1:25' 3.5308
fmferrari 9:17c258c67c33 1197 // '<S1>:1:26' 3.5605
fmferrari 9:17c258c67c33 1198 // '<S1>:1:27' 3.5831
fmferrari 9:17c258c67c33 1199 // '<S1>:1:28' 3.6123
fmferrari 9:17c258c67c33 1200 // '<S1>:1:29' 3.6364
fmferrari 9:17c258c67c33 1201 // '<S1>:1:30' 3.6515
fmferrari 9:17c258c67c33 1202 // '<S1>:1:31' 3.6681
fmferrari 9:17c258c67c33 1203 // '<S1>:1:32' 3.6881
fmferrari 9:17c258c67c33 1204 // '<S1>:1:33' 3.7133
fmferrari 9:17c258c67c33 1205 // '<S1>:1:34' 3.7441
fmferrari 9:17c258c67c33 1206 // '<S1>:1:35' 3.7868
fmferrari 9:17c258c67c33 1207 // '<S1>:1:36' 3.8502
fmferrari 9:17c258c67c33 1208 // '<S1>:1:37' 3.8981
fmferrari 9:17c258c67c33 1209 // '<S1>:1:38' 3.9478
fmferrari 9:17c258c67c33 1210 // '<S1>:1:39' 4.0008
fmferrari 9:17c258c67c33 1211 // '<S1>:1:40' 4.0574
fmferrari 9:17c258c67c33 1212 // '<S1>:1:41' 4.1180
fmferrari 9:17c258c67c33 1213 // '<S1>:1:42' 4.1840]';
fmferrari 9:17c258c67c33 1214 // '<S1>:1:44' OCV_data=OCV_data-3;
fmferrari 9:17c258c67c33 1215 // '<S1>:1:46' z=0:0.05:1;
fmferrari 9:17c258c67c33 1216 // '<S1>:1:47' Order=12;
fmferrari 8:339dd9885af3 1217 // EKF ALLOWS FOR A NON LINEAR OCV FUNCTION
fmferrari 9:17c258c67c33 1218 // '<S1>:1:48' [p]=polyfit(z,OCV_data,Order);
fmferrari 8:339dd9885af3 1219 EKF_polyfit(SOC_spacing, OCV_data, p);
fmferrari 8:339dd9885af3 1220
fmferrari 9:17c258c67c33 1221 // '<S1>:1:49' Q_total=Q_total_t;
fmferrari 9:17c258c67c33 1222 // '<S1>:1:50' R1=R1_t;
fmferrari 9:17c258c67c33 1223 // '<S1>:1:51' C1=C1_t;
fmferrari 9:17c258c67c33 1224 // '<S1>:1:52' R_0=R_0_t;
fmferrari 9:17c258c67c33 1225 // '<S1>:1:53' t_RC=R1*C1;
fmferrari 8:339dd9885af3 1226 // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fmferrari 8:339dd9885af3 1227 // INITIALISE SS VECTORS AND COV. MATRICES
fmferrari 9:17c258c67c33 1228 // '<S1>:1:61' if isempty(Current_prev)
fmferrari 9:17c258c67c33 1229 // '<S1>:1:66' if isempty(Xhat)
fmferrari 9:17c258c67c33 1230 // '<S1>:1:71' if isempty(P0)
fmferrari 9:17c258c67c33 1231 // '<S1>:1:77' sigmaW=1e-5;
fmferrari 8:339dd9885af3 1232 // COV. OF PROCESS NOISE (CURRENT) [Q]
fmferrari 9:17c258c67c33 1233 // '<S1>:1:79' SigmaV=1e-5;
fmferrari 8:339dd9885af3 1234 // COV. OF SENSOR NOISE (VOLTAGE) [R]
fmferrari 8:339dd9885af3 1235 // EXTENDED KALMA FILTER MATRICES
fmferrari 9:17c258c67c33 1236 // '<S1>:1:84' Ahat=[1 0
fmferrari 9:17c258c67c33 1237 // '<S1>:1:85' 0 exp(-dt/t_RC)];
fmferrari 9:17c258c67c33 1238 // '<S1>:1:87' Bhat_I=[-dt/Q_total
fmferrari 9:17c258c67c33 1239 // '<S1>:1:88' dt/C1];
fmferrari 9:17c258c67c33 1240 // '<S1>:1:90' Bhat_W=[-dt/Q_total
fmferrari 9:17c258c67c33 1241 // '<S1>:1:91' dt/C1];
fmferrari 9:17c258c67c33 1242 // '<S1>:1:94' Xhat=Ahat*Xhat+Bhat_I*Current_prev;
fmferrari 9:17c258c67c33 1243 a_0[0] = (0.0 * EKF_DW.Xhat[1] + EKF_DW.Xhat[0]) + -5.2410901467505246E-10 *
fmferrari 9:17c258c67c33 1244 EKF_DW.Current_prev;
fmferrari 9:17c258c67c33 1245 a_0[1] = (0.0 * EKF_DW.Xhat[0] + 0.9999985539602041 * EKF_DW.Xhat[1]) +
fmferrari 9:17c258c67c33 1246 2.0298798311139983E-9 * EKF_DW.Current_prev;
fmferrari 8:339dd9885af3 1247
fmferrari 8:339dd9885af3 1248 // STATE PREDICTION TIME UPDATE
fmferrari 9:17c258c67c33 1249 // '<S1>:1:96' P0= Ahat*P0*Ahat'+ Bhat_W*sigmaW*Bhat_W';
fmferrari 8:339dd9885af3 1250 for (ixLead = 0; ixLead < 2; ixLead++) {
fmferrari 9:17c258c67c33 1251 EKF_DW.Xhat[ixLead] = a_0[ixLead];
fmferrari 8:339dd9885af3 1252 a_1[ixLead] = 0.0;
fmferrari 8:339dd9885af3 1253 a_1[ixLead] += a[ixLead] * EKF_DW.P0[0];
fmferrari 8:339dd9885af3 1254 a_1[ixLead] += a[ixLead + 2] * EKF_DW.P0[1];
fmferrari 8:339dd9885af3 1255 a_1[ixLead + 2] = 0.0;
fmferrari 8:339dd9885af3 1256 a_1[ixLead + 2] += a[ixLead] * EKF_DW.P0[2];
fmferrari 8:339dd9885af3 1257 a_1[ixLead + 2] += a[ixLead + 2] * EKF_DW.P0[3];
fmferrari 8:339dd9885af3 1258 }
fmferrari 8:339dd9885af3 1259
fmferrari 8:339dd9885af3 1260 for (ixLead = 0; ixLead < 2; ixLead++) {
fmferrari 8:339dd9885af3 1261 EKF_DW.P0[ixLead << 1] = (a[(ixLead << 1) + 1] * a_1[2] + a[ixLead << 1] *
fmferrari 8:339dd9885af3 1262 a_1[0]) + b_y[ixLead << 1];
fmferrari 8:339dd9885af3 1263 EKF_DW.P0[1 + (ixLead << 1)] = (a[(ixLead << 1) + 1] * a_1[3] + a[ixLead <<
fmferrari 8:339dd9885af3 1264 1] * a_1[1]) + b_y[(ixLead << 1) + 1];
fmferrari 8:339dd9885af3 1265 }
fmferrari 8:339dd9885af3 1266
fmferrari 8:339dd9885af3 1267 // ERROR COV. TIME UPDATE
fmferrari 9:17c258c67c33 1268 // '<S1>:1:98' W=chol(sigmaW,'lower').*randn(2,1);
fmferrari 8:339dd9885af3 1269 EKF_randn(unusedExpr);
fmferrari 8:339dd9885af3 1270
fmferrari 8:339dd9885af3 1271 // ADD PROCESS NOISE
fmferrari 9:17c258c67c33 1272 // '<S1>:1:99' Ve=chol(SigmaV,'lower').*randn(1);
fmferrari 8:339dd9885af3 1273 Ve = 0.0031622776601683794 * EKF_randn_g();
fmferrari 8:339dd9885af3 1274
fmferrari 8:339dd9885af3 1275 // ADD MEAS. NOISE
fmferrari 9:17c258c67c33 1276 // '<S1>:1:101' Ytrue=Voltage_new+Ve;
fmferrari 8:339dd9885af3 1277 // GET TRUE TERMINAL VOLTAGE READING
fmferrari 9:17c258c67c33 1278 // '<S1>:1:103' Yhat=OCV(p,Xhat(1))-Xhat(2)-R_0*Current;
fmferrari 9:17c258c67c33 1279 // '<S1>:1:131' OCV=polyval(p,SOC);
fmferrari 9:17c258c67c33 1280 KGain_idx_0 = p[0];
fmferrari 8:339dd9885af3 1281 for (ixLead = 0; ixLead < 12; ixLead++) {
fmferrari 9:17c258c67c33 1282 KGain_idx_0 = EKF_DW.Xhat[0] * KGain_idx_0 + p[ixLead + 1];
fmferrari 8:339dd9885af3 1283 }
fmferrari 8:339dd9885af3 1284
fmferrari 8:339dd9885af3 1285 // ESTIMATE SYSTEM OUTPUT USING NON LINEAR OCV(Z)
fmferrari 9:17c258c67c33 1286 // '<S1>:1:105' C=[dOCVatZ(z,OCV_data,Xhat(1)) -1];
fmferrari 8:339dd9885af3 1287 // FIND THE NUMERICAL DERIVATIVE AT A SPECIFIC POINT BY INTERPOLATING
fmferrari 9:17c258c67c33 1288 // '<S1>:1:125' dOCV=dOCV_Num(SOC_spacing,OCV_data);
fmferrari 8:339dd9885af3 1289 // THIS FUNCTION RETURNS THE NUMERICAL DERIVATIVE OF SOME DATA Y (OCV DATA)
fmferrari 8:339dd9885af3 1290 // AND ITS SPACING Z
fmferrari 9:17c258c67c33 1291 // '<S1>:1:138' OCV=OCV_data;
fmferrari 9:17c258c67c33 1292 // '<S1>:1:139' dz=z(2)-z(1);
fmferrari 9:17c258c67c33 1293 // '<S1>:1:140' dUdZ=diff(OCV)/dz;
fmferrari 8:339dd9885af3 1294 ixLead = 1;
fmferrari 8:339dd9885af3 1295 iyLead = 0;
fmferrari 9:17c258c67c33 1296 work = 0.21410000000000018;
fmferrari 8:339dd9885af3 1297 for (high_i = 0; high_i < 20; high_i++) {
fmferrari 8:339dd9885af3 1298 C_idx_0 = work;
fmferrari 8:339dd9885af3 1299 work = d[ixLead];
fmferrari 8:339dd9885af3 1300 C_idx_0 = d[ixLead] - C_idx_0;
fmferrari 8:339dd9885af3 1301 ixLead++;
fmferrari 8:339dd9885af3 1302 b_y1[iyLead] = C_idx_0;
fmferrari 8:339dd9885af3 1303 iyLead++;
fmferrari 8:339dd9885af3 1304 }
fmferrari 8:339dd9885af3 1305
fmferrari 8:339dd9885af3 1306 // FIND THE NUMERICAL DERIVATIVE
fmferrari 9:17c258c67c33 1307 // '<S1>:1:142' dOCV_data=([dUdZ(1) dUdZ]+ [dUdZ dUdZ(end)])/2;
fmferrari 8:339dd9885af3 1308 for (ixLead = 0; ixLead < 20; ixLead++) {
fmferrari 8:339dd9885af3 1309 work = b_y1[ixLead] / 0.05;
fmferrari 8:339dd9885af3 1310 b_y1_0[ixLead + 1] = work;
fmferrari 8:339dd9885af3 1311 b_y1_1[ixLead] = work;
fmferrari 8:339dd9885af3 1312 b_y1[ixLead] = work;
fmferrari 8:339dd9885af3 1313 }
fmferrari 8:339dd9885af3 1314
fmferrari 8:339dd9885af3 1315 b_y1_0[0] = b_y1[0];
fmferrari 8:339dd9885af3 1316 b_y1_1[20] = b_y1[19];
fmferrari 8:339dd9885af3 1317 for (ixLead = 0; ixLead < 21; ixLead++) {
fmferrari 8:339dd9885af3 1318 dOCV[ixLead] = (b_y1_0[ixLead] + b_y1_1[ixLead]) / 2.0;
fmferrari 8:339dd9885af3 1319 }
fmferrari 8:339dd9885af3 1320
fmferrari 8:339dd9885af3 1321 // GET THE WHOLE VECTOR OF THE DERIVATIVE
fmferrari 9:17c258c67c33 1322 // '<S1>:1:127' dOCVz=interp1(SOC_spacing,dOCV,z);
fmferrari 8:339dd9885af3 1323 work = (rtNaN);
fmferrari 9:17c258c67c33 1324 if ((!rtIsNaN(EKF_DW.Xhat[0])) && (!(EKF_DW.Xhat[0] > 1.0)) && (!(EKF_DW.Xhat
fmferrari 8:339dd9885af3 1325 [0] < 0.0))) {
fmferrari 8:339dd9885af3 1326 ixLead = 0;
fmferrari 8:339dd9885af3 1327 iyLead = 2;
fmferrari 8:339dd9885af3 1328 high_i = 21;
fmferrari 8:339dd9885af3 1329 while (high_i > iyLead) {
fmferrari 8:339dd9885af3 1330 mid_i = ((ixLead + high_i) + 1) >> 1;
fmferrari 9:17c258c67c33 1331 if (EKF_DW.Xhat[0] >= e[mid_i - 1]) {
fmferrari 8:339dd9885af3 1332 ixLead = mid_i - 1;
fmferrari 8:339dd9885af3 1333 iyLead = mid_i + 1;
fmferrari 8:339dd9885af3 1334 } else {
fmferrari 8:339dd9885af3 1335 high_i = mid_i;
fmferrari 8:339dd9885af3 1336 }
fmferrari 8:339dd9885af3 1337 }
fmferrari 8:339dd9885af3 1338
fmferrari 9:17c258c67c33 1339 work = (EKF_DW.Xhat[0] - e[ixLead]) / (e[ixLead + 1] - e[ixLead]);
fmferrari 8:339dd9885af3 1340 if (work == 0.0) {
fmferrari 8:339dd9885af3 1341 work = dOCV[ixLead];
fmferrari 8:339dd9885af3 1342 } else if (work == 1.0) {
fmferrari 8:339dd9885af3 1343 work = dOCV[ixLead + 1];
fmferrari 8:339dd9885af3 1344 } else if (dOCV[ixLead + 1] == dOCV[ixLead]) {
fmferrari 8:339dd9885af3 1345 work = dOCV[ixLead];
fmferrari 8:339dd9885af3 1346 } else {
fmferrari 8:339dd9885af3 1347 work = (1.0 - work) * dOCV[ixLead] + dOCV[ixLead + 1] * work;
fmferrari 8:339dd9885af3 1348 }
fmferrari 8:339dd9885af3 1349 }
fmferrari 8:339dd9885af3 1350
fmferrari 8:339dd9885af3 1351 C_idx_0 = work;
fmferrari 8:339dd9885af3 1352
fmferrari 8:339dd9885af3 1353 // FIND C MATRIX FOR GAIN CALCULATION
fmferrari 9:17c258c67c33 1354 // '<S1>:1:106' KGain=P0*C'*(C*P0*C'+SigmaV)^-1;
fmferrari 8:339dd9885af3 1355 work = 1.0 / (((work * EKF_DW.P0[0] + -EKF_DW.P0[1]) * work + -(work *
fmferrari 8:339dd9885af3 1356 EKF_DW.P0[2] + -EKF_DW.P0[3])) + 1.0E-5);
fmferrari 8:339dd9885af3 1357
fmferrari 8:339dd9885af3 1358 // KALMAN FILTER GAIN CALCULATION
fmferrari 8:339dd9885af3 1359 // STATE ESTIMATE MEASURMENT UPDATE
fmferrari 9:17c258c67c33 1360 // '<S1>:1:109' Xhat=Xhat+KGain*(Ytrue - Yhat);
fmferrari 9:17c258c67c33 1361 Ve = (EKF_U.Voltage + Ve) - ((KGain_idx_0 - EKF_DW.Xhat[1]) - 0.00116225 * A);
fmferrari 8:339dd9885af3 1362
fmferrari 9:17c258c67c33 1363 // '<S1>:1:111' P0=P0-KGain*(C*P0*C'+SigmaV)*KGain';
fmferrari 9:17c258c67c33 1364 KGain_idx_0 = (EKF_DW.P0[0] * C_idx_0 + -EKF_DW.P0[2]) * work;
fmferrari 9:17c258c67c33 1365 EKF_DW.Xhat[0] += KGain_idx_0 * Ve;
fmferrari 8:339dd9885af3 1366 work *= EKF_DW.P0[1] * C_idx_0 + -EKF_DW.P0[3];
fmferrari 9:17c258c67c33 1367 EKF_DW.Xhat[1] += work * Ve;
fmferrari 8:339dd9885af3 1368 Ve = (C_idx_0 * EKF_DW.P0[0] + -EKF_DW.P0[1]) * C_idx_0 + -(C_idx_0 *
fmferrari 8:339dd9885af3 1369 EKF_DW.P0[2] + -EKF_DW.P0[3]);
fmferrari 9:17c258c67c33 1370 EKF_DW.P0[0] -= (Ve + 1.0E-5) * KGain_idx_0 * KGain_idx_0;
fmferrari 9:17c258c67c33 1371 EKF_DW.P0[2] -= (Ve + 1.0E-5) * KGain_idx_0 * work;
fmferrari 9:17c258c67c33 1372 EKF_DW.P0[1] -= (Ve + 1.0E-5) * work * KGain_idx_0;
fmferrari 8:339dd9885af3 1373 EKF_DW.P0[3] -= (Ve + 1.0E-5) * work * work;
fmferrari 8:339dd9885af3 1374
fmferrari 9:17c258c67c33 1375 // '<S1>:1:114' Current_prev=Current;
fmferrari 9:17c258c67c33 1376 EKF_DW.Current_prev = A;
fmferrari 9:17c258c67c33 1377
fmferrari 8:339dd9885af3 1378
fmferrari 8:339dd9885af3 1379
fmferrari 9:17c258c67c33 1380 // '<S1>:1:121' SOC=Xhat(1);
fmferrari 9:17c258c67c33 1381 EKF_Y.SOC = EKF_DW.Xhat[0];
fmferrari 6:cb71171a7108 1382 }
fmferrari 6:cb71171a7108 1383
fmferrari 6:cb71171a7108 1384 // Model initialize function
fmferrari 7:e0be546a9112 1385 void EKF_initialize(void)
fmferrari 6:cb71171a7108 1386 {
fmferrari 6:cb71171a7108 1387 // Registration code
fmferrari 6:cb71171a7108 1388
fmferrari 8:339dd9885af3 1389 // initialize non-finites
fmferrari 8:339dd9885af3 1390 rt_InitInfAndNaN(sizeof(real_T));
fmferrari 8:339dd9885af3 1391
fmferrari 6:cb71171a7108 1392 // initialize error status
fmferrari 7:e0be546a9112 1393 rtmSetErrorStatus(EKF_M, (NULL));
fmferrari 6:cb71171a7108 1394
fmferrari 8:339dd9885af3 1395 // states (dwork)
fmferrari 8:339dd9885af3 1396 (void) memset((void *)&EKF_DW, 0,
fmferrari 8:339dd9885af3 1397 sizeof(DW_EKF_T));
fmferrari 8:339dd9885af3 1398
fmferrari 6:cb71171a7108 1399 // external inputs
fmferrari 6:cb71171a7108 1400 (void)memset((void *)&EKF_U, 0, sizeof(ExtU_EKF_T));
fmferrari 6:cb71171a7108 1401
fmferrari 6:cb71171a7108 1402 // external outputs
fmferrari 6:cb71171a7108 1403 EKF_Y.SOC = 0.0;
fmferrari 8:339dd9885af3 1404
fmferrari 8:339dd9885af3 1405 // SystemInitialize for MATLAB Function: '<Root>/MATLAB Function'
fmferrari 9:17c258c67c33 1406 EKF_DW.Xhat[0] = 0.5;
fmferrari 9:17c258c67c33 1407 EKF_DW.Xhat[1] = 0.1;
fmferrari 8:339dd9885af3 1408 EKF_DW.P0[0] = 0.1;
fmferrari 8:339dd9885af3 1409 EKF_DW.P0[1] = 0.0;
fmferrari 8:339dd9885af3 1410 EKF_DW.P0[2] = 0.0;
fmferrari 8:339dd9885af3 1411 EKF_DW.P0[3] = 0.05;
fmferrari 8:339dd9885af3 1412 EKF_DW.method_not_empty = false;
fmferrari 8:339dd9885af3 1413 EKF_DW.state_not_empty = false;
fmferrari 8:339dd9885af3 1414 EKF_DW.method_c = 7UL;
fmferrari 8:339dd9885af3 1415 EKF_DW.state_p = 1144108930UL;
fmferrari 8:339dd9885af3 1416 EKF_DW.state_g[0] = 362436069UL;
fmferrari 8:339dd9885af3 1417 EKF_DW.state_g[1] = 521288629UL;
fmferrari 8:339dd9885af3 1418
fmferrari 9:17c258c67c33 1419 // '<S1>:1:62' Current_prev=0;
fmferrari 9:17c258c67c33 1420 EKF_DW.Current_prev = 0.0;
fmferrari 8:339dd9885af3 1421
fmferrari 9:17c258c67c33 1422 // '<S1>:1:67' Xhat=[0.5
fmferrari 9:17c258c67c33 1423 // '<S1>:1:68' 0.1];
fmferrari 9:17c258c67c33 1424 // '<S1>:1:72' P0=[.1 0
fmferrari 9:17c258c67c33 1425 // '<S1>:1:73' 0 0.05 ];
fmferrari 6:cb71171a7108 1426 }
fmferrari 6:cb71171a7108 1427
fmferrari 6:cb71171a7108 1428 // Model terminate function
fmferrari 7:e0be546a9112 1429 void EKF_terminate(void)
fmferrari 6:cb71171a7108 1430 {
fmferrari 6:cb71171a7108 1431 // (no terminate code required)
fmferrari 6:cb71171a7108 1432 }
fmferrari 6:cb71171a7108 1433
fmferrari 6:cb71171a7108 1434 //
fmferrari 6:cb71171a7108 1435 // File trailer for generated code.
fmferrari 6:cb71171a7108 1436 //
fmferrari 6:cb71171a7108 1437 // [EOF]
fmferrari 6:cb71171a7108 1438 //