A public repository for BMS algorithms for a NUCLEO BOARD.
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
source/Simulink/EKF.cpp@9:17c258c67c33, 2016-12-09 (annotated)
- 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?
User | Revision | Line number | New 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 | // |