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@21:534cd02b6bc4, 2017-03-03 (annotated)
- Committer:
- fmferrari
- Date:
- Fri Mar 03 19:00:02 2017 +0000
- Revision:
- 21:534cd02b6bc4
- Parent:
- 19:09279ceaa882
- Child:
- 22:caf41c9bb9d0
Version with capacity error but workling; ;
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 | 21:534cd02b6bc4 | 10 | // Model version : 1.408 |
fmferrari | 6:cb71171a7108 | 11 | // Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 |
fmferrari | 21:534cd02b6bc4 | 12 | // C/C++ source code generated on : Wed Mar 1 17:25:41 2017 |
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 | 14:4b5df635f248 | 36 | static int16_T EKF_bsearch(const real_T x[4], real_T xi); |
fmferrari | 16:a92fd07112f7 | 37 | static real_T EKF_interp2(real_T varargin_4, real_T varargin_5); |
fmferrari | 14:4b5df635f248 | 38 | static real_T EKF_interp2_dispatch(const real_T V[76], real_T Xq, real_T Yq, |
fmferrari | 14:4b5df635f248 | 39 | real_T extrapval); |
fmferrari | 18:4a769322eb39 | 40 | static void EKF_InterpolateR0(real_T SOC, real_T MeasTemp, real_T XhatParam, |
fmferrari | 18:4a769322eb39 | 41 | real_T *EstimatedT25R0, real_T *TrueT25R0, real_T *DegradationIndex); |
fmferrari | 18:4a769322eb39 | 42 | static void EKF_inv(const real_T x[9], real_T y[9]); |
fmferrari | 18:4a769322eb39 | 43 | static void EKF_mpower(const real_T a[9], real_T b, real_T c[9]); |
fmferrari | 18:4a769322eb39 | 44 | static real_T EKF_PredictTerminalVoltage(real_T Current, const real_T |
fmferrari | 18:4a769322eb39 | 45 | Power_Ahat[9], const real_T Power_Bhat[3], real_T Power_Temp, const real_T |
fmferrari | 18:4a769322eb39 | 46 | Power_Xhat_[3], real_T Power_Iterations, real_T Power_R0); |
fmferrari | 8:339dd9885af3 | 47 | |
fmferrari | 21:534cd02b6bc4 | 48 | // Function for MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 14:4b5df635f248 | 49 | static int16_T EKF_bsearch(const real_T x[4], real_T xi) |
fmferrari | 8:339dd9885af3 | 50 | { |
fmferrari | 14:4b5df635f248 | 51 | int16_T n; |
fmferrari | 14:4b5df635f248 | 52 | int16_T low_ip1; |
fmferrari | 14:4b5df635f248 | 53 | int16_T high_i; |
fmferrari | 14:4b5df635f248 | 54 | int16_T mid_i; |
fmferrari | 14:4b5df635f248 | 55 | n = 1; |
fmferrari | 14:4b5df635f248 | 56 | low_ip1 = 2; |
fmferrari | 14:4b5df635f248 | 57 | high_i = 4; |
fmferrari | 14:4b5df635f248 | 58 | while (high_i > low_ip1) { |
fmferrari | 14:4b5df635f248 | 59 | mid_i = (n + high_i) >> 1; |
fmferrari | 14:4b5df635f248 | 60 | if (xi >= x[mid_i - 1]) { |
fmferrari | 14:4b5df635f248 | 61 | n = mid_i; |
fmferrari | 14:4b5df635f248 | 62 | low_ip1 = mid_i + 1; |
fmferrari | 8:339dd9885af3 | 63 | } else { |
fmferrari | 14:4b5df635f248 | 64 | high_i = mid_i; |
fmferrari | 8:339dd9885af3 | 65 | } |
fmferrari | 8:339dd9885af3 | 66 | } |
fmferrari | 8:339dd9885af3 | 67 | |
fmferrari | 14:4b5df635f248 | 68 | return n; |
fmferrari | 8:339dd9885af3 | 69 | } |
fmferrari | 8:339dd9885af3 | 70 | |
fmferrari | 21:534cd02b6bc4 | 71 | // Function for MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 16:a92fd07112f7 | 72 | static real_T EKF_interp2(real_T varargin_4, real_T varargin_5) |
fmferrari | 16:a92fd07112f7 | 73 | { |
fmferrari | 16:a92fd07112f7 | 74 | real_T Vq; |
fmferrari | 16:a92fd07112f7 | 75 | int32_T b_y1; |
fmferrari | 16:a92fd07112f7 | 76 | int32_T y2; |
fmferrari | 16:a92fd07112f7 | 77 | int16_T low_i; |
fmferrari | 16:a92fd07112f7 | 78 | int16_T low_ip1; |
fmferrari | 16:a92fd07112f7 | 79 | int16_T high_i; |
fmferrari | 16:a92fd07112f7 | 80 | int16_T mid_i; |
fmferrari | 16:a92fd07112f7 | 81 | real_T qx1; |
fmferrari | 16:a92fd07112f7 | 82 | real_T rx; |
fmferrari | 16:a92fd07112f7 | 83 | static const real_T b[16] = { 0.05, 0.1100000000000001, 0.17000000000000004, |
fmferrari | 16:a92fd07112f7 | 84 | 0.22999999999999998, 0.29000000000000004, 0.35, 0.41000000000000003, |
fmferrari | 16:a92fd07112f7 | 85 | 0.47000000000000003, 0.53, 0.59, 0.64999999999999991, 0.71, 0.77, 0.83, 0.89, |
fmferrari | 16:a92fd07112f7 | 86 | 0.95 }; |
fmferrari | 16:a92fd07112f7 | 87 | |
fmferrari | 16:a92fd07112f7 | 88 | static const real_T V[64] = { 16576.9425034523, 20138.979107141495, |
fmferrari | 16:a92fd07112f7 | 89 | 24421.239197254181, 28529.878780245781, 15374.498069286346, |
fmferrari | 16:a92fd07112f7 | 90 | 21772.293746471405, 25693.107843399048, 32291.115447878838, |
fmferrari | 16:a92fd07112f7 | 91 | 18555.804342031479, 20658.149644732475, 25824.466645717621, |
fmferrari | 16:a92fd07112f7 | 92 | 31220.083832740784, 25359.847992658615, 27558.146119117737, |
fmferrari | 16:a92fd07112f7 | 93 | 28088.040053844452, 32270.592749118805, 23069.943785667419, |
fmferrari | 16:a92fd07112f7 | 94 | 32527.030557394028, 42899.709641933441, 47403.486669063568, |
fmferrari | 16:a92fd07112f7 | 95 | 21542.409062385559, 27543.162256479263, 34497.639834880829, |
fmferrari | 16:a92fd07112f7 | 96 | 38461.93790435791, 20651.820003986359, 25631.530284881592, |
fmferrari | 16:a92fd07112f7 | 97 | 31862.854510545731, 33635.095953941345, 19963.297843933105, |
fmferrari | 16:a92fd07112f7 | 98 | 24295.083433389664, 28222.531974315643, 30071.003437042236, |
fmferrari | 16:a92fd07112f7 | 99 | 19400.93457698822, 23581.461608409882, 28570.601940155029, |
fmferrari | 16:a92fd07112f7 | 100 | 29711.003303527832, 20319.71737742424, 24107.016921043396, |
fmferrari | 16:a92fd07112f7 | 101 | 27998.240292072296, 30197.245478630066, 27554.730474948883, |
fmferrari | 16:a92fd07112f7 | 102 | 30108.308792114258, 31208.517402410507, 34530.265778303146, |
fmferrari | 16:a92fd07112f7 | 103 | 40256.447046995163, 47065.51194190979, 48255.230188369751, |
fmferrari | 16:a92fd07112f7 | 104 | 42222.758531570435, 25791.702717542648, 26096.538305282593, |
fmferrari | 16:a92fd07112f7 | 105 | 26316.764354705811, 28954.031467437744, 20904.739797115326, |
fmferrari | 16:a92fd07112f7 | 106 | 22026.374936103821, 24867.414683103561, 29067.600816488266, |
fmferrari | 16:a92fd07112f7 | 107 | 17860.728204250336, 21127.029955387115, 24830.964803695679, |
fmferrari | 16:a92fd07112f7 | 108 | 28136.543035507202, 16835.392713546753, 21224.110424518585, |
fmferrari | 16:a92fd07112f7 | 109 | 25584.730058908463, 28471.727073192596 }; |
fmferrari | 16:a92fd07112f7 | 110 | |
fmferrari | 16:a92fd07112f7 | 111 | real_T tmp[4]; |
fmferrari | 16:a92fd07112f7 | 112 | if ((varargin_4 >= 0.05) && (varargin_4 <= 0.95) && (varargin_5 >= 15.0) && |
fmferrari | 16:a92fd07112f7 | 113 | (varargin_5 <= 45.0)) { |
fmferrari | 16:a92fd07112f7 | 114 | low_i = 0; |
fmferrari | 16:a92fd07112f7 | 115 | low_ip1 = 2; |
fmferrari | 16:a92fd07112f7 | 116 | high_i = 16; |
fmferrari | 16:a92fd07112f7 | 117 | while (high_i > low_ip1) { |
fmferrari | 16:a92fd07112f7 | 118 | mid_i = ((low_i + high_i) + 1) >> 1; |
fmferrari | 16:a92fd07112f7 | 119 | if (varargin_4 >= b[mid_i - 1]) { |
fmferrari | 16:a92fd07112f7 | 120 | low_i = mid_i - 1; |
fmferrari | 16:a92fd07112f7 | 121 | low_ip1 = mid_i + 1; |
fmferrari | 16:a92fd07112f7 | 122 | } else { |
fmferrari | 16:a92fd07112f7 | 123 | high_i = mid_i; |
fmferrari | 16:a92fd07112f7 | 124 | } |
fmferrari | 16:a92fd07112f7 | 125 | } |
fmferrari | 16:a92fd07112f7 | 126 | |
fmferrari | 16:a92fd07112f7 | 127 | tmp[0] = 15.0; |
fmferrari | 16:a92fd07112f7 | 128 | tmp[1] = 25.0; |
fmferrari | 16:a92fd07112f7 | 129 | tmp[2] = 35.0; |
fmferrari | 16:a92fd07112f7 | 130 | tmp[3] = 45.0; |
fmferrari | 16:a92fd07112f7 | 131 | low_ip1 = EKF_bsearch(tmp, varargin_5); |
fmferrari | 16:a92fd07112f7 | 132 | b_y1 = (low_ip1 - 1) * 10L + 15L; |
fmferrari | 16:a92fd07112f7 | 133 | y2 = 10L * low_ip1 + 15L; |
fmferrari | 16:a92fd07112f7 | 134 | if (varargin_4 == b[low_i]) { |
fmferrari | 16:a92fd07112f7 | 135 | qx1 = V[((low_i << 2) + low_ip1) - 1]; |
fmferrari | 16:a92fd07112f7 | 136 | Vq = V[(low_i << 2) + low_ip1]; |
fmferrari | 16:a92fd07112f7 | 137 | } else if (b[low_i + 1] == varargin_4) { |
fmferrari | 16:a92fd07112f7 | 138 | qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1]; |
fmferrari | 16:a92fd07112f7 | 139 | Vq = V[((low_i + 1) << 2) + low_ip1]; |
fmferrari | 16:a92fd07112f7 | 140 | } else { |
fmferrari | 16:a92fd07112f7 | 141 | rx = (varargin_4 - b[low_i]) / (b[low_i + 1] - b[low_i]); |
fmferrari | 16:a92fd07112f7 | 142 | if (V[(((low_i + 1) << 2) + low_ip1) - 1] == V[((low_i << 2) + low_ip1) - |
fmferrari | 16:a92fd07112f7 | 143 | 1]) { |
fmferrari | 16:a92fd07112f7 | 144 | qx1 = V[((low_i << 2) + low_ip1) - 1]; |
fmferrari | 16:a92fd07112f7 | 145 | } else { |
fmferrari | 16:a92fd07112f7 | 146 | qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1] * rx + V[((low_i << 2) + |
fmferrari | 16:a92fd07112f7 | 147 | low_ip1) - 1] * (1.0 - rx); |
fmferrari | 16:a92fd07112f7 | 148 | } |
fmferrari | 16:a92fd07112f7 | 149 | |
fmferrari | 16:a92fd07112f7 | 150 | if (V[((low_i + 1) << 2) + low_ip1] == V[(low_i << 2) + low_ip1]) { |
fmferrari | 16:a92fd07112f7 | 151 | Vq = V[(low_i << 2) + low_ip1]; |
fmferrari | 16:a92fd07112f7 | 152 | } else { |
fmferrari | 16:a92fd07112f7 | 153 | Vq = V[((low_i + 1) << 2) + low_ip1] * rx + V[(low_i << 2) + low_ip1] * |
fmferrari | 16:a92fd07112f7 | 154 | (1.0 - rx); |
fmferrari | 16:a92fd07112f7 | 155 | } |
fmferrari | 16:a92fd07112f7 | 156 | } |
fmferrari | 16:a92fd07112f7 | 157 | |
fmferrari | 16:a92fd07112f7 | 158 | if ((varargin_5 == b_y1) || (qx1 == Vq)) { |
fmferrari | 16:a92fd07112f7 | 159 | Vq = qx1; |
fmferrari | 16:a92fd07112f7 | 160 | } else { |
fmferrari | 16:a92fd07112f7 | 161 | if (!(varargin_5 == y2)) { |
fmferrari | 16:a92fd07112f7 | 162 | rx = (varargin_5 - (real_T)b_y1) / (real_T)(y2 - b_y1); |
fmferrari | 16:a92fd07112f7 | 163 | Vq = (1.0 - rx) * qx1 + rx * Vq; |
fmferrari | 16:a92fd07112f7 | 164 | } |
fmferrari | 16:a92fd07112f7 | 165 | } |
fmferrari | 16:a92fd07112f7 | 166 | } else { |
fmferrari | 16:a92fd07112f7 | 167 | Vq = (rtNaN); |
fmferrari | 16:a92fd07112f7 | 168 | } |
fmferrari | 16:a92fd07112f7 | 169 | |
fmferrari | 16:a92fd07112f7 | 170 | return Vq; |
fmferrari | 16:a92fd07112f7 | 171 | } |
fmferrari | 16:a92fd07112f7 | 172 | |
fmferrari | 21:534cd02b6bc4 | 173 | // Function for MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 14:4b5df635f248 | 174 | static real_T EKF_interp2_dispatch(const real_T V[76], real_T Xq, real_T Yq, |
fmferrari | 14:4b5df635f248 | 175 | real_T extrapval) |
fmferrari | 8:339dd9885af3 | 176 | { |
fmferrari | 14:4b5df635f248 | 177 | real_T Vq; |
fmferrari | 14:4b5df635f248 | 178 | int32_T b_y1; |
fmferrari | 14:4b5df635f248 | 179 | int32_T y2; |
fmferrari | 14:4b5df635f248 | 180 | int16_T low_i; |
fmferrari | 14:4b5df635f248 | 181 | int16_T low_ip1; |
fmferrari | 14:4b5df635f248 | 182 | int16_T high_i; |
fmferrari | 14:4b5df635f248 | 183 | int16_T mid_i; |
fmferrari | 14:4b5df635f248 | 184 | real_T qx1; |
fmferrari | 14:4b5df635f248 | 185 | real_T rx; |
fmferrari | 14:4b5df635f248 | 186 | static const real_T b[19] = { 0.05, 0.099999999999999978, 0.15000000000000002, |
fmferrari | 14:4b5df635f248 | 187 | 0.20000000000000007, 0.25000000000000011, 0.30000000000000004, 0.35, 0.4, |
fmferrari | 14:4b5df635f248 | 188 | 0.44999999999999996, 0.5, 0.55, 0.60000000000000009, 0.64999999999999991, |
fmferrari | 14:4b5df635f248 | 189 | 0.7, 0.75, 0.79999999999999993, 0.85, 0.89999999999999991, 0.95 }; |
fmferrari | 14:4b5df635f248 | 190 | |
fmferrari | 14:4b5df635f248 | 191 | real_T tmp[4]; |
fmferrari | 14:4b5df635f248 | 192 | if ((Xq >= 0.05) && (Xq <= 0.95) && (Yq >= 15.0) && (Yq <= 45.0)) { |
fmferrari | 14:4b5df635f248 | 193 | low_i = 0; |
fmferrari | 14:4b5df635f248 | 194 | low_ip1 = 2; |
fmferrari | 14:4b5df635f248 | 195 | high_i = 19; |
fmferrari | 14:4b5df635f248 | 196 | while (high_i > low_ip1) { |
fmferrari | 14:4b5df635f248 | 197 | mid_i = ((low_i + high_i) + 1) >> 1; |
fmferrari | 14:4b5df635f248 | 198 | if (Xq >= b[mid_i - 1]) { |
fmferrari | 14:4b5df635f248 | 199 | low_i = mid_i - 1; |
fmferrari | 14:4b5df635f248 | 200 | low_ip1 = mid_i + 1; |
fmferrari | 14:4b5df635f248 | 201 | } else { |
fmferrari | 14:4b5df635f248 | 202 | high_i = mid_i; |
fmferrari | 14:4b5df635f248 | 203 | } |
fmferrari | 14:4b5df635f248 | 204 | } |
fmferrari | 14:4b5df635f248 | 205 | |
fmferrari | 14:4b5df635f248 | 206 | tmp[0] = 15.0; |
fmferrari | 14:4b5df635f248 | 207 | tmp[1] = 25.0; |
fmferrari | 14:4b5df635f248 | 208 | tmp[2] = 35.0; |
fmferrari | 14:4b5df635f248 | 209 | tmp[3] = 45.0; |
fmferrari | 14:4b5df635f248 | 210 | low_ip1 = EKF_bsearch(tmp, Yq); |
fmferrari | 14:4b5df635f248 | 211 | b_y1 = (low_ip1 - 1) * 10L + 15L; |
fmferrari | 14:4b5df635f248 | 212 | y2 = 10L * low_ip1 + 15L; |
fmferrari | 14:4b5df635f248 | 213 | if (Xq == b[low_i]) { |
fmferrari | 14:4b5df635f248 | 214 | qx1 = V[((low_i << 2) + low_ip1) - 1]; |
fmferrari | 14:4b5df635f248 | 215 | Vq = V[(low_i << 2) + low_ip1]; |
fmferrari | 14:4b5df635f248 | 216 | } else if (b[low_i + 1] == Xq) { |
fmferrari | 14:4b5df635f248 | 217 | qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1]; |
fmferrari | 14:4b5df635f248 | 218 | Vq = V[((low_i + 1) << 2) + low_ip1]; |
fmferrari | 14:4b5df635f248 | 219 | } else { |
fmferrari | 14:4b5df635f248 | 220 | rx = (Xq - b[low_i]) / (b[low_i + 1] - b[low_i]); |
fmferrari | 14:4b5df635f248 | 221 | if (V[(((low_i + 1) << 2) + low_ip1) - 1] == V[((low_i << 2) + low_ip1) - |
fmferrari | 14:4b5df635f248 | 222 | 1]) { |
fmferrari | 14:4b5df635f248 | 223 | qx1 = V[((low_i << 2) + low_ip1) - 1]; |
fmferrari | 14:4b5df635f248 | 224 | } else { |
fmferrari | 14:4b5df635f248 | 225 | qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1] * rx + V[((low_i << 2) + |
fmferrari | 14:4b5df635f248 | 226 | low_ip1) - 1] * (1.0 - rx); |
fmferrari | 8:339dd9885af3 | 227 | } |
fmferrari | 8:339dd9885af3 | 228 | |
fmferrari | 14:4b5df635f248 | 229 | if (V[((low_i + 1) << 2) + low_ip1] == V[(low_i << 2) + low_ip1]) { |
fmferrari | 14:4b5df635f248 | 230 | Vq = V[(low_i << 2) + low_ip1]; |
fmferrari | 8:339dd9885af3 | 231 | } else { |
fmferrari | 14:4b5df635f248 | 232 | Vq = V[((low_i + 1) << 2) + low_ip1] * rx + V[(low_i << 2) + low_ip1] * |
fmferrari | 14:4b5df635f248 | 233 | (1.0 - rx); |
fmferrari | 8:339dd9885af3 | 234 | } |
fmferrari | 8:339dd9885af3 | 235 | } |
fmferrari | 8:339dd9885af3 | 236 | |
fmferrari | 14:4b5df635f248 | 237 | if ((Yq == b_y1) || (qx1 == Vq)) { |
fmferrari | 14:4b5df635f248 | 238 | Vq = qx1; |
fmferrari | 14:4b5df635f248 | 239 | } else { |
fmferrari | 14:4b5df635f248 | 240 | if (!(Yq == y2)) { |
fmferrari | 14:4b5df635f248 | 241 | rx = (Yq - (real_T)b_y1) / (real_T)(y2 - b_y1); |
fmferrari | 14:4b5df635f248 | 242 | Vq = (1.0 - rx) * qx1 + rx * Vq; |
fmferrari | 8:339dd9885af3 | 243 | } |
fmferrari | 8:339dd9885af3 | 244 | } |
fmferrari | 8:339dd9885af3 | 245 | } else { |
fmferrari | 14:4b5df635f248 | 246 | Vq = extrapval; |
fmferrari | 8:339dd9885af3 | 247 | } |
fmferrari | 8:339dd9885af3 | 248 | |
fmferrari | 14:4b5df635f248 | 249 | return Vq; |
fmferrari | 8:339dd9885af3 | 250 | } |
fmferrari | 8:339dd9885af3 | 251 | |
fmferrari | 18:4a769322eb39 | 252 | // |
fmferrari | 21:534cd02b6bc4 | 253 | // Function for MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 18:4a769322eb39 | 254 | // function [EstimatedT25R0,TrueT25R0,DegradationIndex]=InterpolateR0(SOC,MeasTemp,XhatParam,ParamData) |
fmferrari | 18:4a769322eb39 | 255 | // THIS SCRIPT INTERPOLATES THE DATA CORRESPONDING TO THE CORRECT SLOPE |
fmferrari | 18:4a769322eb39 | 256 | // IT NORMALIZES THE RESULT BY DETERMINING THE CORRESPONDING VALUE AT 25°C AT |
fmferrari | 18:4a769322eb39 | 257 | // THE CURRENT SOC VALUE |
fmferrari | 18:4a769322eb39 | 258 | // |
fmferrari | 18:4a769322eb39 | 259 | static void EKF_InterpolateR0(real_T SOC, real_T MeasTemp, real_T XhatParam, |
fmferrari | 18:4a769322eb39 | 260 | real_T *EstimatedT25R0, real_T *TrueT25R0, real_T *DegradationIndex) |
fmferrari | 18:4a769322eb39 | 261 | { |
fmferrari | 18:4a769322eb39 | 262 | real_T InterpSlope; |
fmferrari | 18:4a769322eb39 | 263 | int16_T low_i; |
fmferrari | 18:4a769322eb39 | 264 | int16_T low_ip1; |
fmferrari | 18:4a769322eb39 | 265 | int16_T high_i; |
fmferrari | 18:4a769322eb39 | 266 | int16_T mid_i; |
fmferrari | 18:4a769322eb39 | 267 | int32_T b_y1; |
fmferrari | 18:4a769322eb39 | 268 | int32_T y2; |
fmferrari | 18:4a769322eb39 | 269 | real_T rx; |
fmferrari | 18:4a769322eb39 | 270 | static const real_T V[72] = { 0.0017736518359183257, 0.0015094909241858197, |
fmferrari | 18:4a769322eb39 | 271 | 0.0013207048790611728, 0.00113203275348101, 0.0017734510602973419, |
fmferrari | 18:4a769322eb39 | 272 | 0.0014341246178812793, 0.0012453300124533138, 0.001094257037204736, |
fmferrari | 18:4a769322eb39 | 273 | 0.0017359800739678564, 0.0014338540487510473, 0.0012074560410535065, |
fmferrari | 18:4a769322eb39 | 274 | 0.0010943809200347151, 0.0017357180590144073, 0.0013963317986263083, |
fmferrari | 18:4a769322eb39 | 275 | 0.0011697671785970392, 0.0010565240359218181, 0.001735718059014424, |
fmferrari | 18:4a769322eb39 | 276 | 0.00139643719806763, 0.0011321181931393564, 0.00094332503207306454, |
fmferrari | 18:4a769322eb39 | 277 | 0.0016981132075471839, 0.001358593101366142, 0.0010945461407812762, |
fmferrari | 18:4a769322eb39 | 278 | 0.00094328943893145513, 0.0016604400166044017, 0.0013585418317672378, |
fmferrari | 18:4a769322eb39 | 279 | 0.0011321181931393564, 0.00090566037735849132, 0.0016603773584905675, |
fmferrari | 18:4a769322eb39 | 280 | 0.0013586956521739143, 0.0011321181931393564, 0.000943396226415091, |
fmferrari | 18:4a769322eb39 | 281 | 0.0016225190551656535, 0.0013207048790611728, 0.0010566436469300739, |
fmferrari | 18:4a769322eb39 | 282 | 0.00090552369453667447, 0.001584666465439172, 0.0012830188679245377, |
fmferrari | 18:4a769322eb39 | 283 | 0.0010942983283649642, 0.00094336062790083058, 0.0015850851039740279, |
fmferrari | 18:4a769322eb39 | 284 | 0.0013205055649877435, 0.0010565639032489348, 0.00090559203079012987, |
fmferrari | 18:4a769322eb39 | 285 | 0.0015849654703951025, 0.0012830672855579553, 0.0010563247444071386, |
fmferrari | 18:4a769322eb39 | 286 | 0.00090552369453667447, 0.001584786053882719, 0.0012451890423364244, |
fmferrari | 18:4a769322eb39 | 287 | 0.00098116910072079113, 0.00086789177766877213, 0.0015470530525998011, |
fmferrari | 18:4a769322eb39 | 288 | 0.0012075927393486558, 0.00098116910072079113, 0.00083015735255272615, |
fmferrari | 18:4a769322eb39 | 289 | 0.0015093200513168832, 0.0012075927393486558, 0.00098109505301685975, |
fmferrari | 18:4a769322eb39 | 290 | 0.000867859029507212, 0.0014715870500339485, 0.0011698113207547055, |
fmferrari | 18:4a769322eb39 | 291 | 0.00094350303807977929, 0.00083022000830219249, 0.0015094339622641524, |
fmferrari | 18:4a769322eb39 | 292 | 0.0011698996150652763, 0.0009810580333559824, 0.00083009470625966108, |
fmferrari | 18:4a769322eb39 | 293 | 0.0014718091931466412, 0.0012075927393486391, 0.00094346743150424436, |
fmferrari | 18:4a769322eb39 | 294 | 0.00083009470625967788 }; |
fmferrari | 18:4a769322eb39 | 295 | |
fmferrari | 18:4a769322eb39 | 296 | static const real_T b[18] = { 0.05, 0.10294117647058831, 0.15588235294117658, |
fmferrari | 18:4a769322eb39 | 297 | 0.20882352941176474, 0.2617647058823529, 0.31470588235294117, |
fmferrari | 18:4a769322eb39 | 298 | 0.36764705882352944, 0.4205882352941176, 0.47352941176470587, |
fmferrari | 18:4a769322eb39 | 299 | 0.52647058823529413, 0.57941176470588229, 0.63235294117647056, |
fmferrari | 18:4a769322eb39 | 300 | 0.68529411764705883, 0.7382352941176471, 0.79117647058823526, |
fmferrari | 18:4a769322eb39 | 301 | 0.84411764705882353, 0.89705882352941169, 0.95 }; |
fmferrari | 18:4a769322eb39 | 302 | |
fmferrari | 18:4a769322eb39 | 303 | static const real_T c[20] = { 0.0, 0.052631578947368474, 0.10526315789473684, |
fmferrari | 18:4a769322eb39 | 304 | 0.15789473684210531, 0.21052631578947367, 0.26315789473684215, |
fmferrari | 18:4a769322eb39 | 305 | 0.31578947368421051, 0.368421052631579, 0.42105263157894735, |
fmferrari | 18:4a769322eb39 | 306 | 0.47368421052631582, 0.52631578947368429, 0.57894736842105265, |
fmferrari | 18:4a769322eb39 | 307 | 0.631578947368421, 0.68421052631578949, 0.736842105263158, |
fmferrari | 18:4a769322eb39 | 308 | 0.78947368421052633, 0.84210526315789469, 0.89473684210526316, |
fmferrari | 18:4a769322eb39 | 309 | 0.94736842105263164, 1.0 }; |
fmferrari | 18:4a769322eb39 | 310 | |
fmferrari | 18:4a769322eb39 | 311 | static const real_T d[20] = { 7.5474530734005014E-7, -2.1136432924365931E-5, |
fmferrari | 18:4a769322eb39 | 312 | -2.2263766747057817E-5, -2.151195469496965E-5, -2.2641466893070361E-5, |
fmferrari | 18:4a769322eb39 | 313 | -2.6414980857523514E-5, -2.5285182664320518E-5, -2.490762556365612E-5, |
fmferrari | 18:4a769322eb39 | 314 | -2.3775208552609869E-5, -2.4150473140180349E-5, -2.112638052174597E-5, |
fmferrari | 18:4a769322eb39 | 315 | -2.3024208812905025E-5, -2.2650678687261015E-5, -2.4147027702574732E-5, |
fmferrari | 18:4a769322eb39 | 316 | -2.3771107387690893E-5, -2.1508807517608091E-5, -2.1504094078701937E-5, |
fmferrari | 18:4a769322eb39 | 317 | -2.2268593497227676E-5, -2.1892687685052847E-5, -2.0001423964217564E-5 }; |
fmferrari | 18:4a769322eb39 | 318 | |
fmferrari | 18:4a769322eb39 | 319 | real_T tmp[4]; |
fmferrari | 18:4a769322eb39 | 320 | |
fmferrari | 19:09279ceaa882 | 321 | // IT ASSUMES IT KNOWS THE BEHAVIOUR OF THE DATA BUT NOT THE VALUE (I |
fmferrari | 19:09279ceaa882 | 322 | // CALCULATE THE ESTIMATED R0 AND I CAN USE THE KNOWN PARAMETERS TO GET THE |
fmferrari | 19:09279ceaa882 | 323 | // 25°C VALUE |
fmferrari | 21:534cd02b6bc4 | 324 | // '<S1>:1:261' zData=fliplr(ParamData.zR0); |
fmferrari | 21:534cd02b6bc4 | 325 | // '<S1>:1:262' SlopeData= fliplr(ParamData.SlopeR0); |
fmferrari | 21:534cd02b6bc4 | 326 | // '<S1>:1:264' InterpSlope=interp1(zData,SlopeData,SOC); |
fmferrari | 18:4a769322eb39 | 327 | InterpSlope = (rtNaN); |
fmferrari | 18:4a769322eb39 | 328 | if ((!rtIsNaN(SOC)) && (!((SOC > 1.0) || (SOC < 0.0)))) { |
fmferrari | 18:4a769322eb39 | 329 | low_i = 0; |
fmferrari | 18:4a769322eb39 | 330 | low_ip1 = 2; |
fmferrari | 18:4a769322eb39 | 331 | high_i = 20; |
fmferrari | 18:4a769322eb39 | 332 | while (high_i > low_ip1) { |
fmferrari | 18:4a769322eb39 | 333 | mid_i = ((low_i + high_i) + 1) >> 1; |
fmferrari | 18:4a769322eb39 | 334 | if (SOC >= c[mid_i - 1]) { |
fmferrari | 18:4a769322eb39 | 335 | low_i = mid_i - 1; |
fmferrari | 18:4a769322eb39 | 336 | low_ip1 = mid_i + 1; |
fmferrari | 18:4a769322eb39 | 337 | } else { |
fmferrari | 18:4a769322eb39 | 338 | high_i = mid_i; |
fmferrari | 18:4a769322eb39 | 339 | } |
fmferrari | 18:4a769322eb39 | 340 | } |
fmferrari | 18:4a769322eb39 | 341 | |
fmferrari | 18:4a769322eb39 | 342 | InterpSlope = (SOC - c[low_i]) / (c[low_i + 1] - c[low_i]); |
fmferrari | 18:4a769322eb39 | 343 | if (InterpSlope == 0.0) { |
fmferrari | 18:4a769322eb39 | 344 | InterpSlope = d[low_i]; |
fmferrari | 18:4a769322eb39 | 345 | } else if (InterpSlope == 1.0) { |
fmferrari | 18:4a769322eb39 | 346 | InterpSlope = d[low_i + 1]; |
fmferrari | 18:4a769322eb39 | 347 | } else if (d[low_i + 1] == d[low_i]) { |
fmferrari | 18:4a769322eb39 | 348 | InterpSlope = d[low_i]; |
fmferrari | 18:4a769322eb39 | 349 | } else { |
fmferrari | 18:4a769322eb39 | 350 | InterpSlope = (1.0 - InterpSlope) * d[low_i] + d[low_i + 1] * InterpSlope; |
fmferrari | 18:4a769322eb39 | 351 | } |
fmferrari | 18:4a769322eb39 | 352 | } |
fmferrari | 18:4a769322eb39 | 353 | |
fmferrari | 21:534cd02b6bc4 | 354 | // '<S1>:1:266' ParamOffset=XhatParam-InterpSlope*MeasTemp; |
fmferrari | 21:534cd02b6bc4 | 355 | // '<S1>:1:267' Param25Value=InterpSlope*25+ParamOffset; |
fmferrari | 18:4a769322eb39 | 356 | *EstimatedT25R0 = (XhatParam - InterpSlope * MeasTemp) + InterpSlope * 25.0; |
fmferrari | 18:4a769322eb39 | 357 | |
fmferrari | 21:534cd02b6bc4 | 358 | // '<S1>:1:270' EstimatedT25R0=Param25Value; |
fmferrari | 18:4a769322eb39 | 359 | // DETERMINE THE CORRECT R0 AT 25 VALUE |
fmferrari | 21:534cd02b6bc4 | 360 | // '<S1>:1:274' TrueT25R0=interp2(ParamData.R0d_x,ParamData.R0d_y,ParamData.R0_map,SOC,25); |
fmferrari | 18:4a769322eb39 | 361 | if ((SOC >= 0.05) && (SOC <= 0.95)) { |
fmferrari | 18:4a769322eb39 | 362 | low_i = 0; |
fmferrari | 18:4a769322eb39 | 363 | low_ip1 = 2; |
fmferrari | 18:4a769322eb39 | 364 | high_i = 18; |
fmferrari | 18:4a769322eb39 | 365 | while (high_i > low_ip1) { |
fmferrari | 18:4a769322eb39 | 366 | mid_i = ((low_i + high_i) + 1) >> 1; |
fmferrari | 18:4a769322eb39 | 367 | if (SOC >= b[mid_i - 1]) { |
fmferrari | 18:4a769322eb39 | 368 | low_i = mid_i - 1; |
fmferrari | 18:4a769322eb39 | 369 | low_ip1 = mid_i + 1; |
fmferrari | 18:4a769322eb39 | 370 | } else { |
fmferrari | 18:4a769322eb39 | 371 | high_i = mid_i; |
fmferrari | 18:4a769322eb39 | 372 | } |
fmferrari | 18:4a769322eb39 | 373 | } |
fmferrari | 18:4a769322eb39 | 374 | |
fmferrari | 18:4a769322eb39 | 375 | tmp[0] = 15.0; |
fmferrari | 18:4a769322eb39 | 376 | tmp[1] = 25.0; |
fmferrari | 18:4a769322eb39 | 377 | tmp[2] = 35.0; |
fmferrari | 18:4a769322eb39 | 378 | tmp[3] = 45.0; |
fmferrari | 18:4a769322eb39 | 379 | low_ip1 = EKF_bsearch(tmp, 25.0); |
fmferrari | 18:4a769322eb39 | 380 | b_y1 = (low_ip1 - 1) * 10L + 15L; |
fmferrari | 18:4a769322eb39 | 381 | y2 = 10L * low_ip1 + 15L; |
fmferrari | 18:4a769322eb39 | 382 | if (SOC == b[low_i]) { |
fmferrari | 18:4a769322eb39 | 383 | InterpSlope = V[((low_i << 2) + low_ip1) - 1]; |
fmferrari | 18:4a769322eb39 | 384 | *TrueT25R0 = V[(low_i << 2) + low_ip1]; |
fmferrari | 18:4a769322eb39 | 385 | } else if (b[low_i + 1] == SOC) { |
fmferrari | 18:4a769322eb39 | 386 | InterpSlope = V[(((low_i + 1) << 2) + low_ip1) - 1]; |
fmferrari | 18:4a769322eb39 | 387 | *TrueT25R0 = V[((low_i + 1) << 2) + low_ip1]; |
fmferrari | 18:4a769322eb39 | 388 | } else { |
fmferrari | 18:4a769322eb39 | 389 | rx = (SOC - b[low_i]) / (b[low_i + 1] - b[low_i]); |
fmferrari | 18:4a769322eb39 | 390 | if (V[(((low_i + 1) << 2) + low_ip1) - 1] == V[((low_i << 2) + low_ip1) - |
fmferrari | 18:4a769322eb39 | 391 | 1]) { |
fmferrari | 18:4a769322eb39 | 392 | InterpSlope = V[((low_i << 2) + low_ip1) - 1]; |
fmferrari | 18:4a769322eb39 | 393 | } else { |
fmferrari | 18:4a769322eb39 | 394 | InterpSlope = V[(((low_i + 1) << 2) + low_ip1) - 1] * rx + V[((low_i << |
fmferrari | 18:4a769322eb39 | 395 | 2) + low_ip1) - 1] * (1.0 - rx); |
fmferrari | 18:4a769322eb39 | 396 | } |
fmferrari | 18:4a769322eb39 | 397 | |
fmferrari | 18:4a769322eb39 | 398 | if (V[((low_i + 1) << 2) + low_ip1] == V[(low_i << 2) + low_ip1]) { |
fmferrari | 18:4a769322eb39 | 399 | *TrueT25R0 = V[(low_i << 2) + low_ip1]; |
fmferrari | 18:4a769322eb39 | 400 | } else { |
fmferrari | 18:4a769322eb39 | 401 | *TrueT25R0 = V[((low_i + 1) << 2) + low_ip1] * rx + V[(low_i << 2) + |
fmferrari | 18:4a769322eb39 | 402 | low_ip1] * (1.0 - rx); |
fmferrari | 18:4a769322eb39 | 403 | } |
fmferrari | 18:4a769322eb39 | 404 | } |
fmferrari | 18:4a769322eb39 | 405 | |
fmferrari | 18:4a769322eb39 | 406 | if ((25L == b_y1) || (InterpSlope == *TrueT25R0)) { |
fmferrari | 18:4a769322eb39 | 407 | *TrueT25R0 = InterpSlope; |
fmferrari | 18:4a769322eb39 | 408 | } else { |
fmferrari | 18:4a769322eb39 | 409 | if (25L != y2) { |
fmferrari | 18:4a769322eb39 | 410 | rx = (25.0 - (real_T)b_y1) / (real_T)(y2 - b_y1); |
fmferrari | 18:4a769322eb39 | 411 | *TrueT25R0 = (1.0 - rx) * InterpSlope + rx * *TrueT25R0; |
fmferrari | 18:4a769322eb39 | 412 | } |
fmferrari | 18:4a769322eb39 | 413 | } |
fmferrari | 18:4a769322eb39 | 414 | } else { |
fmferrari | 18:4a769322eb39 | 415 | *TrueT25R0 = (rtNaN); |
fmferrari | 18:4a769322eb39 | 416 | } |
fmferrari | 18:4a769322eb39 | 417 | |
fmferrari | 21:534cd02b6bc4 | 418 | // '<S1>:1:276' DegradationIndex=EstimatedT25R0/TrueT25R0; |
fmferrari | 18:4a769322eb39 | 419 | *DegradationIndex = *EstimatedT25R0 / *TrueT25R0; |
fmferrari | 18:4a769322eb39 | 420 | } |
fmferrari | 18:4a769322eb39 | 421 | |
fmferrari | 21:534cd02b6bc4 | 422 | // Function for MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 18:4a769322eb39 | 423 | static void EKF_inv(const real_T x[9], real_T y[9]) |
fmferrari | 18:4a769322eb39 | 424 | { |
fmferrari | 18:4a769322eb39 | 425 | real_T b_x[9]; |
fmferrari | 18:4a769322eb39 | 426 | int16_T p1; |
fmferrari | 18:4a769322eb39 | 427 | int16_T p2; |
fmferrari | 18:4a769322eb39 | 428 | int16_T p3; |
fmferrari | 18:4a769322eb39 | 429 | real_T absx11; |
fmferrari | 18:4a769322eb39 | 430 | real_T absx21; |
fmferrari | 18:4a769322eb39 | 431 | real_T absx31; |
fmferrari | 18:4a769322eb39 | 432 | int16_T itmp; |
fmferrari | 18:4a769322eb39 | 433 | memcpy(&b_x[0], &x[0], 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 434 | p1 = 0; |
fmferrari | 18:4a769322eb39 | 435 | p2 = 3; |
fmferrari | 18:4a769322eb39 | 436 | p3 = 6; |
fmferrari | 18:4a769322eb39 | 437 | absx11 = std::abs(x[0]); |
fmferrari | 18:4a769322eb39 | 438 | absx21 = std::abs(x[1]); |
fmferrari | 18:4a769322eb39 | 439 | absx31 = std::abs(x[2]); |
fmferrari | 18:4a769322eb39 | 440 | if ((absx21 > absx11) && (absx21 > absx31)) { |
fmferrari | 18:4a769322eb39 | 441 | p1 = 3; |
fmferrari | 18:4a769322eb39 | 442 | p2 = 0; |
fmferrari | 18:4a769322eb39 | 443 | b_x[0] = x[1]; |
fmferrari | 18:4a769322eb39 | 444 | b_x[1] = x[0]; |
fmferrari | 18:4a769322eb39 | 445 | b_x[3] = x[4]; |
fmferrari | 18:4a769322eb39 | 446 | b_x[4] = x[3]; |
fmferrari | 18:4a769322eb39 | 447 | b_x[6] = x[7]; |
fmferrari | 18:4a769322eb39 | 448 | b_x[7] = x[6]; |
fmferrari | 18:4a769322eb39 | 449 | } else { |
fmferrari | 18:4a769322eb39 | 450 | if (absx31 > absx11) { |
fmferrari | 18:4a769322eb39 | 451 | p1 = 6; |
fmferrari | 18:4a769322eb39 | 452 | p3 = 0; |
fmferrari | 18:4a769322eb39 | 453 | b_x[0] = x[2]; |
fmferrari | 18:4a769322eb39 | 454 | b_x[2] = x[0]; |
fmferrari | 18:4a769322eb39 | 455 | b_x[3] = x[5]; |
fmferrari | 18:4a769322eb39 | 456 | b_x[5] = x[3]; |
fmferrari | 18:4a769322eb39 | 457 | b_x[6] = x[8]; |
fmferrari | 18:4a769322eb39 | 458 | b_x[8] = x[6]; |
fmferrari | 18:4a769322eb39 | 459 | } |
fmferrari | 18:4a769322eb39 | 460 | } |
fmferrari | 18:4a769322eb39 | 461 | |
fmferrari | 18:4a769322eb39 | 462 | absx11 = b_x[1] / b_x[0]; |
fmferrari | 18:4a769322eb39 | 463 | b_x[1] /= b_x[0]; |
fmferrari | 18:4a769322eb39 | 464 | absx21 = b_x[2] / b_x[0]; |
fmferrari | 18:4a769322eb39 | 465 | b_x[2] /= b_x[0]; |
fmferrari | 18:4a769322eb39 | 466 | b_x[4] -= absx11 * b_x[3]; |
fmferrari | 18:4a769322eb39 | 467 | b_x[5] -= absx21 * b_x[3]; |
fmferrari | 18:4a769322eb39 | 468 | b_x[7] -= absx11 * b_x[6]; |
fmferrari | 18:4a769322eb39 | 469 | b_x[8] -= absx21 * b_x[6]; |
fmferrari | 18:4a769322eb39 | 470 | if (std::abs(b_x[5]) > std::abs(b_x[4])) { |
fmferrari | 18:4a769322eb39 | 471 | itmp = p2; |
fmferrari | 18:4a769322eb39 | 472 | p2 = p3; |
fmferrari | 18:4a769322eb39 | 473 | p3 = itmp; |
fmferrari | 18:4a769322eb39 | 474 | b_x[1] = absx21; |
fmferrari | 18:4a769322eb39 | 475 | b_x[2] = absx11; |
fmferrari | 18:4a769322eb39 | 476 | absx11 = b_x[4]; |
fmferrari | 18:4a769322eb39 | 477 | b_x[4] = b_x[5]; |
fmferrari | 18:4a769322eb39 | 478 | b_x[5] = absx11; |
fmferrari | 18:4a769322eb39 | 479 | absx11 = b_x[7]; |
fmferrari | 18:4a769322eb39 | 480 | b_x[7] = b_x[8]; |
fmferrari | 18:4a769322eb39 | 481 | b_x[8] = absx11; |
fmferrari | 18:4a769322eb39 | 482 | } |
fmferrari | 18:4a769322eb39 | 483 | |
fmferrari | 18:4a769322eb39 | 484 | absx11 = b_x[5] / b_x[4]; |
fmferrari | 18:4a769322eb39 | 485 | b_x[5] /= b_x[4]; |
fmferrari | 18:4a769322eb39 | 486 | b_x[8] -= absx11 * b_x[7]; |
fmferrari | 18:4a769322eb39 | 487 | absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8]; |
fmferrari | 18:4a769322eb39 | 488 | absx21 = -(b_x[7] * absx11 + b_x[1]) / b_x[4]; |
fmferrari | 18:4a769322eb39 | 489 | y[p1] = ((1.0 - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0]; |
fmferrari | 18:4a769322eb39 | 490 | y[p1 + 1] = absx21; |
fmferrari | 18:4a769322eb39 | 491 | y[p1 + 2] = absx11; |
fmferrari | 18:4a769322eb39 | 492 | absx11 = -b_x[5] / b_x[8]; |
fmferrari | 18:4a769322eb39 | 493 | absx21 = (1.0 - b_x[7] * absx11) / b_x[4]; |
fmferrari | 18:4a769322eb39 | 494 | y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; |
fmferrari | 18:4a769322eb39 | 495 | y[p2 + 1] = absx21; |
fmferrari | 18:4a769322eb39 | 496 | y[p2 + 2] = absx11; |
fmferrari | 18:4a769322eb39 | 497 | absx11 = 1.0 / b_x[8]; |
fmferrari | 18:4a769322eb39 | 498 | absx21 = -b_x[7] * absx11 / b_x[4]; |
fmferrari | 18:4a769322eb39 | 499 | y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; |
fmferrari | 18:4a769322eb39 | 500 | y[p3 + 1] = absx21; |
fmferrari | 18:4a769322eb39 | 501 | y[p3 + 2] = absx11; |
fmferrari | 18:4a769322eb39 | 502 | } |
fmferrari | 18:4a769322eb39 | 503 | |
fmferrari | 21:534cd02b6bc4 | 504 | // Function for MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 18:4a769322eb39 | 505 | static void EKF_mpower(const real_T a[9], real_T b, real_T c[9]) |
fmferrari | 18:4a769322eb39 | 506 | { |
fmferrari | 18:4a769322eb39 | 507 | real_T b_a[9]; |
fmferrari | 18:4a769322eb39 | 508 | real_T e; |
fmferrari | 18:4a769322eb39 | 509 | boolean_T firstmult; |
fmferrari | 18:4a769322eb39 | 510 | real_T ed2; |
fmferrari | 18:4a769322eb39 | 511 | int16_T n; |
fmferrari | 18:4a769322eb39 | 512 | real_T cBuffer[9]; |
fmferrari | 18:4a769322eb39 | 513 | real_T aBuffer[9]; |
fmferrari | 18:4a769322eb39 | 514 | boolean_T aBufferInUse; |
fmferrari | 18:4a769322eb39 | 515 | boolean_T lsb; |
fmferrari | 18:4a769322eb39 | 516 | int16_T nb; |
fmferrari | 18:4a769322eb39 | 517 | int16_T nbitson; |
fmferrari | 18:4a769322eb39 | 518 | int16_T b_n; |
fmferrari | 18:4a769322eb39 | 519 | real_T tmp[9]; |
fmferrari | 18:4a769322eb39 | 520 | real_T tmp_0[9]; |
fmferrari | 18:4a769322eb39 | 521 | real_T aBufferInUse_0[9]; |
fmferrari | 18:4a769322eb39 | 522 | real_T c_0[9]; |
fmferrari | 18:4a769322eb39 | 523 | int16_T i; |
fmferrari | 18:4a769322eb39 | 524 | real_T c_1[9]; |
fmferrari | 18:4a769322eb39 | 525 | int32_T exitg1; |
fmferrari | 18:4a769322eb39 | 526 | if (std::floor(b) == b) { |
fmferrari | 18:4a769322eb39 | 527 | if (32767.0 >= std::abs(b)) { |
fmferrari | 18:4a769322eb39 | 528 | memcpy(&b_a[0], &a[0], 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 529 | e = std::abs(b); |
fmferrari | 18:4a769322eb39 | 530 | n = (int16_T)std::abs(b); |
fmferrari | 18:4a769322eb39 | 531 | b_n = (int16_T)e; |
fmferrari | 18:4a769322eb39 | 532 | nbitson = 0; |
fmferrari | 18:4a769322eb39 | 533 | nb = -1; |
fmferrari | 18:4a769322eb39 | 534 | while (b_n > 0) { |
fmferrari | 18:4a769322eb39 | 535 | nb++; |
fmferrari | 18:4a769322eb39 | 536 | if ((b_n & 1) != 0) { |
fmferrari | 18:4a769322eb39 | 537 | nbitson++; |
fmferrari | 18:4a769322eb39 | 538 | } |
fmferrari | 18:4a769322eb39 | 539 | |
fmferrari | 18:4a769322eb39 | 540 | b_n >>= 1; |
fmferrari | 18:4a769322eb39 | 541 | } |
fmferrari | 18:4a769322eb39 | 542 | |
fmferrari | 18:4a769322eb39 | 543 | if ((int16_T)e <= 2) { |
fmferrari | 18:4a769322eb39 | 544 | if (b == 2.0) { |
fmferrari | 18:4a769322eb39 | 545 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 546 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 547 | c[i + 3 * nbitson] = 0.0; |
fmferrari | 18:4a769322eb39 | 548 | c[i + 3 * nbitson] += a[3 * nbitson] * a[i]; |
fmferrari | 18:4a769322eb39 | 549 | c[i + 3 * nbitson] += a[3 * nbitson + 1] * a[i + 3]; |
fmferrari | 18:4a769322eb39 | 550 | c[i + 3 * nbitson] += a[3 * nbitson + 2] * a[i + 6]; |
fmferrari | 18:4a769322eb39 | 551 | } |
fmferrari | 18:4a769322eb39 | 552 | } |
fmferrari | 18:4a769322eb39 | 553 | } else if (b == 1.0) { |
fmferrari | 18:4a769322eb39 | 554 | memcpy(&c[0], &a[0], 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 555 | } else if (b == -1.0) { |
fmferrari | 18:4a769322eb39 | 556 | EKF_inv(a, c); |
fmferrari | 18:4a769322eb39 | 557 | } else if (b == -2.0) { |
fmferrari | 18:4a769322eb39 | 558 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 559 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 560 | b_a[i + 3 * nbitson] = 0.0; |
fmferrari | 18:4a769322eb39 | 561 | b_a[i + 3 * nbitson] += a[3 * nbitson] * a[i]; |
fmferrari | 18:4a769322eb39 | 562 | b_a[i + 3 * nbitson] += a[3 * nbitson + 1] * a[i + 3]; |
fmferrari | 18:4a769322eb39 | 563 | b_a[i + 3 * nbitson] += a[3 * nbitson + 2] * a[i + 6]; |
fmferrari | 18:4a769322eb39 | 564 | } |
fmferrari | 18:4a769322eb39 | 565 | } |
fmferrari | 18:4a769322eb39 | 566 | |
fmferrari | 18:4a769322eb39 | 567 | EKF_inv(b_a, c); |
fmferrari | 18:4a769322eb39 | 568 | } else { |
fmferrari | 18:4a769322eb39 | 569 | memset(&c[0], 0, 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 570 | c[0] = 1.0; |
fmferrari | 18:4a769322eb39 | 571 | c[4] = 1.0; |
fmferrari | 18:4a769322eb39 | 572 | c[8] = 1.0; |
fmferrari | 18:4a769322eb39 | 573 | } |
fmferrari | 18:4a769322eb39 | 574 | } else { |
fmferrari | 18:4a769322eb39 | 575 | firstmult = true; |
fmferrari | 18:4a769322eb39 | 576 | aBufferInUse = false; |
fmferrari | 18:4a769322eb39 | 577 | lsb = ((nbitson & 1) != 0); |
fmferrari | 18:4a769322eb39 | 578 | lsb = ((lsb && (b < 0.0)) || ((!lsb) && (b >= 0.0))); |
fmferrari | 18:4a769322eb39 | 579 | for (b_n = 1; b_n <= nb; b_n++) { |
fmferrari | 18:4a769322eb39 | 580 | if ((n & 1) != 0) { |
fmferrari | 18:4a769322eb39 | 581 | if (firstmult) { |
fmferrari | 18:4a769322eb39 | 582 | firstmult = false; |
fmferrari | 18:4a769322eb39 | 583 | if (lsb) { |
fmferrari | 18:4a769322eb39 | 584 | if (aBufferInUse) { |
fmferrari | 18:4a769322eb39 | 585 | memcpy(&cBuffer[0], &aBuffer[0], 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 586 | } else { |
fmferrari | 18:4a769322eb39 | 587 | memcpy(&cBuffer[0], &b_a[0], 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 588 | } |
fmferrari | 18:4a769322eb39 | 589 | } else if (aBufferInUse) { |
fmferrari | 18:4a769322eb39 | 590 | memcpy(&c[0], &aBuffer[0], 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 591 | } else { |
fmferrari | 18:4a769322eb39 | 592 | memcpy(&c[0], &b_a[0], 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 593 | } |
fmferrari | 18:4a769322eb39 | 594 | } else { |
fmferrari | 18:4a769322eb39 | 595 | if (aBufferInUse) { |
fmferrari | 18:4a769322eb39 | 596 | if (lsb) { |
fmferrari | 18:4a769322eb39 | 597 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 598 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 599 | c[nbitson + 3 * i] = 0.0; |
fmferrari | 18:4a769322eb39 | 600 | c[nbitson + 3 * i] += aBuffer[3 * i] * cBuffer[nbitson]; |
fmferrari | 18:4a769322eb39 | 601 | c[nbitson + 3 * i] += aBuffer[3 * i + 1] * cBuffer[nbitson |
fmferrari | 18:4a769322eb39 | 602 | + 3]; |
fmferrari | 18:4a769322eb39 | 603 | c[nbitson + 3 * i] += aBuffer[3 * i + 2] * cBuffer[nbitson |
fmferrari | 18:4a769322eb39 | 604 | + 6]; |
fmferrari | 18:4a769322eb39 | 605 | } |
fmferrari | 18:4a769322eb39 | 606 | } |
fmferrari | 18:4a769322eb39 | 607 | } else { |
fmferrari | 18:4a769322eb39 | 608 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 609 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 610 | cBuffer[nbitson + 3 * i] = 0.0; |
fmferrari | 18:4a769322eb39 | 611 | cBuffer[nbitson + 3 * i] += aBuffer[3 * i] * c[nbitson]; |
fmferrari | 18:4a769322eb39 | 612 | cBuffer[nbitson + 3 * i] += aBuffer[3 * i + 1] * c[nbitson |
fmferrari | 18:4a769322eb39 | 613 | + 3]; |
fmferrari | 18:4a769322eb39 | 614 | cBuffer[nbitson + 3 * i] += aBuffer[3 * i + 2] * c[nbitson |
fmferrari | 18:4a769322eb39 | 615 | + 6]; |
fmferrari | 18:4a769322eb39 | 616 | } |
fmferrari | 18:4a769322eb39 | 617 | } |
fmferrari | 18:4a769322eb39 | 618 | } |
fmferrari | 18:4a769322eb39 | 619 | } else if (lsb) { |
fmferrari | 18:4a769322eb39 | 620 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 621 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 622 | c[nbitson + 3 * i] = 0.0; |
fmferrari | 18:4a769322eb39 | 623 | c[nbitson + 3 * i] += b_a[3 * i] * cBuffer[nbitson]; |
fmferrari | 18:4a769322eb39 | 624 | c[nbitson + 3 * i] += b_a[3 * i + 1] * cBuffer[nbitson + 3]; |
fmferrari | 18:4a769322eb39 | 625 | c[nbitson + 3 * i] += b_a[3 * i + 2] * cBuffer[nbitson + 6]; |
fmferrari | 18:4a769322eb39 | 626 | } |
fmferrari | 18:4a769322eb39 | 627 | } |
fmferrari | 18:4a769322eb39 | 628 | } else { |
fmferrari | 18:4a769322eb39 | 629 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 630 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 631 | cBuffer[nbitson + 3 * i] = 0.0; |
fmferrari | 18:4a769322eb39 | 632 | cBuffer[nbitson + 3 * i] += b_a[3 * i] * c[nbitson]; |
fmferrari | 18:4a769322eb39 | 633 | cBuffer[nbitson + 3 * i] += b_a[3 * i + 1] * c[nbitson + 3]; |
fmferrari | 18:4a769322eb39 | 634 | cBuffer[nbitson + 3 * i] += b_a[3 * i + 2] * c[nbitson + 6]; |
fmferrari | 18:4a769322eb39 | 635 | } |
fmferrari | 18:4a769322eb39 | 636 | } |
fmferrari | 18:4a769322eb39 | 637 | } |
fmferrari | 18:4a769322eb39 | 638 | |
fmferrari | 18:4a769322eb39 | 639 | lsb = !lsb; |
fmferrari | 18:4a769322eb39 | 640 | } |
fmferrari | 18:4a769322eb39 | 641 | } |
fmferrari | 18:4a769322eb39 | 642 | |
fmferrari | 18:4a769322eb39 | 643 | n >>= 1; |
fmferrari | 18:4a769322eb39 | 644 | if (aBufferInUse) { |
fmferrari | 18:4a769322eb39 | 645 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 646 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 647 | b_a[nbitson + 3 * i] = 0.0; |
fmferrari | 18:4a769322eb39 | 648 | b_a[nbitson + 3 * i] += aBuffer[3 * i] * aBuffer[nbitson]; |
fmferrari | 18:4a769322eb39 | 649 | b_a[nbitson + 3 * i] += aBuffer[3 * i + 1] * aBuffer[nbitson + 3]; |
fmferrari | 18:4a769322eb39 | 650 | b_a[nbitson + 3 * i] += aBuffer[3 * i + 2] * aBuffer[nbitson + 6]; |
fmferrari | 18:4a769322eb39 | 651 | } |
fmferrari | 18:4a769322eb39 | 652 | } |
fmferrari | 18:4a769322eb39 | 653 | } else { |
fmferrari | 18:4a769322eb39 | 654 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 655 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 656 | aBuffer[nbitson + 3 * i] = 0.0; |
fmferrari | 18:4a769322eb39 | 657 | aBuffer[nbitson + 3 * i] += b_a[3 * i] * b_a[nbitson]; |
fmferrari | 18:4a769322eb39 | 658 | aBuffer[nbitson + 3 * i] += b_a[3 * i + 1] * b_a[nbitson + 3]; |
fmferrari | 18:4a769322eb39 | 659 | aBuffer[nbitson + 3 * i] += b_a[3 * i + 2] * b_a[nbitson + 6]; |
fmferrari | 18:4a769322eb39 | 660 | } |
fmferrari | 18:4a769322eb39 | 661 | } |
fmferrari | 18:4a769322eb39 | 662 | } |
fmferrari | 18:4a769322eb39 | 663 | |
fmferrari | 18:4a769322eb39 | 664 | aBufferInUse = !aBufferInUse; |
fmferrari | 18:4a769322eb39 | 665 | } |
fmferrari | 18:4a769322eb39 | 666 | |
fmferrari | 18:4a769322eb39 | 667 | EKF_inv(aBuffer, tmp); |
fmferrari | 18:4a769322eb39 | 668 | EKF_inv(b_a, tmp_0); |
fmferrari | 18:4a769322eb39 | 669 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 670 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 671 | c_0[i + 3 * nbitson] = 0.0; |
fmferrari | 18:4a769322eb39 | 672 | c_1[i + 3 * nbitson] = 0.0; |
fmferrari | 18:4a769322eb39 | 673 | c_0[i + 3 * nbitson] += aBuffer[3 * nbitson] * c[i]; |
fmferrari | 18:4a769322eb39 | 674 | c_1[i + 3 * nbitson] += b_a[3 * nbitson] * c[i]; |
fmferrari | 18:4a769322eb39 | 675 | c_0[i + 3 * nbitson] += aBuffer[3 * nbitson + 1] * c[i + 3]; |
fmferrari | 18:4a769322eb39 | 676 | c_1[i + 3 * nbitson] += b_a[3 * nbitson + 1] * c[i + 3]; |
fmferrari | 18:4a769322eb39 | 677 | c_0[i + 3 * nbitson] += aBuffer[3 * nbitson + 2] * c[i + 6]; |
fmferrari | 18:4a769322eb39 | 678 | c_1[i + 3 * nbitson] += b_a[3 * nbitson + 2] * c[i + 6]; |
fmferrari | 18:4a769322eb39 | 679 | } |
fmferrari | 18:4a769322eb39 | 680 | } |
fmferrari | 18:4a769322eb39 | 681 | |
fmferrari | 18:4a769322eb39 | 682 | if (aBufferInUse) { |
fmferrari | 18:4a769322eb39 | 683 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 684 | aBufferInUse_0[3 * nbitson] = c_0[3 * nbitson]; |
fmferrari | 18:4a769322eb39 | 685 | aBufferInUse_0[1 + 3 * nbitson] = c_0[3 * nbitson + 1]; |
fmferrari | 18:4a769322eb39 | 686 | aBufferInUse_0[2 + 3 * nbitson] = c_0[3 * nbitson + 2]; |
fmferrari | 18:4a769322eb39 | 687 | } |
fmferrari | 18:4a769322eb39 | 688 | } else { |
fmferrari | 18:4a769322eb39 | 689 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 690 | aBufferInUse_0[3 * nbitson] = c_1[3 * nbitson]; |
fmferrari | 18:4a769322eb39 | 691 | aBufferInUse_0[1 + 3 * nbitson] = c_1[3 * nbitson + 1]; |
fmferrari | 18:4a769322eb39 | 692 | aBufferInUse_0[2 + 3 * nbitson] = c_1[3 * nbitson + 2]; |
fmferrari | 18:4a769322eb39 | 693 | } |
fmferrari | 18:4a769322eb39 | 694 | } |
fmferrari | 18:4a769322eb39 | 695 | |
fmferrari | 18:4a769322eb39 | 696 | EKF_inv(aBufferInUse_0, c_0); |
fmferrari | 18:4a769322eb39 | 697 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 698 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 699 | c_1[i + 3 * nbitson] = 0.0; |
fmferrari | 18:4a769322eb39 | 700 | aBufferInUse_0[i + 3 * nbitson] = 0.0; |
fmferrari | 18:4a769322eb39 | 701 | c_1[i + 3 * nbitson] += aBuffer[3 * nbitson] * cBuffer[i]; |
fmferrari | 18:4a769322eb39 | 702 | aBufferInUse_0[i + 3 * nbitson] += b_a[3 * nbitson] * cBuffer[i]; |
fmferrari | 18:4a769322eb39 | 703 | c_1[i + 3 * nbitson] += aBuffer[3 * nbitson + 1] * cBuffer[i + 3]; |
fmferrari | 18:4a769322eb39 | 704 | aBufferInUse_0[i + 3 * nbitson] += b_a[3 * nbitson + 1] * cBuffer[i |
fmferrari | 18:4a769322eb39 | 705 | + 3]; |
fmferrari | 18:4a769322eb39 | 706 | c_1[i + 3 * nbitson] += aBuffer[3 * nbitson + 2] * cBuffer[i + 6]; |
fmferrari | 18:4a769322eb39 | 707 | aBufferInUse_0[i + 3 * nbitson] += b_a[3 * nbitson + 2] * cBuffer[i |
fmferrari | 18:4a769322eb39 | 708 | + 6]; |
fmferrari | 18:4a769322eb39 | 709 | } |
fmferrari | 18:4a769322eb39 | 710 | } |
fmferrari | 18:4a769322eb39 | 711 | |
fmferrari | 18:4a769322eb39 | 712 | if (firstmult) { |
fmferrari | 18:4a769322eb39 | 713 | if (b < 0.0) { |
fmferrari | 18:4a769322eb39 | 714 | if (aBufferInUse) { |
fmferrari | 18:4a769322eb39 | 715 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 716 | c[3 * nbitson] = tmp[3 * nbitson]; |
fmferrari | 18:4a769322eb39 | 717 | c[1 + 3 * nbitson] = tmp[3 * nbitson + 1]; |
fmferrari | 18:4a769322eb39 | 718 | c[2 + 3 * nbitson] = tmp[3 * nbitson + 2]; |
fmferrari | 18:4a769322eb39 | 719 | } |
fmferrari | 18:4a769322eb39 | 720 | } else { |
fmferrari | 18:4a769322eb39 | 721 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 722 | c[3 * nbitson] = tmp_0[3 * nbitson]; |
fmferrari | 18:4a769322eb39 | 723 | c[1 + 3 * nbitson] = tmp_0[3 * nbitson + 1]; |
fmferrari | 18:4a769322eb39 | 724 | c[2 + 3 * nbitson] = tmp_0[3 * nbitson + 2]; |
fmferrari | 18:4a769322eb39 | 725 | } |
fmferrari | 18:4a769322eb39 | 726 | } |
fmferrari | 18:4a769322eb39 | 727 | } else if (aBufferInUse) { |
fmferrari | 18:4a769322eb39 | 728 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 729 | c[3 * nbitson] = aBuffer[3 * nbitson]; |
fmferrari | 18:4a769322eb39 | 730 | c[1 + 3 * nbitson] = aBuffer[3 * nbitson + 1]; |
fmferrari | 18:4a769322eb39 | 731 | c[2 + 3 * nbitson] = aBuffer[3 * nbitson + 2]; |
fmferrari | 18:4a769322eb39 | 732 | } |
fmferrari | 18:4a769322eb39 | 733 | } else { |
fmferrari | 18:4a769322eb39 | 734 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 735 | c[3 * nbitson] = b_a[3 * nbitson]; |
fmferrari | 18:4a769322eb39 | 736 | c[1 + 3 * nbitson] = b_a[3 * nbitson + 1]; |
fmferrari | 18:4a769322eb39 | 737 | c[2 + 3 * nbitson] = b_a[3 * nbitson + 2]; |
fmferrari | 18:4a769322eb39 | 738 | } |
fmferrari | 18:4a769322eb39 | 739 | } |
fmferrari | 18:4a769322eb39 | 740 | } else if (b < 0.0) { |
fmferrari | 18:4a769322eb39 | 741 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 742 | c[3 * nbitson] = c_0[3 * nbitson]; |
fmferrari | 18:4a769322eb39 | 743 | c[1 + 3 * nbitson] = c_0[3 * nbitson + 1]; |
fmferrari | 18:4a769322eb39 | 744 | c[2 + 3 * nbitson] = c_0[3 * nbitson + 2]; |
fmferrari | 18:4a769322eb39 | 745 | } |
fmferrari | 18:4a769322eb39 | 746 | } else if (aBufferInUse) { |
fmferrari | 18:4a769322eb39 | 747 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 748 | c[3 * nbitson] = c_1[3 * nbitson]; |
fmferrari | 18:4a769322eb39 | 749 | c[1 + 3 * nbitson] = c_1[3 * nbitson + 1]; |
fmferrari | 18:4a769322eb39 | 750 | c[2 + 3 * nbitson] = c_1[3 * nbitson + 2]; |
fmferrari | 18:4a769322eb39 | 751 | } |
fmferrari | 18:4a769322eb39 | 752 | } else { |
fmferrari | 18:4a769322eb39 | 753 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 754 | c[3 * nbitson] = aBufferInUse_0[3 * nbitson]; |
fmferrari | 18:4a769322eb39 | 755 | c[1 + 3 * nbitson] = aBufferInUse_0[3 * nbitson + 1]; |
fmferrari | 18:4a769322eb39 | 756 | c[2 + 3 * nbitson] = aBufferInUse_0[3 * nbitson + 2]; |
fmferrari | 18:4a769322eb39 | 757 | } |
fmferrari | 18:4a769322eb39 | 758 | } |
fmferrari | 18:4a769322eb39 | 759 | } |
fmferrari | 18:4a769322eb39 | 760 | } else { |
fmferrari | 18:4a769322eb39 | 761 | memcpy(&b_a[0], &a[0], 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 762 | if ((!rtIsInf(b)) && (!rtIsNaN(b))) { |
fmferrari | 18:4a769322eb39 | 763 | e = std::abs(b); |
fmferrari | 18:4a769322eb39 | 764 | firstmult = true; |
fmferrari | 18:4a769322eb39 | 765 | do { |
fmferrari | 18:4a769322eb39 | 766 | exitg1 = 0L; |
fmferrari | 18:4a769322eb39 | 767 | ed2 = std::floor(e / 2.0); |
fmferrari | 18:4a769322eb39 | 768 | if (2.0 * ed2 != e) { |
fmferrari | 18:4a769322eb39 | 769 | if (firstmult) { |
fmferrari | 18:4a769322eb39 | 770 | memcpy(&c[0], &b_a[0], 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 771 | firstmult = false; |
fmferrari | 18:4a769322eb39 | 772 | } else { |
fmferrari | 18:4a769322eb39 | 773 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 774 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 775 | c_0[nbitson + 3 * i] = 0.0; |
fmferrari | 18:4a769322eb39 | 776 | c_0[nbitson + 3 * i] += b_a[3 * i] * c[nbitson]; |
fmferrari | 18:4a769322eb39 | 777 | c_0[nbitson + 3 * i] += b_a[3 * i + 1] * c[nbitson + 3]; |
fmferrari | 18:4a769322eb39 | 778 | c_0[nbitson + 3 * i] += b_a[3 * i + 2] * c[nbitson + 6]; |
fmferrari | 18:4a769322eb39 | 779 | } |
fmferrari | 18:4a769322eb39 | 780 | } |
fmferrari | 18:4a769322eb39 | 781 | |
fmferrari | 18:4a769322eb39 | 782 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 783 | c[3 * nbitson] = c_0[3 * nbitson]; |
fmferrari | 18:4a769322eb39 | 784 | c[1 + 3 * nbitson] = c_0[3 * nbitson + 1]; |
fmferrari | 18:4a769322eb39 | 785 | c[2 + 3 * nbitson] = c_0[3 * nbitson + 2]; |
fmferrari | 18:4a769322eb39 | 786 | } |
fmferrari | 18:4a769322eb39 | 787 | } |
fmferrari | 18:4a769322eb39 | 788 | } |
fmferrari | 18:4a769322eb39 | 789 | |
fmferrari | 18:4a769322eb39 | 790 | if (ed2 == 0.0) { |
fmferrari | 18:4a769322eb39 | 791 | exitg1 = 1L; |
fmferrari | 18:4a769322eb39 | 792 | } else { |
fmferrari | 18:4a769322eb39 | 793 | e = ed2; |
fmferrari | 18:4a769322eb39 | 794 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 795 | for (i = 0; i < 3; i++) { |
fmferrari | 18:4a769322eb39 | 796 | cBuffer[nbitson + 3 * i] = 0.0; |
fmferrari | 18:4a769322eb39 | 797 | cBuffer[nbitson + 3 * i] += b_a[3 * i] * b_a[nbitson]; |
fmferrari | 18:4a769322eb39 | 798 | cBuffer[nbitson + 3 * i] += b_a[3 * i + 1] * b_a[nbitson + 3]; |
fmferrari | 18:4a769322eb39 | 799 | cBuffer[nbitson + 3 * i] += b_a[3 * i + 2] * b_a[nbitson + 6]; |
fmferrari | 18:4a769322eb39 | 800 | } |
fmferrari | 18:4a769322eb39 | 801 | } |
fmferrari | 18:4a769322eb39 | 802 | |
fmferrari | 18:4a769322eb39 | 803 | for (nbitson = 0; nbitson < 3; nbitson++) { |
fmferrari | 18:4a769322eb39 | 804 | b_a[3 * nbitson] = cBuffer[3 * nbitson]; |
fmferrari | 18:4a769322eb39 | 805 | b_a[1 + 3 * nbitson] = cBuffer[3 * nbitson + 1]; |
fmferrari | 18:4a769322eb39 | 806 | b_a[2 + 3 * nbitson] = cBuffer[3 * nbitson + 2]; |
fmferrari | 18:4a769322eb39 | 807 | } |
fmferrari | 18:4a769322eb39 | 808 | } |
fmferrari | 18:4a769322eb39 | 809 | } while (exitg1 == 0L); |
fmferrari | 18:4a769322eb39 | 810 | |
fmferrari | 18:4a769322eb39 | 811 | if (b < 0.0) { |
fmferrari | 18:4a769322eb39 | 812 | memcpy(&c_0[0], &c[0], 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 813 | EKF_inv(c_0, c); |
fmferrari | 18:4a769322eb39 | 814 | } |
fmferrari | 18:4a769322eb39 | 815 | } else { |
fmferrari | 18:4a769322eb39 | 816 | for (nbitson = 0; nbitson < 9; nbitson++) { |
fmferrari | 18:4a769322eb39 | 817 | c[nbitson] = (rtNaN); |
fmferrari | 18:4a769322eb39 | 818 | } |
fmferrari | 18:4a769322eb39 | 819 | } |
fmferrari | 18:4a769322eb39 | 820 | } |
fmferrari | 18:4a769322eb39 | 821 | } else { |
fmferrari | 18:4a769322eb39 | 822 | for (nbitson = 0; nbitson < 9; nbitson++) { |
fmferrari | 18:4a769322eb39 | 823 | c[nbitson] = (rtNaN); |
fmferrari | 18:4a769322eb39 | 824 | } |
fmferrari | 18:4a769322eb39 | 825 | } |
fmferrari | 18:4a769322eb39 | 826 | } |
fmferrari | 18:4a769322eb39 | 827 | |
fmferrari | 18:4a769322eb39 | 828 | // |
fmferrari | 21:534cd02b6bc4 | 829 | // Function for MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 18:4a769322eb39 | 830 | // function TerminalVoltage=PredictTerminalVoltage(Current,Power) |
fmferrari | 18:4a769322eb39 | 831 | // |
fmferrari | 18:4a769322eb39 | 832 | static real_T EKF_PredictTerminalVoltage(real_T Current, const real_T |
fmferrari | 18:4a769322eb39 | 833 | Power_Ahat[9], const real_T Power_Bhat[3], real_T Power_Temp, const real_T |
fmferrari | 18:4a769322eb39 | 834 | Power_Xhat_[3], real_T Power_Iterations, real_T Power_R0) |
fmferrari | 18:4a769322eb39 | 835 | { |
fmferrari | 18:4a769322eb39 | 836 | real_T TerminalVoltage; |
fmferrari | 18:4a769322eb39 | 837 | real_T PolynomialA[9]; |
fmferrari | 18:4a769322eb39 | 838 | real_T PredictedStates[3]; |
fmferrari | 18:4a769322eb39 | 839 | int16_T i; |
fmferrari | 18:4a769322eb39 | 840 | static const real_T b[76] = { 3.493, 3.464, 3.407, 3.412, 3.526, 3.495, 3.482, |
fmferrari | 18:4a769322eb39 | 841 | 3.483, 3.556, 3.531, 3.517, 3.517, 3.578, 3.561, 3.549, 3.549, 3.604, 3.583, |
fmferrari | 18:4a769322eb39 | 842 | 3.574, 3.573, 3.629, 3.612, 3.6, 3.6, 3.643, 3.636, 3.633, 3.634, 3.658, |
fmferrari | 18:4a769322eb39 | 843 | 3.651, 3.649, 3.651, 3.675, 3.668, 3.666, 3.668, 3.696, 3.688, 3.685, 3.687, |
fmferrari | 18:4a769322eb39 | 844 | 3.722, 3.713, 3.71, 3.712, 3.754, 3.744, 3.74, 3.743, 3.801, 3.787, 3.781, |
fmferrari | 18:4a769322eb39 | 845 | 3.785, 3.859, 3.85, 3.846, 3.848, 3.904, 3.898, 3.895, 3.897, 3.952, 3.948, |
fmferrari | 18:4a769322eb39 | 846 | 3.946, 3.948, 4.002, 4.001, 4.0, 4.002, 4.056, 4.057, 4.058, 4.06, 4.114, |
fmferrari | 18:4a769322eb39 | 847 | 4.118, 4.12, 4.123 }; |
fmferrari | 18:4a769322eb39 | 848 | |
fmferrari | 18:4a769322eb39 | 849 | real_T tmp[9]; |
fmferrari | 18:4a769322eb39 | 850 | int16_T i_0; |
fmferrari | 18:4a769322eb39 | 851 | real_T Current_0[3]; |
fmferrari | 18:4a769322eb39 | 852 | |
fmferrari | 21:534cd02b6bc4 | 853 | // '<S1>:1:229' LoadedData=coder.load('ParamData.mat'); |
fmferrari | 21:534cd02b6bc4 | 854 | // '<S1>:1:230' ParamData=LoadedData.ParamData; |
fmferrari | 21:534cd02b6bc4 | 855 | // '<S1>:1:232' A=Power.Ahat; |
fmferrari | 21:534cd02b6bc4 | 856 | // '<S1>:1:233' Xhat=Power.Xhat_; |
fmferrari | 21:534cd02b6bc4 | 857 | // '<S1>:1:234' TempBattInt=Power.Temp; |
fmferrari | 21:534cd02b6bc4 | 858 | // '<S1>:1:235' Iterations=Power.Iterations; |
fmferrari | 21:534cd02b6bc4 | 859 | // '<S1>:1:236' B=Power.Bhat; |
fmferrari | 21:534cd02b6bc4 | 860 | // '<S1>:1:237' R0=Power.R0; |
fmferrari | 21:534cd02b6bc4 | 861 | // '<S1>:1:239' PolynomialA=0*A; |
fmferrari | 18:4a769322eb39 | 862 | for (i_0 = 0; i_0 < 9; i_0++) { |
fmferrari | 18:4a769322eb39 | 863 | PolynomialA[i_0] = 0.0 * Power_Ahat[i_0]; |
fmferrari | 18:4a769322eb39 | 864 | } |
fmferrari | 18:4a769322eb39 | 865 | |
fmferrari | 21:534cd02b6bc4 | 866 | // '<S1>:1:241' for i=0:1:(Iterations-1) |
fmferrari | 18:4a769322eb39 | 867 | for (i = 0; i < (int16_T)((Power_Iterations - 1.0) + 1.0); i++) { |
fmferrari | 21:534cd02b6bc4 | 868 | // '<S1>:1:242' PolynomialA=PolynomialA+A^i; |
fmferrari | 18:4a769322eb39 | 869 | EKF_mpower(Power_Ahat, (real_T)i, tmp); |
fmferrari | 18:4a769322eb39 | 870 | for (i_0 = 0; i_0 < 9; i_0++) { |
fmferrari | 18:4a769322eb39 | 871 | PolynomialA[i_0] += tmp[i_0]; |
fmferrari | 18:4a769322eb39 | 872 | } |
fmferrari | 18:4a769322eb39 | 873 | } |
fmferrari | 18:4a769322eb39 | 874 | |
fmferrari | 21:534cd02b6bc4 | 875 | // '<S1>:1:245' PredictedStates=A^(Iterations)*Xhat+PolynomialA*B.*[Current Current 0]'; |
fmferrari | 18:4a769322eb39 | 876 | EKF_mpower(Power_Ahat, Power_Iterations, tmp); |
fmferrari | 18:4a769322eb39 | 877 | Current_0[0] = Current; |
fmferrari | 18:4a769322eb39 | 878 | Current_0[1] = Current; |
fmferrari | 18:4a769322eb39 | 879 | Current_0[2] = 0.0; |
fmferrari | 18:4a769322eb39 | 880 | for (i_0 = 0; i_0 < 3; i_0++) { |
fmferrari | 18:4a769322eb39 | 881 | PredictedStates[i_0] = ((tmp[i_0 + 3] * Power_Xhat_[1] + tmp[i_0] * |
fmferrari | 18:4a769322eb39 | 882 | Power_Xhat_[0]) + tmp[i_0 + 6] * Power_Xhat_[2]) + (PolynomialA[i_0 + 6] * |
fmferrari | 18:4a769322eb39 | 883 | Power_Bhat[2] + (PolynomialA[i_0 + 3] * Power_Bhat[1] + PolynomialA[i_0] * |
fmferrari | 18:4a769322eb39 | 884 | Power_Bhat[0])) * Current_0[i_0]; |
fmferrari | 18:4a769322eb39 | 885 | } |
fmferrari | 18:4a769322eb39 | 886 | |
fmferrari | 21:534cd02b6bc4 | 887 | // '<S1>:1:247' OCVatSOC= interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,PredictedStates(1),TempBattInt); |
fmferrari | 21:534cd02b6bc4 | 888 | // '<S1>:1:249' TerminalVoltage=OCVatSOC -PredictedStates(2) -R0*(Current); |
fmferrari | 18:4a769322eb39 | 889 | TerminalVoltage = (EKF_interp2_dispatch(b, PredictedStates[0], Power_Temp, |
fmferrari | 18:4a769322eb39 | 890 | (rtNaN)) - PredictedStates[1]) - Power_R0 * Current; |
fmferrari | 18:4a769322eb39 | 891 | return TerminalVoltage; |
fmferrari | 18:4a769322eb39 | 892 | } |
fmferrari | 18:4a769322eb39 | 893 | |
fmferrari | 18:4a769322eb39 | 894 | real_T rt_powd_snf(real_T u0, real_T u1) |
fmferrari | 18:4a769322eb39 | 895 | { |
fmferrari | 18:4a769322eb39 | 896 | real_T y; |
fmferrari | 18:4a769322eb39 | 897 | real_T tmp; |
fmferrari | 18:4a769322eb39 | 898 | real_T tmp_0; |
fmferrari | 18:4a769322eb39 | 899 | if (rtIsNaN(u0) || rtIsNaN(u1)) { |
fmferrari | 18:4a769322eb39 | 900 | y = (rtNaN); |
fmferrari | 18:4a769322eb39 | 901 | } else { |
fmferrari | 18:4a769322eb39 | 902 | tmp = std::abs(u0); |
fmferrari | 18:4a769322eb39 | 903 | tmp_0 = std::abs(u1); |
fmferrari | 18:4a769322eb39 | 904 | if (rtIsInf(u1)) { |
fmferrari | 18:4a769322eb39 | 905 | if (tmp == 1.0) { |
fmferrari | 18:4a769322eb39 | 906 | y = (rtNaN); |
fmferrari | 18:4a769322eb39 | 907 | } else if (tmp > 1.0) { |
fmferrari | 18:4a769322eb39 | 908 | if (u1 > 0.0) { |
fmferrari | 18:4a769322eb39 | 909 | y = (rtInf); |
fmferrari | 18:4a769322eb39 | 910 | } else { |
fmferrari | 18:4a769322eb39 | 911 | y = 0.0; |
fmferrari | 18:4a769322eb39 | 912 | } |
fmferrari | 18:4a769322eb39 | 913 | } else if (u1 > 0.0) { |
fmferrari | 18:4a769322eb39 | 914 | y = 0.0; |
fmferrari | 18:4a769322eb39 | 915 | } else { |
fmferrari | 18:4a769322eb39 | 916 | y = (rtInf); |
fmferrari | 18:4a769322eb39 | 917 | } |
fmferrari | 18:4a769322eb39 | 918 | } else if (tmp_0 == 0.0) { |
fmferrari | 18:4a769322eb39 | 919 | y = 1.0; |
fmferrari | 18:4a769322eb39 | 920 | } else if (tmp_0 == 1.0) { |
fmferrari | 18:4a769322eb39 | 921 | if (u1 > 0.0) { |
fmferrari | 18:4a769322eb39 | 922 | y = u0; |
fmferrari | 18:4a769322eb39 | 923 | } else { |
fmferrari | 18:4a769322eb39 | 924 | y = 1.0 / u0; |
fmferrari | 18:4a769322eb39 | 925 | } |
fmferrari | 18:4a769322eb39 | 926 | } else if (u1 == 2.0) { |
fmferrari | 18:4a769322eb39 | 927 | y = u0 * u0; |
fmferrari | 18:4a769322eb39 | 928 | } else if ((u1 == 0.5) && (u0 >= 0.0)) { |
fmferrari | 18:4a769322eb39 | 929 | y = std::sqrt(u0); |
fmferrari | 18:4a769322eb39 | 930 | } else if ((u0 < 0.0) && (u1 > std::floor(u1))) { |
fmferrari | 18:4a769322eb39 | 931 | y = (rtNaN); |
fmferrari | 18:4a769322eb39 | 932 | } else { |
fmferrari | 18:4a769322eb39 | 933 | y = pow(u0, u1); |
fmferrari | 18:4a769322eb39 | 934 | } |
fmferrari | 18:4a769322eb39 | 935 | } |
fmferrari | 18:4a769322eb39 | 936 | |
fmferrari | 18:4a769322eb39 | 937 | return y; |
fmferrari | 18:4a769322eb39 | 938 | } |
fmferrari | 18:4a769322eb39 | 939 | |
fmferrari | 6:cb71171a7108 | 940 | // Model step function |
fmferrari | 7:e0be546a9112 | 941 | void EKF_step(void) |
fmferrari | 6:cb71171a7108 | 942 | { |
fmferrari | 16:a92fd07112f7 | 943 | real_T C1; |
fmferrari | 14:4b5df635f248 | 944 | real_T Ahat[9]; |
fmferrari | 13:831eab218c33 | 945 | real_T Bhat[3]; |
fmferrari | 11:e39c490420d2 | 946 | real_T C[3]; |
fmferrari | 18:4a769322eb39 | 947 | real_T KGain[3]; |
fmferrari | 18:4a769322eb39 | 948 | real_T dOCVatSOC_power; |
fmferrari | 18:4a769322eb39 | 949 | real_T A; |
fmferrari | 18:4a769322eb39 | 950 | real_T PolynomialA; |
fmferrari | 14:4b5df635f248 | 951 | int32_T b_y1; |
fmferrari | 14:4b5df635f248 | 952 | int32_T y2; |
fmferrari | 14:4b5df635f248 | 953 | int16_T low_i; |
fmferrari | 14:4b5df635f248 | 954 | int16_T low_ip1; |
fmferrari | 8:339dd9885af3 | 955 | int16_T high_i; |
fmferrari | 18:4a769322eb39 | 956 | int16_T mid_i; |
fmferrari | 14:4b5df635f248 | 957 | real_T qx1; |
fmferrari | 14:4b5df635f248 | 958 | real_T rx; |
fmferrari | 14:4b5df635f248 | 959 | real_T ry; |
fmferrari | 16:a92fd07112f7 | 960 | static const real_T d[76] = { 1.2199999999999978, 1.2699999999999967, |
fmferrari | 16:a92fd07112f7 | 961 | 1.2900000000000034, 1.3100000000000012, 1.12, 1.169999999999999, 1.2, |
fmferrari | 16:a92fd07112f7 | 962 | 1.2100000000000033, 1.04, 1.0900000000000034, 1.1199999999999957, |
fmferrari | 16:a92fd07112f7 | 963 | 1.1199999999999957, 0.97999999999999776, 1.0300000000000011, |
fmferrari | 16:a92fd07112f7 | 964 | 1.0499999999999989, 1.0499999999999989, 0.92999999999999883, |
fmferrari | 16:a92fd07112f7 | 965 | 0.97999999999999776, 1.0, 1.0, 1.0299999999999967, 1.1100000000000012, |
fmferrari | 16:a92fd07112f7 | 966 | 1.1399999999999979, 1.1199999999999957, 1.0499999999999989, |
fmferrari | 16:a92fd07112f7 | 967 | 1.0599999999999978, 1.0599999999999978, 1.0499999999999989, |
fmferrari | 16:a92fd07112f7 | 968 | 0.79000000000000115, 0.73999999999999777, 0.71000000000000107, |
fmferrari | 16:a92fd07112f7 | 969 | 0.72999999999999887, 0.57999999999999785, 0.56, 0.55000000000000115, 0.56, |
fmferrari | 16:a92fd07112f7 | 970 | 0.47000000000000108, 0.4499999999999989, 0.44, 0.44, 0.38000000000000222, |
fmferrari | 16:a92fd07112f7 | 971 | 0.37000000000000333, 0.36, 0.36, 0.32, 0.32, 0.32999999999999891, |
fmferrari | 16:a92fd07112f7 | 972 | 0.34000000000000224, 0.28999999999999893, 0.38999999999999668, |
fmferrari | 16:a92fd07112f7 | 973 | 0.48999999999999888, 0.50999999999999668, 0.38999999999999668, |
fmferrari | 16:a92fd07112f7 | 974 | 0.52999999999999892, 0.59000000000000119, 0.609999999999999, |
fmferrari | 16:a92fd07112f7 | 975 | 0.51000000000000112, 0.51000000000000112, 0.51000000000000112, |
fmferrari | 16:a92fd07112f7 | 976 | 0.51000000000000112, 0.48, 0.52, 0.569999999999999, 0.56, 0.52, |
fmferrari | 16:a92fd07112f7 | 977 | 0.6599999999999977, 0.6699999999999966, 0.65999999999999781, |
fmferrari | 16:a92fd07112f7 | 978 | 0.63000000000000111, 0.670000000000001, 1.0999999999999979, |
fmferrari | 16:a92fd07112f7 | 979 | 1.0499999999999989, 1.3099999999999967, 2.8099999999999987, |
fmferrari | 16:a92fd07112f7 | 980 | 4.1499999999999968, 4.3099999999999969 }; |
fmferrari | 8:339dd9885af3 | 981 | |
fmferrari | 16:a92fd07112f7 | 982 | static const real_T e[76] = { 3.493, 3.464, 3.407, 3.412, 3.526, 3.495, 3.482, |
fmferrari | 16:a92fd07112f7 | 983 | 3.483, 3.556, 3.531, 3.517, 3.517, 3.578, 3.561, 3.549, 3.549, 3.604, 3.583, |
fmferrari | 16:a92fd07112f7 | 984 | 3.574, 3.573, 3.629, 3.612, 3.6, 3.6, 3.643, 3.636, 3.633, 3.634, 3.658, |
fmferrari | 16:a92fd07112f7 | 985 | 3.651, 3.649, 3.651, 3.675, 3.668, 3.666, 3.668, 3.696, 3.688, 3.685, 3.687, |
fmferrari | 16:a92fd07112f7 | 986 | 3.722, 3.713, 3.71, 3.712, 3.754, 3.744, 3.74, 3.743, 3.801, 3.787, 3.781, |
fmferrari | 16:a92fd07112f7 | 987 | 3.785, 3.859, 3.85, 3.846, 3.848, 3.904, 3.898, 3.895, 3.897, 3.952, 3.948, |
fmferrari | 16:a92fd07112f7 | 988 | 3.946, 3.948, 4.002, 4.001, 4.0, 4.002, 4.056, 4.057, 4.058, 4.06, 4.114, |
fmferrari | 16:a92fd07112f7 | 989 | 4.118, 4.12, 4.123 }; |
fmferrari | 14:4b5df635f248 | 990 | |
fmferrari | 16:a92fd07112f7 | 991 | static const real_T V[68] = { 0.003318750942826959, 0.0023001508295625922, |
fmferrari | 14:4b5df635f248 | 992 | 0.001810091258767631, 0.0014328808446455602, 0.0030545289991703731, |
fmferrari | 14:4b5df635f248 | 993 | 0.0021115342558727062, 0.0016216012369423278, 0.0012823414045409901, |
fmferrari | 14:4b5df635f248 | 994 | 0.0027529509371346668, 0.002036583066188943, 0.001545886433903926, |
fmferrari | 14:4b5df635f248 | 995 | 0.0012446724248481845, 0.0027524319432923596, 0.0017720469026882386, |
fmferrari | 14:4b5df635f248 | 996 | 0.0013951734539969804, 0.0011314777098891079, 0.0029413982954973922, |
fmferrari | 14:4b5df635f248 | 997 | 0.0018856539447880626, 0.0013197089099204458, 0.0010937617862261414, |
fmferrari | 14:4b5df635f248 | 998 | 0.0027527433161129741, 0.0020366598778004175, 0.001659312893615418, |
fmferrari | 14:4b5df635f248 | 999 | 0.0013955418096782679, 0.0025641025641025663, 0.0018852273584194189, |
fmferrari | 14:4b5df635f248 | 1000 | 0.0015085800490288528, 0.0012819546037252021, 0.0023759239704329357, |
fmferrari | 14:4b5df635f248 | 1001 | 0.0017728490060729568, 0.0014328268164850424, 0.0012065910033558322, |
fmferrari | 14:4b5df635f248 | 1002 | 0.0022251555723175622, 0.0016591877521776855, 0.0012822446824558853, |
fmferrari | 14:4b5df635f248 | 1003 | 0.0010559263868461751, 0.0021121713876211686, 0.0015462945502545701, |
fmferrari | 14:4b5df635f248 | 1004 | 0.0012444846702115593, 0.0010182531301855535, 0.0019607843137254919, |
fmferrari | 14:4b5df635f248 | 1005 | 0.0014709210228558553, 0.0011689731890342826, 0.00098024430704267071, |
fmferrari | 14:4b5df635f248 | 1006 | 0.0022244844097575752, 0.0015838298514216688, 0.0012444846702115593, |
fmferrari | 14:4b5df635f248 | 1007 | 0.0010559662090813104, 0.0027153416804947981, 0.0021879361726206297, |
fmferrari | 14:4b5df635f248 | 1008 | 0.0018478711769808023, 0.0015460613145292029, 0.0025639092074504211, |
fmferrari | 14:4b5df635f248 | 1009 | 0.001998717803673113, 0.0016216012369423445, 0.0013197089099204289, |
fmferrari | 14:4b5df635f248 | 1010 | 0.002413455011690175, 0.001810023002375657, 0.0014330969980389297, |
fmferrari | 14:4b5df635f248 | 1011 | 0.0011689291101055859, 0.0021869461935824378, 0.001659250320537012, |
fmferrari | 14:4b5df635f248 | 1012 | 0.0012820029410655633, 0.0010935555639352886, 0.0020366598778004175, |
fmferrari | 14:4b5df635f248 | 1013 | 0.0014707546102500505, 0.0012066820015837712, 0.0010181379388362777 }; |
fmferrari | 8:339dd9885af3 | 1014 | |
fmferrari | 16:a92fd07112f7 | 1015 | static const real_T h[17] = { 0.05, 0.10625000000000007, 0.16250000000000009, |
fmferrari | 16:a92fd07112f7 | 1016 | 0.21875, 0.275, 0.33125000000000004, 0.38749999999999996, 0.44375, 0.5, |
fmferrari | 16:a92fd07112f7 | 1017 | 0.55625, 0.6125, 0.66875, 0.725, 0.78125, 0.83749999999999991, |
fmferrari | 16:a92fd07112f7 | 1018 | 0.89374999999999993, 0.95 }; |
fmferrari | 8:339dd9885af3 | 1019 | |
fmferrari | 14:4b5df635f248 | 1020 | real_T tmp[4]; |
fmferrari | 14:4b5df635f248 | 1021 | real_T Ahat_0[3]; |
fmferrari | 14:4b5df635f248 | 1022 | real_T Ahat_1[9]; |
fmferrari | 14:4b5df635f248 | 1023 | real_T Ahat_2[9]; |
fmferrari | 13:831eab218c33 | 1024 | real_T Bhat_0[9]; |
fmferrari | 18:4a769322eb39 | 1025 | boolean_T exitg1; |
fmferrari | 18:4a769322eb39 | 1026 | boolean_T exitg2; |
fmferrari | 8:339dd9885af3 | 1027 | |
fmferrari | 21:534cd02b6bc4 | 1028 | // MATLAB Function: '<Root>/MATLAB Function' incorporates: |
fmferrari | 12:bf3e3b87224e | 1029 | // Inport: '<Root>/Current' |
fmferrari | 13:831eab218c33 | 1030 | // Inport: '<Root>/MacroTime' |
fmferrari | 14:4b5df635f248 | 1031 | // Inport: '<Root>/TemperatureIn' |
fmferrari | 14:4b5df635f248 | 1032 | // Inport: '<Root>/TimeStep' |
fmferrari | 6:cb71171a7108 | 1033 | // Inport: '<Root>/Voltage' |
fmferrari | 6:cb71171a7108 | 1034 | |
fmferrari | 13:831eab218c33 | 1035 | // DECIDE WHAT CAPACITY TO USE IN BATTERY SIMULATION AND WHAT IN ESTIMATION |
fmferrari | 21:534cd02b6bc4 | 1036 | // MATLAB Function 'MATLAB Function': '<S1>:1' |
fmferrari | 13:831eab218c33 | 1037 | // DECIDE ON PROCESS AND MEASUREMENT NOISE COVARIANCES |
fmferrari | 15:b39f568faa27 | 1038 | // '<S1>:1:8' Q=1; |
fmferrari | 13:831eab218c33 | 1039 | // PROCESS NOISE (CURRENT) |
fmferrari | 15:b39f568faa27 | 1040 | // '<S1>:1:9' R=0.01; |
fmferrari | 13:831eab218c33 | 1041 | // SENSOR NOISE (VOLTAGE) THIS NEEDS TO BE FAIRLY LARGE TO ALLOW TO CONVERGE |
fmferrari | 13:831eab218c33 | 1042 | // '<S1>:1:12' InitialSOC=0.8; |
fmferrari | 14:4b5df635f248 | 1043 | // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
fmferrari | 14:4b5df635f248 | 1044 | // INITIALISE ESTIMATION VECTORS AND P0 MATRICES |
fmferrari | 21:534cd02b6bc4 | 1045 | // '<S1>:1:22' LoadedData=coder.load('ParamData.mat'); |
fmferrari | 21:534cd02b6bc4 | 1046 | // '<S1>:1:23' ParamData=LoadedData.ParamData; |
fmferrari | 21:534cd02b6bc4 | 1047 | // '<S1>:1:26' if isempty(Count) |
fmferrari | 13:831eab218c33 | 1048 | if (!EKF_DW.Count_not_empty) { |
fmferrari | 21:534cd02b6bc4 | 1049 | // '<S1>:1:27' Count=0; |
fmferrari | 13:831eab218c33 | 1050 | EKF_DW.Count = 0.0; |
fmferrari | 13:831eab218c33 | 1051 | EKF_DW.Count_not_empty = true; |
fmferrari | 13:831eab218c33 | 1052 | |
fmferrari | 21:534cd02b6bc4 | 1053 | // '<S1>:1:29' Xhat=[0.5 0.1 -6.76]'; |
fmferrari | 21:534cd02b6bc4 | 1054 | // '<S1>:1:30' P0=[0.1 0 0; |
fmferrari | 21:534cd02b6bc4 | 1055 | // '<S1>:1:31' 0 0.05 0; |
fmferrari | 21:534cd02b6bc4 | 1056 | // '<S1>:1:32' 0 0 .1]; |
fmferrari | 21:534cd02b6bc4 | 1057 | // '<S1>:1:34' Qhat=53; |
fmferrari | 21:534cd02b6bc4 | 1058 | // '<S1>:1:35' ThetaHat=Qhat; |
fmferrari | 13:831eab218c33 | 1059 | EKF_DW.ThetaHat = EKF_DW.Qhat; |
fmferrari | 13:831eab218c33 | 1060 | |
fmferrari | 21:534cd02b6bc4 | 1061 | // '<S1>:1:36' P0th=2; |
fmferrari | 21:534cd02b6bc4 | 1062 | // '<S1>:1:37' XhatTh=InitialSOC; |
fmferrari | 21:534cd02b6bc4 | 1063 | // '<S1>:1:38' CthPrev=0; |
fmferrari | 21:534cd02b6bc4 | 1064 | // '<S1>:1:39' iQ=1; |
fmferrari | 21:534cd02b6bc4 | 1065 | // '<S1>:1:40' UpdateMacro=60*20; |
fmferrari | 21:534cd02b6bc4 | 1066 | // '<S1>:1:42' R0_filt=exp(Xhat(3)); |
fmferrari | 21:534cd02b6bc4 | 1067 | EKF_DW.R0_filt = EKF_DW.Xhat[2]; |
fmferrari | 21:534cd02b6bc4 | 1068 | EKF_DW.R0_filt = std::exp(EKF_DW.R0_filt); |
fmferrari | 21:534cd02b6bc4 | 1069 | |
fmferrari | 21:534cd02b6bc4 | 1070 | // '<S1>:1:43' R0_filt_prev=exp(Xhat(3)); |
fmferrari | 21:534cd02b6bc4 | 1071 | EKF_DW.R0_filt_prev = EKF_DW.Xhat[2]; |
fmferrari | 21:534cd02b6bc4 | 1072 | EKF_DW.R0_filt_prev = std::exp(EKF_DW.R0_filt_prev); |
fmferrari | 21:534cd02b6bc4 | 1073 | |
fmferrari | 21:534cd02b6bc4 | 1074 | // '<S1>:1:45' Current_prev=0; |
fmferrari | 21:534cd02b6bc4 | 1075 | // '<S1>:1:48' Power.iMaxDisch=424; |
fmferrari | 18:4a769322eb39 | 1076 | EKF_DW.Power.iMaxDisch = 424.0; |
fmferrari | 18:4a769322eb39 | 1077 | |
fmferrari | 18:4a769322eb39 | 1078 | // for max 10s otherwise mac is 265 |
fmferrari | 21:534cd02b6bc4 | 1079 | // '<S1>:1:49' Power.iMaxCh=-100; |
fmferrari | 18:4a769322eb39 | 1080 | EKF_DW.Power.iMaxCh = -100.0; |
fmferrari | 18:4a769322eb39 | 1081 | |
fmferrari | 21:534cd02b6bc4 | 1082 | // '<S1>:1:50' Power.MaxSOC=0.95; |
fmferrari | 18:4a769322eb39 | 1083 | EKF_DW.Power.MaxSOC = 0.95; |
fmferrari | 18:4a769322eb39 | 1084 | |
fmferrari | 21:534cd02b6bc4 | 1085 | // '<S1>:1:51' Power.MinSOC=0.05; |
fmferrari | 18:4a769322eb39 | 1086 | EKF_DW.Power.MinSOC = 0.05; |
fmferrari | 18:4a769322eb39 | 1087 | |
fmferrari | 21:534cd02b6bc4 | 1088 | // '<S1>:1:52' Power.Vmax=4.2; |
fmferrari | 18:4a769322eb39 | 1089 | EKF_DW.Power.Vmax = 4.2; |
fmferrari | 18:4a769322eb39 | 1090 | |
fmferrari | 21:534cd02b6bc4 | 1091 | // '<S1>:1:53' Power.Vmin=2.7; |
fmferrari | 18:4a769322eb39 | 1092 | EKF_DW.Power.Vmin = 2.7; |
fmferrari | 18:4a769322eb39 | 1093 | |
fmferrari | 21:534cd02b6bc4 | 1094 | // '<S1>:1:54' Power.Time=0; |
fmferrari | 18:4a769322eb39 | 1095 | EKF_DW.Power.Time = 0.0; |
fmferrari | 18:4a769322eb39 | 1096 | |
fmferrari | 21:534cd02b6bc4 | 1097 | // '<S1>:1:55' Power.Ahat=zeros(3,3); |
fmferrari | 18:4a769322eb39 | 1098 | memset(&EKF_DW.Power.Ahat[0], 0, 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 1099 | |
fmferrari | 21:534cd02b6bc4 | 1100 | // '<S1>:1:56' Power.Bhat=[0 0 0]'; |
fmferrari | 21:534cd02b6bc4 | 1101 | // '<S1>:1:57' Power.Temp=0; |
fmferrari | 18:4a769322eb39 | 1102 | EKF_DW.Power.Temp = 0.0; |
fmferrari | 18:4a769322eb39 | 1103 | |
fmferrari | 21:534cd02b6bc4 | 1104 | // '<S1>:1:58' Power.Xhat_=Xhat; |
fmferrari | 18:4a769322eb39 | 1105 | EKF_DW.Power.Bhat[0] = 0.0; |
fmferrari | 18:4a769322eb39 | 1106 | EKF_DW.Power.Xhat_[0] = EKF_DW.Xhat[0]; |
fmferrari | 18:4a769322eb39 | 1107 | EKF_DW.Power.Bhat[1] = 0.0; |
fmferrari | 18:4a769322eb39 | 1108 | EKF_DW.Power.Xhat_[1] = EKF_DW.Xhat[1]; |
fmferrari | 18:4a769322eb39 | 1109 | EKF_DW.Power.Bhat[2] = 0.0; |
fmferrari | 18:4a769322eb39 | 1110 | EKF_DW.Power.Xhat_[2] = EKF_DW.Xhat[2]; |
fmferrari | 18:4a769322eb39 | 1111 | |
fmferrari | 21:534cd02b6bc4 | 1112 | // '<S1>:1:59' Power.TimeStep=0; |
fmferrari | 18:4a769322eb39 | 1113 | EKF_DW.Power.TimeStep = 0.0; |
fmferrari | 18:4a769322eb39 | 1114 | |
fmferrari | 21:534cd02b6bc4 | 1115 | // '<S1>:1:60' Power.Iterations=0; |
fmferrari | 18:4a769322eb39 | 1116 | EKF_DW.Power.Iterations = 0.0; |
fmferrari | 18:4a769322eb39 | 1117 | |
fmferrari | 21:534cd02b6bc4 | 1118 | // '<S1>:1:61' Power.R0=0; |
fmferrari | 18:4a769322eb39 | 1119 | EKF_DW.Power.R0 = 0.0; |
fmferrari | 18:4a769322eb39 | 1120 | |
fmferrari | 21:534cd02b6bc4 | 1121 | // '<S1>:1:62' PowerDischHat=0; |
fmferrari | 21:534cd02b6bc4 | 1122 | // '<S1>:1:63' PowerChHat=0; |
fmferrari | 21:534cd02b6bc4 | 1123 | // '<S1>:1:64' iP=1; |
fmferrari | 13:831eab218c33 | 1124 | } else { |
fmferrari | 21:534cd02b6bc4 | 1125 | // '<S1>:1:66' else |
fmferrari | 21:534cd02b6bc4 | 1126 | // '<S1>:1:67' Count=Count+1; |
fmferrari | 13:831eab218c33 | 1127 | EKF_DW.Count++; |
fmferrari | 11:e39c490420d2 | 1128 | } |
fmferrari | 8:339dd9885af3 | 1129 | |
fmferrari | 21:534cd02b6bc4 | 1130 | // '<S1>:1:71' Power.TimeWindow=10; |
fmferrari | 19:09279ceaa882 | 1131 | EKF_DW.Power.TimeWindow = 10.0; |
fmferrari | 19:09279ceaa882 | 1132 | |
fmferrari | 21:534cd02b6bc4 | 1133 | // '<S1>:1:72' UpdatePower=1; |
fmferrari | 14:4b5df635f248 | 1134 | // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
fmferrari | 13:831eab218c33 | 1135 | // INITIALISE VARIABLE FOR CAPACITY ESTIMATION |
fmferrari | 13:831eab218c33 | 1136 | // IN SECONDS |
fmferrari | 21:534cd02b6bc4 | 1137 | // Qth=Qhat*1/100*4; |
fmferrari | 21:534cd02b6bc4 | 1138 | // '<S1>:1:80' Qth=5; |
fmferrari | 21:534cd02b6bc4 | 1139 | // '<S1>:1:81' Rth=1e-6; |
fmferrari | 21:534cd02b6bc4 | 1140 | // SENSOR NOISE |
fmferrari | 14:4b5df635f248 | 1141 | // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
fmferrari | 14:4b5df635f248 | 1142 | // INTERPOLATE PARAMETRES |
fmferrari | 21:534cd02b6bc4 | 1143 | // '<S1>:1:85' C1=interp2(ParamData.C1d_x,ParamData.C1d_y,ParamData.C1_map,Xhat(1),TemperatureIn); |
fmferrari | 16:a92fd07112f7 | 1144 | C1 = EKF_interp2(EKF_DW.Xhat[0], EKF_U.TemperatureIn); |
fmferrari | 16:a92fd07112f7 | 1145 | |
fmferrari | 21:534cd02b6bc4 | 1146 | // '<S1>:1:86' R1=interp2(ParamData.R1d_x,ParamData.R1d_y,ParamData.R1_map,Xhat(1),TemperatureIn); |
fmferrari | 14:4b5df635f248 | 1147 | if ((EKF_DW.Xhat[0] >= 0.05) && (EKF_DW.Xhat[0] <= 0.95) && |
fmferrari | 14:4b5df635f248 | 1148 | ((EKF_U.TemperatureIn >= 15.0) && (EKF_U.TemperatureIn <= 45.0))) { |
fmferrari | 14:4b5df635f248 | 1149 | low_i = 0; |
fmferrari | 14:4b5df635f248 | 1150 | low_ip1 = 2; |
fmferrari | 16:a92fd07112f7 | 1151 | high_i = 17; |
fmferrari | 14:4b5df635f248 | 1152 | while (high_i > low_ip1) { |
fmferrari | 18:4a769322eb39 | 1153 | mid_i = ((low_i + high_i) + 1) >> 1; |
fmferrari | 18:4a769322eb39 | 1154 | if (EKF_DW.Xhat[0] >= h[mid_i - 1]) { |
fmferrari | 18:4a769322eb39 | 1155 | low_i = mid_i - 1; |
fmferrari | 18:4a769322eb39 | 1156 | low_ip1 = mid_i + 1; |
fmferrari | 14:4b5df635f248 | 1157 | } else { |
fmferrari | 18:4a769322eb39 | 1158 | high_i = mid_i; |
fmferrari | 14:4b5df635f248 | 1159 | } |
fmferrari | 14:4b5df635f248 | 1160 | } |
fmferrari | 14:4b5df635f248 | 1161 | |
fmferrari | 16:a92fd07112f7 | 1162 | tmp[0] = 15.0; |
fmferrari | 16:a92fd07112f7 | 1163 | tmp[1] = 25.0; |
fmferrari | 16:a92fd07112f7 | 1164 | tmp[2] = 35.0; |
fmferrari | 16:a92fd07112f7 | 1165 | tmp[3] = 45.0; |
fmferrari | 16:a92fd07112f7 | 1166 | low_ip1 = EKF_bsearch(tmp, EKF_U.TemperatureIn); |
fmferrari | 14:4b5df635f248 | 1167 | b_y1 = (low_ip1 - 1) * 10L + 15L; |
fmferrari | 14:4b5df635f248 | 1168 | y2 = 10L * low_ip1 + 15L; |
fmferrari | 16:a92fd07112f7 | 1169 | if (EKF_DW.Xhat[0] == h[low_i]) { |
fmferrari | 14:4b5df635f248 | 1170 | qx1 = V[((low_i << 2) + low_ip1) - 1]; |
fmferrari | 14:4b5df635f248 | 1171 | rx = V[(low_i << 2) + low_ip1]; |
fmferrari | 16:a92fd07112f7 | 1172 | } else if (h[low_i + 1] == EKF_DW.Xhat[0]) { |
fmferrari | 14:4b5df635f248 | 1173 | qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1]; |
fmferrari | 14:4b5df635f248 | 1174 | rx = V[((low_i + 1) << 2) + low_ip1]; |
fmferrari | 14:4b5df635f248 | 1175 | } else { |
fmferrari | 16:a92fd07112f7 | 1176 | rx = (EKF_DW.Xhat[0] - h[low_i]) / (h[low_i + 1] - h[low_i]); |
fmferrari | 14:4b5df635f248 | 1177 | if (V[(((low_i + 1) << 2) + low_ip1) - 1] == V[((low_i << 2) + low_ip1) - |
fmferrari | 14:4b5df635f248 | 1178 | 1]) { |
fmferrari | 14:4b5df635f248 | 1179 | qx1 = V[((low_i << 2) + low_ip1) - 1]; |
fmferrari | 14:4b5df635f248 | 1180 | } else { |
fmferrari | 14:4b5df635f248 | 1181 | qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1] * rx + V[((low_i << 2) + |
fmferrari | 14:4b5df635f248 | 1182 | low_ip1) - 1] * (1.0 - rx); |
fmferrari | 14:4b5df635f248 | 1183 | } |
fmferrari | 14:4b5df635f248 | 1184 | |
fmferrari | 14:4b5df635f248 | 1185 | if (V[((low_i + 1) << 2) + low_ip1] == V[(low_i << 2) + low_ip1]) { |
fmferrari | 14:4b5df635f248 | 1186 | rx = V[(low_i << 2) + low_ip1]; |
fmferrari | 14:4b5df635f248 | 1187 | } else { |
fmferrari | 14:4b5df635f248 | 1188 | rx = V[((low_i + 1) << 2) + low_ip1] * rx + V[(low_i << 2) + low_ip1] * |
fmferrari | 14:4b5df635f248 | 1189 | (1.0 - rx); |
fmferrari | 14:4b5df635f248 | 1190 | } |
fmferrari | 14:4b5df635f248 | 1191 | } |
fmferrari | 14:4b5df635f248 | 1192 | |
fmferrari | 14:4b5df635f248 | 1193 | if ((EKF_U.TemperatureIn == b_y1) || (qx1 == rx)) { |
fmferrari | 14:4b5df635f248 | 1194 | rx = qx1; |
fmferrari | 14:4b5df635f248 | 1195 | } else { |
fmferrari | 14:4b5df635f248 | 1196 | if (!(EKF_U.TemperatureIn == y2)) { |
fmferrari | 14:4b5df635f248 | 1197 | ry = (EKF_U.TemperatureIn - (real_T)b_y1) / (real_T)(y2 - b_y1); |
fmferrari | 14:4b5df635f248 | 1198 | rx = (1.0 - ry) * qx1 + ry * rx; |
fmferrari | 14:4b5df635f248 | 1199 | } |
fmferrari | 14:4b5df635f248 | 1200 | } |
fmferrari | 14:4b5df635f248 | 1201 | } else { |
fmferrari | 14:4b5df635f248 | 1202 | rx = (rtNaN); |
fmferrari | 14:4b5df635f248 | 1203 | } |
fmferrari | 14:4b5df635f248 | 1204 | |
fmferrari | 21:534cd02b6bc4 | 1205 | // '<S1>:1:87' tau=C1*R1; |
fmferrari | 18:4a769322eb39 | 1206 | qx1 = C1 * rx; |
fmferrari | 18:4a769322eb39 | 1207 | |
fmferrari | 14:4b5df635f248 | 1208 | // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
fmferrari | 13:831eab218c33 | 1209 | // DISCRETE TIME SS MATRICES |
fmferrari | 21:534cd02b6bc4 | 1210 | // '<S1>:1:91' Ahat=[1 0 0; |
fmferrari | 21:534cd02b6bc4 | 1211 | // '<S1>:1:92' 0 exp(-TimeStep/tau) 0; |
fmferrari | 21:534cd02b6bc4 | 1212 | // '<S1>:1:93' 0 0 1;]; |
fmferrari | 14:4b5df635f248 | 1213 | Ahat[1] = 0.0; |
fmferrari | 18:4a769322eb39 | 1214 | Ahat[4] = std::exp(-EKF_U.TimeStep / qx1); |
fmferrari | 14:4b5df635f248 | 1215 | Ahat[7] = 0.0; |
fmferrari | 14:4b5df635f248 | 1216 | Ahat[0] = 1.0; |
fmferrari | 14:4b5df635f248 | 1217 | Ahat[2] = 0.0; |
fmferrari | 14:4b5df635f248 | 1218 | Ahat[3] = 0.0; |
fmferrari | 14:4b5df635f248 | 1219 | Ahat[5] = 0.0; |
fmferrari | 14:4b5df635f248 | 1220 | Ahat[6] = 0.0; |
fmferrari | 14:4b5df635f248 | 1221 | Ahat[8] = 1.0; |
fmferrari | 14:4b5df635f248 | 1222 | |
fmferrari | 21:534cd02b6bc4 | 1223 | // '<S1>:1:96' Bhat=[-TimeStep/(Qhat*3600) |
fmferrari | 21:534cd02b6bc4 | 1224 | // '<S1>:1:97' TimeStep/C1 |
fmferrari | 21:534cd02b6bc4 | 1225 | // '<S1>:1:98' 1]; |
fmferrari | 14:4b5df635f248 | 1226 | Bhat[0] = -EKF_U.TimeStep / (EKF_DW.Qhat * 3600.0); |
fmferrari | 16:a92fd07112f7 | 1227 | Bhat[1] = EKF_U.TimeStep / C1; |
fmferrari | 13:831eab218c33 | 1228 | Bhat[2] = 1.0; |
fmferrari | 13:831eab218c33 | 1229 | |
fmferrari | 14:4b5df635f248 | 1230 | // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
fmferrari | 13:831eab218c33 | 1231 | // PREDICTION STAGE |
fmferrari | 14:4b5df635f248 | 1232 | // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
fmferrari | 13:831eab218c33 | 1233 | // SOC ESTIMATION FOR CAPACITY ESTIMATION |
fmferrari | 21:534cd02b6bc4 | 1234 | // '<S1>:1:105' if MacroTime>5*60 |
fmferrari | 21:534cd02b6bc4 | 1235 | if (EKF_U.MacroTime > 300.0) { |
fmferrari | 21:534cd02b6bc4 | 1236 | // '<S1>:1:106' XhatTh=XhatTh-TimeStep/(Qhat*3600)*Current_prev; |
fmferrari | 21:534cd02b6bc4 | 1237 | EKF_DW.XhatTh -= EKF_U.TimeStep / (EKF_DW.Qhat * 3600.0) * |
fmferrari | 21:534cd02b6bc4 | 1238 | EKF_DW.Current_prev; |
fmferrari | 21:534cd02b6bc4 | 1239 | } else { |
fmferrari | 21:534cd02b6bc4 | 1240 | // '<S1>:1:107' else |
fmferrari | 21:534cd02b6bc4 | 1241 | // '<S1>:1:108' XhatTh=Xhat(1); |
fmferrari | 21:534cd02b6bc4 | 1242 | EKF_DW.XhatTh = EKF_DW.Xhat[0]; |
fmferrari | 21:534cd02b6bc4 | 1243 | } |
fmferrari | 13:831eab218c33 | 1244 | |
fmferrari | 21:534cd02b6bc4 | 1245 | // '<S1>:1:112' if MacroTime>UpdateMacro*iQ |
fmferrari | 21:534cd02b6bc4 | 1246 | if (EKF_U.MacroTime > EKF_DW.UpdateMacro * EKF_DW.iQ) { |
fmferrari | 13:831eab218c33 | 1247 | // MACROSCALE EKF FOR CAPACITY ESTIMATION |
fmferrari | 21:534cd02b6bc4 | 1248 | // '<S1>:1:114' P0th=P0th+Qth; |
fmferrari | 21:534cd02b6bc4 | 1249 | EKF_DW.P0th += 5.0; |
fmferrari | 13:831eab218c33 | 1250 | |
fmferrari | 21:534cd02b6bc4 | 1251 | // '<S1>:1:115' Cth=+Current_prev*TimeStep/(ThetaHat^2)+ CthPrev; |
fmferrari | 18:4a769322eb39 | 1252 | rx = EKF_DW.Current_prev * EKF_U.TimeStep / (EKF_DW.ThetaHat * |
fmferrari | 14:4b5df635f248 | 1253 | EKF_DW.ThetaHat) + EKF_DW.CthPrev; |
fmferrari | 13:831eab218c33 | 1254 | |
fmferrari | 21:534cd02b6bc4 | 1255 | // '<S1>:1:116' KGainTh=P0th*(Cth')*(Cth*P0th*Cth'+Rth)^-1; |
fmferrari | 21:534cd02b6bc4 | 1256 | ry = 1.0 / (rx * EKF_DW.P0th * rx + 1.0E-6) * (EKF_DW.P0th * rx); |
fmferrari | 13:831eab218c33 | 1257 | |
fmferrari | 21:534cd02b6bc4 | 1258 | // '<S1>:1:117' ThetaHat=ThetaHat+KGainTh*(Xhat(1)-XhatTh(1)); |
fmferrari | 18:4a769322eb39 | 1259 | EKF_DW.ThetaHat += (EKF_DW.Xhat[0] - EKF_DW.XhatTh) * ry; |
fmferrari | 13:831eab218c33 | 1260 | |
fmferrari | 21:534cd02b6bc4 | 1261 | // '<S1>:1:118' P0th=P0th-KGainTh*Cth*KGainTh'; |
fmferrari | 18:4a769322eb39 | 1262 | EKF_DW.P0th -= ry * rx * ry; |
fmferrari | 13:831eab218c33 | 1263 | |
fmferrari | 21:534cd02b6bc4 | 1264 | // '<S1>:1:119' CthPrev=Cth; |
fmferrari | 18:4a769322eb39 | 1265 | EKF_DW.CthPrev = rx; |
fmferrari | 13:831eab218c33 | 1266 | |
fmferrari | 21:534cd02b6bc4 | 1267 | // '<S1>:1:120' Qhat=ThetaHat |
fmferrari | 13:831eab218c33 | 1268 | EKF_DW.Qhat = EKF_DW.ThetaHat; |
fmferrari | 13:831eab218c33 | 1269 | |
fmferrari | 13:831eab218c33 | 1270 | // THIS HAS TO BE DONE TO AVOID OVERSHOOTING |
fmferrari | 21:534cd02b6bc4 | 1271 | // '<S1>:1:122' XhatTh=Xhat(1); |
fmferrari | 13:831eab218c33 | 1272 | EKF_DW.XhatTh = EKF_DW.Xhat[0]; |
fmferrari | 13:831eab218c33 | 1273 | |
fmferrari | 21:534cd02b6bc4 | 1274 | // '<S1>:1:123' iQ=iQ+1; |
fmferrari | 13:831eab218c33 | 1275 | EKF_DW.iQ++; |
fmferrari | 13:831eab218c33 | 1276 | } |
fmferrari | 13:831eab218c33 | 1277 | |
fmferrari | 14:4b5df635f248 | 1278 | // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
fmferrari | 14:4b5df635f248 | 1279 | // PREDICTION FOR STATES |
fmferrari | 21:534cd02b6bc4 | 1280 | // '<S1>:1:129' Xhat=Ahat*Xhat+Bhat.*[Current_prev Current_prev 0]'; |
fmferrari | 18:4a769322eb39 | 1281 | KGain[0] = EKF_DW.Current_prev; |
fmferrari | 18:4a769322eb39 | 1282 | KGain[1] = EKF_DW.Current_prev; |
fmferrari | 18:4a769322eb39 | 1283 | KGain[2] = 0.0; |
fmferrari | 14:4b5df635f248 | 1284 | for (low_i = 0; low_i < 3; low_i++) { |
fmferrari | 14:4b5df635f248 | 1285 | Ahat_0[low_i] = ((Ahat[low_i + 3] * EKF_DW.Xhat[1] + Ahat[low_i] * |
fmferrari | 14:4b5df635f248 | 1286 | EKF_DW.Xhat[0]) + Ahat[low_i + 6] * EKF_DW.Xhat[2]) + |
fmferrari | 18:4a769322eb39 | 1287 | Bhat[low_i] * KGain[low_i]; |
fmferrari | 13:831eab218c33 | 1288 | } |
fmferrari | 13:831eab218c33 | 1289 | |
fmferrari | 21:534cd02b6bc4 | 1290 | // '<S1>:1:130' P0=Ahat*P0*Ahat' + Bhat*Q*Bhat'; |
fmferrari | 14:4b5df635f248 | 1291 | for (low_i = 0; low_i < 3; low_i++) { |
fmferrari | 14:4b5df635f248 | 1292 | EKF_DW.Xhat[low_i] = Ahat_0[low_i]; |
fmferrari | 14:4b5df635f248 | 1293 | for (low_ip1 = 0; low_ip1 < 3; low_ip1++) { |
fmferrari | 14:4b5df635f248 | 1294 | Ahat_1[low_i + 3 * low_ip1] = 0.0; |
fmferrari | 14:4b5df635f248 | 1295 | Ahat_1[low_i + 3 * low_ip1] += EKF_DW.P0[3 * low_ip1] * Ahat[low_i]; |
fmferrari | 14:4b5df635f248 | 1296 | Ahat_1[low_i + 3 * low_ip1] += EKF_DW.P0[3 * low_ip1 + 1] * Ahat[low_i + 3]; |
fmferrari | 14:4b5df635f248 | 1297 | Ahat_1[low_i + 3 * low_ip1] += EKF_DW.P0[3 * low_ip1 + 2] * Ahat[low_i + 6]; |
fmferrari | 13:831eab218c33 | 1298 | } |
fmferrari | 13:831eab218c33 | 1299 | |
fmferrari | 14:4b5df635f248 | 1300 | for (low_ip1 = 0; low_ip1 < 3; low_ip1++) { |
fmferrari | 14:4b5df635f248 | 1301 | Ahat_2[low_i + 3 * low_ip1] = 0.0; |
fmferrari | 14:4b5df635f248 | 1302 | Ahat_2[low_i + 3 * low_ip1] += Ahat_1[low_i] * Ahat[low_ip1]; |
fmferrari | 14:4b5df635f248 | 1303 | Ahat_2[low_i + 3 * low_ip1] += Ahat_1[low_i + 3] * Ahat[low_ip1 + 3]; |
fmferrari | 14:4b5df635f248 | 1304 | Ahat_2[low_i + 3 * low_ip1] += Ahat_1[low_i + 6] * Ahat[low_ip1 + 6]; |
fmferrari | 15:b39f568faa27 | 1305 | Bhat_0[low_i + 3 * low_ip1] = Bhat[low_i] * Bhat[low_ip1]; |
fmferrari | 11:e39c490420d2 | 1306 | } |
fmferrari | 8:339dd9885af3 | 1307 | } |
fmferrari | 8:339dd9885af3 | 1308 | |
fmferrari | 21:534cd02b6bc4 | 1309 | // '<S1>:1:133' Ytrue=Voltage; |
fmferrari | 13:831eab218c33 | 1310 | // ESTIMATION STAGE |
fmferrari | 14:4b5df635f248 | 1311 | // INTERPOLATE CORRECT VALUES |
fmferrari | 21:534cd02b6bc4 | 1312 | // '<S1>:1:139' OCVatSOC=interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,Xhat(1),TemperatureIn); |
fmferrari | 21:534cd02b6bc4 | 1313 | // '<S1>:1:140' dOCVatSOC=interp2(ParamData.dOCVdatad_x,ParamData.dOCVdatad_y,ParamData.dOCVdata_map,Xhat(1),TemperatureIn); |
fmferrari | 21:534cd02b6bc4 | 1314 | // '<S1>:1:143' Yhat=OCVatSOC -Xhat(2) -exp(Xhat(3))*Current; |
fmferrari | 21:534cd02b6bc4 | 1315 | // '<S1>:1:144' C=[dOCVatSOC -1 -exp(Xhat(3))*Current]; |
fmferrari | 18:4a769322eb39 | 1316 | ry = EKF_interp2_dispatch(d, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN)); |
fmferrari | 16:a92fd07112f7 | 1317 | C[0] = EKF_interp2_dispatch(d, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN)); |
fmferrari | 11:e39c490420d2 | 1318 | C[1] = -1.0; |
fmferrari | 13:831eab218c33 | 1319 | C[2] = -std::exp(EKF_DW.Xhat[2]) * EKF_U.Current; |
fmferrari | 8:339dd9885af3 | 1320 | |
fmferrari | 21:534cd02b6bc4 | 1321 | // '<S1>:1:145' KGain=P0*C'*(C*P0*C'+R)^-1; |
fmferrari | 18:4a769322eb39 | 1322 | dOCVatSOC_power = 0.0; |
fmferrari | 14:4b5df635f248 | 1323 | for (low_i = 0; low_i < 3; low_i++) { |
fmferrari | 14:4b5df635f248 | 1324 | EKF_DW.P0[3 * low_i] = Ahat_2[3 * low_i] + Bhat_0[3 * low_i]; |
fmferrari | 18:4a769322eb39 | 1325 | rx = EKF_DW.P0[3 * low_i] * ry; |
fmferrari | 14:4b5df635f248 | 1326 | EKF_DW.P0[1 + 3 * low_i] = Ahat_2[3 * low_i + 1] + Bhat_0[3 * low_i + 1]; |
fmferrari | 18:4a769322eb39 | 1327 | rx += -EKF_DW.P0[3 * low_i + 1]; |
fmferrari | 14:4b5df635f248 | 1328 | EKF_DW.P0[2 + 3 * low_i] = Ahat_2[3 * low_i + 2] + Bhat_0[3 * low_i + 2]; |
fmferrari | 18:4a769322eb39 | 1329 | rx += EKF_DW.P0[3 * low_i + 2] * C[2]; |
fmferrari | 18:4a769322eb39 | 1330 | dOCVatSOC_power += rx * C[low_i]; |
fmferrari | 14:4b5df635f248 | 1331 | } |
fmferrari | 14:4b5df635f248 | 1332 | |
fmferrari | 18:4a769322eb39 | 1333 | rx = 1.0 / (dOCVatSOC_power + 0.01); |
fmferrari | 14:4b5df635f248 | 1334 | for (low_i = 0; low_i < 3; low_i++) { |
fmferrari | 18:4a769322eb39 | 1335 | KGain[low_i] = (EKF_DW.P0[low_i + 6] * C[2] + (-EKF_DW.P0[low_i + 3] + |
fmferrari | 18:4a769322eb39 | 1336 | EKF_DW.P0[low_i] * ry)) * rx; |
fmferrari | 14:4b5df635f248 | 1337 | } |
fmferrari | 14:4b5df635f248 | 1338 | |
fmferrari | 21:534cd02b6bc4 | 1339 | // '<S1>:1:146' Xhat=Xhat+KGain*(Ytrue-Yhat); |
fmferrari | 18:4a769322eb39 | 1340 | rx = EKF_U.Voltage - ((EKF_interp2_dispatch(e, EKF_DW.Xhat[0], |
fmferrari | 14:4b5df635f248 | 1341 | EKF_U.TemperatureIn, (rtNaN)) - EKF_DW.Xhat[1]) - std::exp(EKF_DW.Xhat[2]) * |
fmferrari | 14:4b5df635f248 | 1342 | EKF_U.Current); |
fmferrari | 14:4b5df635f248 | 1343 | |
fmferrari | 21:534cd02b6bc4 | 1344 | // '<S1>:1:147' P0=P0-KGain*(C*P0*C'+R)*KGain'; |
fmferrari | 18:4a769322eb39 | 1345 | dOCVatSOC_power = 0.0; |
fmferrari | 14:4b5df635f248 | 1346 | for (low_i = 0; low_i < 3; low_i++) { |
fmferrari | 18:4a769322eb39 | 1347 | EKF_DW.Xhat[low_i] += KGain[low_i] * rx; |
fmferrari | 18:4a769322eb39 | 1348 | dOCVatSOC_power += (EKF_DW.P0[3 * low_i + 2] * C[2] + (-EKF_DW.P0[3 * low_i |
fmferrari | 18:4a769322eb39 | 1349 | + 1] + EKF_DW.P0[3 * low_i] * ry)) * C[low_i]; |
fmferrari | 11:e39c490420d2 | 1350 | } |
fmferrari | 11:e39c490420d2 | 1351 | |
fmferrari | 14:4b5df635f248 | 1352 | for (low_i = 0; low_i < 3; low_i++) { |
fmferrari | 18:4a769322eb39 | 1353 | EKF_DW.P0[low_i] -= (dOCVatSOC_power + 0.01) * KGain[low_i] * KGain[0]; |
fmferrari | 18:4a769322eb39 | 1354 | EKF_DW.P0[low_i + 3] -= (dOCVatSOC_power + 0.01) * KGain[low_i] * KGain[1]; |
fmferrari | 18:4a769322eb39 | 1355 | EKF_DW.P0[low_i + 6] -= (dOCVatSOC_power + 0.01) * KGain[low_i] * KGain[2]; |
fmferrari | 11:e39c490420d2 | 1356 | } |
fmferrari | 11:e39c490420d2 | 1357 | |
fmferrari | 21:534cd02b6bc4 | 1358 | // R0 FILTER |
fmferrari | 21:534cd02b6bc4 | 1359 | // '<S1>:1:152' DI=Current-Current_prev; |
fmferrari | 21:534cd02b6bc4 | 1360 | // '<S1>:1:153' Current_prev=Current; |
fmferrari | 21:534cd02b6bc4 | 1361 | EKF_DW.Current_prev = EKF_U.Current; |
fmferrari | 14:4b5df635f248 | 1362 | |
fmferrari | 21:534cd02b6bc4 | 1363 | // '<S1>:1:155' alpha=0.995; |
fmferrari | 21:534cd02b6bc4 | 1364 | // '<S1>:1:156' if abs(Current)>20 |
fmferrari | 21:534cd02b6bc4 | 1365 | if (std::abs(EKF_U.Current) > 5.0) { |
fmferrari | 21:534cd02b6bc4 | 1366 | // '<S1>:1:158' R0_filt=R0_filt_prev*alpha + (1-alpha)*exp(Xhat(3)); |
fmferrari | 21:534cd02b6bc4 | 1367 | EKF_DW.R0_filt = EKF_DW.R0_filt_prev * 0.995 + 0.0050000000000000044 * std:: |
fmferrari | 21:534cd02b6bc4 | 1368 | exp(EKF_DW.Xhat[2]); |
fmferrari | 14:4b5df635f248 | 1369 | |
fmferrari | 21:534cd02b6bc4 | 1370 | // '<S1>:1:159' R0_filt_prev=R0_filt; |
fmferrari | 21:534cd02b6bc4 | 1371 | EKF_DW.R0_filt_prev = EKF_DW.R0_filt; |
fmferrari | 21:534cd02b6bc4 | 1372 | } |
fmferrari | 18:4a769322eb39 | 1373 | |
fmferrari | 21:534cd02b6bc4 | 1374 | // '<S1>:1:162' [~,~,DegradationIndex]=InterpolateR0(Xhat(1),TemperatureIn, R0_filt,ParamData); |
fmferrari | 21:534cd02b6bc4 | 1375 | EKF_InterpolateR0(EKF_DW.Xhat[0], EKF_U.TemperatureIn, EKF_DW.R0_filt, &ry, |
fmferrari | 21:534cd02b6bc4 | 1376 | &dOCVatSOC_power, &rx); |
fmferrari | 21:534cd02b6bc4 | 1377 | |
fmferrari | 21:534cd02b6bc4 | 1378 | // '<S1>:1:162' ~ |
fmferrari | 21:534cd02b6bc4 | 1379 | // POWER ESTIMATION |
fmferrari | 21:534cd02b6bc4 | 1380 | // '<S1>:1:166' if MacroTime>UpdatePower*iP |
fmferrari | 19:09279ceaa882 | 1381 | if (EKF_U.MacroTime > EKF_DW.iP) { |
fmferrari | 21:534cd02b6bc4 | 1382 | // '<S1>:1:168' Power.Ahat=Ahat; |
fmferrari | 18:4a769322eb39 | 1383 | memcpy(&EKF_DW.Power.Ahat[0], &Ahat[0], 9U * sizeof(real_T)); |
fmferrari | 18:4a769322eb39 | 1384 | |
fmferrari | 21:534cd02b6bc4 | 1385 | // '<S1>:1:169' Power.Bhat=Bhat; |
fmferrari | 21:534cd02b6bc4 | 1386 | // '<S1>:1:170' Power.Temp=TemperatureIn; |
fmferrari | 18:4a769322eb39 | 1387 | EKF_DW.Power.Temp = EKF_U.TemperatureIn; |
fmferrari | 18:4a769322eb39 | 1388 | |
fmferrari | 21:534cd02b6bc4 | 1389 | // '<S1>:1:171' Power.Xhat_=Xhat; |
fmferrari | 18:4a769322eb39 | 1390 | EKF_DW.Power.Bhat[0] = Bhat[0]; |
fmferrari | 18:4a769322eb39 | 1391 | EKF_DW.Power.Xhat_[0] = EKF_DW.Xhat[0]; |
fmferrari | 18:4a769322eb39 | 1392 | EKF_DW.Power.Bhat[1] = Bhat[1]; |
fmferrari | 18:4a769322eb39 | 1393 | EKF_DW.Power.Xhat_[1] = EKF_DW.Xhat[1]; |
fmferrari | 18:4a769322eb39 | 1394 | EKF_DW.Power.Bhat[2] = 1.0; |
fmferrari | 18:4a769322eb39 | 1395 | EKF_DW.Power.Xhat_[2] = EKF_DW.Xhat[2]; |
fmferrari | 18:4a769322eb39 | 1396 | |
fmferrari | 21:534cd02b6bc4 | 1397 | // '<S1>:1:172' Power.TimeStep=TimeStep; |
fmferrari | 18:4a769322eb39 | 1398 | EKF_DW.Power.TimeStep = EKF_U.TimeStep; |
fmferrari | 18:4a769322eb39 | 1399 | |
fmferrari | 21:534cd02b6bc4 | 1400 | // '<S1>:1:173' Power.Iterations=50; |
fmferrari | 21:534cd02b6bc4 | 1401 | EKF_DW.Power.Iterations = 50.0; |
fmferrari | 18:4a769322eb39 | 1402 | |
fmferrari | 21:534cd02b6bc4 | 1403 | // 50 iterations at 0.2s time step is 10seconds |
fmferrari | 21:534cd02b6bc4 | 1404 | // '<S1>:1:174' Power.R0=R0_filt; |
fmferrari | 21:534cd02b6bc4 | 1405 | EKF_DW.Power.R0 = EKF_DW.R0_filt; |
fmferrari | 18:4a769322eb39 | 1406 | |
fmferrari | 21:534cd02b6bc4 | 1407 | // '<S1>:1:176' iChSOC=(Xhat(1)-Power.MaxSOC)*(Qhat*3600)/(Power.TimeWindow); |
fmferrari | 21:534cd02b6bc4 | 1408 | // '<S1>:1:177' iDischSOC=(Xhat(1)-Power.MinSOC)*(Qhat*3600)/(Power.TimeWindow); |
fmferrari | 21:534cd02b6bc4 | 1409 | // '<S1>:1:179' OCVatSOC_power=interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,Xhat(1),TemperatureIn); |
fmferrari | 18:4a769322eb39 | 1410 | ry = EKF_interp2_dispatch(e, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN)); |
fmferrari | 18:4a769322eb39 | 1411 | |
fmferrari | 21:534cd02b6bc4 | 1412 | // '<S1>:1:180' dOCVatSOC_power=interp2(ParamData.dOCVdatad_x,ParamData.dOCVdatad_y,ParamData.dOCVdata_map,Xhat(1),TemperatureIn); |
fmferrari | 18:4a769322eb39 | 1413 | dOCVatSOC_power = EKF_interp2_dispatch(d, EKF_DW.Xhat[0], |
fmferrari | 18:4a769322eb39 | 1414 | EKF_U.TemperatureIn, (rtNaN)); |
fmferrari | 18:4a769322eb39 | 1415 | |
fmferrari | 21:534cd02b6bc4 | 1416 | // '<S1>:1:182' A=exp(-Power.TimeWindow/tau); |
fmferrari | 18:4a769322eb39 | 1417 | A = std::exp(-EKF_DW.Power.TimeWindow / qx1); |
fmferrari | 18:4a769322eb39 | 1418 | |
fmferrari | 21:534cd02b6bc4 | 1419 | // '<S1>:1:183' PolynomialA=0; |
fmferrari | 18:4a769322eb39 | 1420 | PolynomialA = 0.0; |
fmferrari | 18:4a769322eb39 | 1421 | |
fmferrari | 21:534cd02b6bc4 | 1422 | // '<S1>:1:184' for i=0:1:(Power.Iterations-1) |
fmferrari | 18:4a769322eb39 | 1423 | for (low_i = 0; low_i < (int16_T)((EKF_DW.Power.Iterations - 1.0) + 1.0); |
fmferrari | 18:4a769322eb39 | 1424 | low_i++) { |
fmferrari | 21:534cd02b6bc4 | 1425 | // '<S1>:1:185' PolynomialA=PolynomialA+A^i; |
fmferrari | 18:4a769322eb39 | 1426 | PolynomialA += rt_powd_snf(A, (real_T)low_i); |
fmferrari | 18:4a769322eb39 | 1427 | } |
fmferrari | 18:4a769322eb39 | 1428 | |
fmferrari | 21:534cd02b6bc4 | 1429 | // '<S1>:1:188' iDischOCV=(OCVatSOC_power-Xhat(2)*exp(-Power.TimeWindow/tau)-Power.Vmin)/... |
fmferrari | 21:534cd02b6bc4 | 1430 | // '<S1>:1:189' ((Power.TimeWindow/(Qhat*3600))*dOCVatSOC_power+... |
fmferrari | 21:534cd02b6bc4 | 1431 | // '<S1>:1:190' (Power.TimeStep/C1)*PolynomialA+... |
fmferrari | 21:534cd02b6bc4 | 1432 | // '<S1>:1:191' Power.R0); |
fmferrari | 21:534cd02b6bc4 | 1433 | // '<S1>:1:193' iChOCV=(OCVatSOC_power-Xhat(2)*exp(-Power.TimeWindow/tau)-Power.Vmax)/... |
fmferrari | 21:534cd02b6bc4 | 1434 | // '<S1>:1:194' ((Power.TimeWindow/(Qhat*3600))*dOCVatSOC_power+... |
fmferrari | 21:534cd02b6bc4 | 1435 | // '<S1>:1:195' (Power.TimeStep/C1)*PolynomialA+... |
fmferrari | 21:534cd02b6bc4 | 1436 | // '<S1>:1:196' Power.R0); |
fmferrari | 21:534cd02b6bc4 | 1437 | // '<S1>:1:198' IMaxDisch=min([Power.iMaxDisch; iDischOCV; iDischSOC]); |
fmferrari | 18:4a769322eb39 | 1438 | Bhat[0] = EKF_DW.Power.iMaxDisch; |
fmferrari | 18:4a769322eb39 | 1439 | Bhat[1] = ((ry - std::exp(-EKF_DW.Power.TimeWindow / qx1) * EKF_DW.Xhat[1]) |
fmferrari | 18:4a769322eb39 | 1440 | - EKF_DW.Power.Vmin) / ((EKF_DW.Power.TimeWindow / (EKF_DW.Qhat * |
fmferrari | 18:4a769322eb39 | 1441 | 3600.0) * dOCVatSOC_power + EKF_DW.Power.TimeStep / C1 * PolynomialA) + |
fmferrari | 18:4a769322eb39 | 1442 | EKF_DW.Power.R0); |
fmferrari | 18:4a769322eb39 | 1443 | Bhat[2] = (EKF_DW.Xhat[0] - EKF_DW.Power.MinSOC) * (EKF_DW.Qhat * 3600.0) / |
fmferrari | 18:4a769322eb39 | 1444 | EKF_DW.Power.TimeWindow; |
fmferrari | 18:4a769322eb39 | 1445 | low_i = 1; |
fmferrari | 18:4a769322eb39 | 1446 | A = EKF_DW.Power.iMaxDisch; |
fmferrari | 18:4a769322eb39 | 1447 | if (rtIsNaN(EKF_DW.Power.iMaxDisch)) { |
fmferrari | 18:4a769322eb39 | 1448 | low_ip1 = 2; |
fmferrari | 18:4a769322eb39 | 1449 | exitg2 = false; |
fmferrari | 18:4a769322eb39 | 1450 | while ((!exitg2) && (low_ip1 < 4)) { |
fmferrari | 18:4a769322eb39 | 1451 | low_i = low_ip1; |
fmferrari | 18:4a769322eb39 | 1452 | if (!rtIsNaN(Bhat[low_ip1 - 1])) { |
fmferrari | 18:4a769322eb39 | 1453 | A = Bhat[low_ip1 - 1]; |
fmferrari | 18:4a769322eb39 | 1454 | exitg2 = true; |
fmferrari | 18:4a769322eb39 | 1455 | } else { |
fmferrari | 18:4a769322eb39 | 1456 | low_ip1++; |
fmferrari | 18:4a769322eb39 | 1457 | } |
fmferrari | 16:a92fd07112f7 | 1458 | } |
fmferrari | 16:a92fd07112f7 | 1459 | } |
fmferrari | 16:a92fd07112f7 | 1460 | |
fmferrari | 18:4a769322eb39 | 1461 | if (low_i < 3) { |
fmferrari | 18:4a769322eb39 | 1462 | while (low_i + 1 < 4) { |
fmferrari | 18:4a769322eb39 | 1463 | if (Bhat[low_i] < A) { |
fmferrari | 18:4a769322eb39 | 1464 | A = Bhat[low_i]; |
fmferrari | 18:4a769322eb39 | 1465 | } |
fmferrari | 18:4a769322eb39 | 1466 | |
fmferrari | 18:4a769322eb39 | 1467 | low_i++; |
fmferrari | 18:4a769322eb39 | 1468 | } |
fmferrari | 16:a92fd07112f7 | 1469 | } |
fmferrari | 18:4a769322eb39 | 1470 | |
fmferrari | 21:534cd02b6bc4 | 1471 | // '<S1>:1:199' IMaxCh=max([Power.iMaxCh;iChOCV;iChSOC]); |
fmferrari | 18:4a769322eb39 | 1472 | Bhat[0] = EKF_DW.Power.iMaxCh; |
fmferrari | 18:4a769322eb39 | 1473 | Bhat[1] = ((ry - std::exp(-EKF_DW.Power.TimeWindow / qx1) * EKF_DW.Xhat[1]) |
fmferrari | 18:4a769322eb39 | 1474 | - EKF_DW.Power.Vmax) / ((EKF_DW.Power.TimeWindow / (EKF_DW.Qhat * |
fmferrari | 18:4a769322eb39 | 1475 | 3600.0) * dOCVatSOC_power + EKF_DW.Power.TimeStep / C1 * PolynomialA) + |
fmferrari | 18:4a769322eb39 | 1476 | EKF_DW.Power.R0); |
fmferrari | 18:4a769322eb39 | 1477 | Bhat[2] = (EKF_DW.Xhat[0] - EKF_DW.Power.MaxSOC) * (EKF_DW.Qhat * 3600.0) / |
fmferrari | 18:4a769322eb39 | 1478 | EKF_DW.Power.TimeWindow; |
fmferrari | 18:4a769322eb39 | 1479 | low_i = 1; |
fmferrari | 18:4a769322eb39 | 1480 | C1 = EKF_DW.Power.iMaxCh; |
fmferrari | 18:4a769322eb39 | 1481 | if (rtIsNaN(EKF_DW.Power.iMaxCh)) { |
fmferrari | 18:4a769322eb39 | 1482 | low_ip1 = 2; |
fmferrari | 18:4a769322eb39 | 1483 | exitg1 = false; |
fmferrari | 18:4a769322eb39 | 1484 | while ((!exitg1) && (low_ip1 < 4)) { |
fmferrari | 18:4a769322eb39 | 1485 | low_i = low_ip1; |
fmferrari | 18:4a769322eb39 | 1486 | if (!rtIsNaN(Bhat[low_ip1 - 1])) { |
fmferrari | 18:4a769322eb39 | 1487 | C1 = Bhat[low_ip1 - 1]; |
fmferrari | 18:4a769322eb39 | 1488 | exitg1 = true; |
fmferrari | 18:4a769322eb39 | 1489 | } else { |
fmferrari | 18:4a769322eb39 | 1490 | low_ip1++; |
fmferrari | 18:4a769322eb39 | 1491 | } |
fmferrari | 18:4a769322eb39 | 1492 | } |
fmferrari | 18:4a769322eb39 | 1493 | } |
fmferrari | 18:4a769322eb39 | 1494 | |
fmferrari | 18:4a769322eb39 | 1495 | if (low_i < 3) { |
fmferrari | 18:4a769322eb39 | 1496 | while (low_i + 1 < 4) { |
fmferrari | 18:4a769322eb39 | 1497 | if (Bhat[low_i] > C1) { |
fmferrari | 18:4a769322eb39 | 1498 | C1 = Bhat[low_i]; |
fmferrari | 18:4a769322eb39 | 1499 | } |
fmferrari | 18:4a769322eb39 | 1500 | |
fmferrari | 18:4a769322eb39 | 1501 | low_i++; |
fmferrari | 18:4a769322eb39 | 1502 | } |
fmferrari | 18:4a769322eb39 | 1503 | } |
fmferrari | 18:4a769322eb39 | 1504 | |
fmferrari | 21:534cd02b6bc4 | 1505 | // '<S1>:1:201' VoltTermDisch=PredictTerminalVoltage(IMaxDisch,Power); |
fmferrari | 21:534cd02b6bc4 | 1506 | // '<S1>:1:202' VoltTermCh=PredictTerminalVoltage(IMaxCh,Power); |
fmferrari | 21:534cd02b6bc4 | 1507 | // '<S1>:1:204' PowerDischHat=abs(IMaxDisch*VoltTermDisch); |
fmferrari | 18:4a769322eb39 | 1508 | EKF_DW.PowerDischHat = std::abs(A * EKF_PredictTerminalVoltage(A, |
fmferrari | 18:4a769322eb39 | 1509 | EKF_DW.Power.Ahat, EKF_DW.Power.Bhat, EKF_DW.Power.Temp, |
fmferrari | 18:4a769322eb39 | 1510 | EKF_DW.Power.Xhat_, EKF_DW.Power.Iterations, EKF_DW.Power.R0)); |
fmferrari | 18:4a769322eb39 | 1511 | |
fmferrari | 21:534cd02b6bc4 | 1512 | // '<S1>:1:205' PowerChHat=abs(IMaxCh*VoltTermCh); |
fmferrari | 18:4a769322eb39 | 1513 | EKF_DW.PowerChHat = std::abs(C1 * EKF_PredictTerminalVoltage(C1, |
fmferrari | 18:4a769322eb39 | 1514 | EKF_DW.Power.Ahat, EKF_DW.Power.Bhat, EKF_DW.Power.Temp, |
fmferrari | 18:4a769322eb39 | 1515 | EKF_DW.Power.Xhat_, EKF_DW.Power.Iterations, EKF_DW.Power.R0)); |
fmferrari | 19:09279ceaa882 | 1516 | |
fmferrari | 21:534cd02b6bc4 | 1517 | // '<S1>:1:206' iP=iP+1; |
fmferrari | 19:09279ceaa882 | 1518 | EKF_DW.iP++; |
fmferrari | 16:a92fd07112f7 | 1519 | } |
fmferrari | 16:a92fd07112f7 | 1520 | |
fmferrari | 14:4b5df635f248 | 1521 | // MAKE SURE SOC IS WITHIN LIMITS |
fmferrari | 21:534cd02b6bc4 | 1522 | // '<S1>:1:212' if Xhat(1)>1 |
fmferrari | 14:4b5df635f248 | 1523 | if (EKF_DW.Xhat[0] > 1.0) { |
fmferrari | 21:534cd02b6bc4 | 1524 | // '<S1>:1:213' Xhat(1)=1; |
fmferrari | 14:4b5df635f248 | 1525 | EKF_DW.Xhat[0] = 1.0; |
fmferrari | 14:4b5df635f248 | 1526 | } |
fmferrari | 14:4b5df635f248 | 1527 | |
fmferrari | 21:534cd02b6bc4 | 1528 | // '<S1>:1:216' if Xhat(1)<0 |
fmferrari | 14:4b5df635f248 | 1529 | if (EKF_DW.Xhat[0] < 0.0) { |
fmferrari | 21:534cd02b6bc4 | 1530 | // '<S1>:1:217' Xhat(1)=0; |
fmferrari | 14:4b5df635f248 | 1531 | EKF_DW.Xhat[0] = 0.0; |
fmferrari | 11:e39c490420d2 | 1532 | } |
fmferrari | 9:17c258c67c33 | 1533 | |
fmferrari | 11:e39c490420d2 | 1534 | // Outport: '<Root>/SOC' incorporates: |
fmferrari | 21:534cd02b6bc4 | 1535 | // MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 8:339dd9885af3 | 1536 | |
fmferrari | 14:4b5df635f248 | 1537 | // OUTPUT THE VALUES |
fmferrari | 21:534cd02b6bc4 | 1538 | // '<S1>:1:221' SOC=Xhat(1); |
fmferrari | 21:534cd02b6bc4 | 1539 | // '<S1>:1:222' Q0=ThetaHat; |
fmferrari | 21:534cd02b6bc4 | 1540 | // '<S1>:1:223' R0=R0_filt; |
fmferrari | 21:534cd02b6bc4 | 1541 | // '<S1>:1:225' PowerDisch=PowerDischHat; |
fmferrari | 21:534cd02b6bc4 | 1542 | // '<S1>:1:226' PowerCh=PowerChHat; |
fmferrari | 11:e39c490420d2 | 1543 | EKF_Y.SOC = EKF_DW.Xhat[0]; |
fmferrari | 8:339dd9885af3 | 1544 | |
fmferrari | 13:831eab218c33 | 1545 | // Outport: '<Root>/Q0' incorporates: |
fmferrari | 21:534cd02b6bc4 | 1546 | // MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 13:831eab218c33 | 1547 | |
fmferrari | 13:831eab218c33 | 1548 | EKF_Y.Q0 = EKF_DW.ThetaHat; |
fmferrari | 16:a92fd07112f7 | 1549 | |
fmferrari | 21:534cd02b6bc4 | 1550 | // Outport: '<Root>/R0' incorporates: |
fmferrari | 21:534cd02b6bc4 | 1551 | // MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 18:4a769322eb39 | 1552 | |
fmferrari | 21:534cd02b6bc4 | 1553 | EKF_Y.R0 = EKF_DW.R0_filt; |
fmferrari | 18:4a769322eb39 | 1554 | |
fmferrari | 18:4a769322eb39 | 1555 | // Outport: '<Root>/PowerDisch' incorporates: |
fmferrari | 21:534cd02b6bc4 | 1556 | // MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 16:a92fd07112f7 | 1557 | |
fmferrari | 18:4a769322eb39 | 1558 | EKF_Y.PowerDisch = EKF_DW.PowerDischHat; |
fmferrari | 18:4a769322eb39 | 1559 | |
fmferrari | 18:4a769322eb39 | 1560 | // Outport: '<Root>/PowerCh' incorporates: |
fmferrari | 21:534cd02b6bc4 | 1561 | // MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 18:4a769322eb39 | 1562 | |
fmferrari | 18:4a769322eb39 | 1563 | EKF_Y.PowerCh = EKF_DW.PowerChHat; |
fmferrari | 21:534cd02b6bc4 | 1564 | |
fmferrari | 21:534cd02b6bc4 | 1565 | // Outport: '<Root>/DegradationIndex' incorporates: |
fmferrari | 21:534cd02b6bc4 | 1566 | // MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 21:534cd02b6bc4 | 1567 | |
fmferrari | 21:534cd02b6bc4 | 1568 | EKF_Y.DegradationIndex = rx; |
fmferrari | 6:cb71171a7108 | 1569 | } |
fmferrari | 6:cb71171a7108 | 1570 | |
fmferrari | 6:cb71171a7108 | 1571 | // Model initialize function |
fmferrari | 7:e0be546a9112 | 1572 | void EKF_initialize(void) |
fmferrari | 6:cb71171a7108 | 1573 | { |
fmferrari | 6:cb71171a7108 | 1574 | // Registration code |
fmferrari | 6:cb71171a7108 | 1575 | |
fmferrari | 8:339dd9885af3 | 1576 | // initialize non-finites |
fmferrari | 8:339dd9885af3 | 1577 | rt_InitInfAndNaN(sizeof(real_T)); |
fmferrari | 8:339dd9885af3 | 1578 | |
fmferrari | 6:cb71171a7108 | 1579 | // initialize error status |
fmferrari | 7:e0be546a9112 | 1580 | rtmSetErrorStatus(EKF_M, (NULL)); |
fmferrari | 6:cb71171a7108 | 1581 | |
fmferrari | 8:339dd9885af3 | 1582 | // states (dwork) |
fmferrari | 8:339dd9885af3 | 1583 | (void) memset((void *)&EKF_DW, 0, |
fmferrari | 8:339dd9885af3 | 1584 | sizeof(DW_EKF_T)); |
fmferrari | 8:339dd9885af3 | 1585 | |
fmferrari | 6:cb71171a7108 | 1586 | // external inputs |
fmferrari | 6:cb71171a7108 | 1587 | (void)memset((void *)&EKF_U, 0, sizeof(ExtU_EKF_T)); |
fmferrari | 6:cb71171a7108 | 1588 | |
fmferrari | 6:cb71171a7108 | 1589 | // external outputs |
fmferrari | 11:e39c490420d2 | 1590 | (void) memset((void *)&EKF_Y, 0, |
fmferrari | 11:e39c490420d2 | 1591 | sizeof(ExtY_EKF_T)); |
fmferrari | 11:e39c490420d2 | 1592 | |
fmferrari | 11:e39c490420d2 | 1593 | { |
fmferrari | 13:831eab218c33 | 1594 | static const real_T tmp[9] = { 0.1, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.1 |
fmferrari | 11:e39c490420d2 | 1595 | }; |
fmferrari | 8:339dd9885af3 | 1596 | |
fmferrari | 21:534cd02b6bc4 | 1597 | // SystemInitialize for MATLAB Function: '<Root>/MATLAB Function' |
fmferrari | 11:e39c490420d2 | 1598 | EKF_DW.Xhat[0] = 0.5; |
fmferrari | 11:e39c490420d2 | 1599 | EKF_DW.Xhat[1] = 0.1; |
fmferrari | 21:534cd02b6bc4 | 1600 | EKF_DW.Xhat[2] = -6.76; |
fmferrari | 11:e39c490420d2 | 1601 | memcpy(&EKF_DW.P0[0], &tmp[0], 9U * sizeof(real_T)); |
fmferrari | 13:831eab218c33 | 1602 | EKF_DW.Count_not_empty = false; |
fmferrari | 15:b39f568faa27 | 1603 | EKF_DW.Qhat = 53.0; |
fmferrari | 13:831eab218c33 | 1604 | EKF_DW.P0th = 2.0; |
fmferrari | 13:831eab218c33 | 1605 | EKF_DW.XhatTh = 0.8; |
fmferrari | 13:831eab218c33 | 1606 | EKF_DW.CthPrev = 0.0; |
fmferrari | 21:534cd02b6bc4 | 1607 | EKF_DW.iQ = 1.0; |
fmferrari | 21:534cd02b6bc4 | 1608 | EKF_DW.UpdateMacro = 1200.0; |
fmferrari | 11:e39c490420d2 | 1609 | EKF_DW.Current_prev = 0.0; |
fmferrari | 21:534cd02b6bc4 | 1610 | EKF_DW.PowerDischHat = 0.0; |
fmferrari | 21:534cd02b6bc4 | 1611 | EKF_DW.PowerChHat = 0.0; |
fmferrari | 19:09279ceaa882 | 1612 | EKF_DW.iP = 1.0; |
fmferrari | 11:e39c490420d2 | 1613 | } |
fmferrari | 6:cb71171a7108 | 1614 | } |
fmferrari | 6:cb71171a7108 | 1615 | |
fmferrari | 6:cb71171a7108 | 1616 | // Model terminate function |
fmferrari | 7:e0be546a9112 | 1617 | void EKF_terminate(void) |
fmferrari | 6:cb71171a7108 | 1618 | { |
fmferrari | 6:cb71171a7108 | 1619 | // (no terminate code required) |
fmferrari | 6:cb71171a7108 | 1620 | } |
fmferrari | 6:cb71171a7108 | 1621 | |
fmferrari | 6:cb71171a7108 | 1622 | // |
fmferrari | 6:cb71171a7108 | 1623 | // File trailer for generated code. |
fmferrari | 6:cb71171a7108 | 1624 | // |
fmferrari | 6:cb71171a7108 | 1625 | // [EOF] |
fmferrari | 6:cb71171a7108 | 1626 | // |