A public repository for BMS algorithms for a NUCLEO BOARD.

Dependencies:   mbed

Hi Everyone!

Welcome to this repository from Howey's Research Group at the University of Oxford.

The code published here incorporates BMS algorithms for diagnosis functions such as SOC, SOH and Power estimation on a Kokam 53Ah Li-ion battery. This code was designed to work with a NUCLEO F401-RE board and to be tested with a dSPACE HIL Simulator. A short guide on how the set up works is available at https://bitbucket.org/ff95/bms .

The code is made up of three key parts. "Headers" and "Source" folders and the "main.cpp" file. As the code was generated by converting a Simulink model ( available on the BitBucket page), the headers and source code files generated by the conversion are in the corresponding "Headers" and "Source" folders. The "main.cpp" file sets up the ADC, the USB data transmission and starts the estimation (once a character "y" has been received by the computer it is connected to). It also transmits the data from the estimation via USB. Explanation on how to set up the communication with the board is available at BitBucket webpage, from where a MATLAB file can be downloaded which allows real time communication.

For any questions you can contact the author at federicomariaferrari@gmail.com .

The Simulink and Matlab files, together with a short guide, are all available at: https://bitbucket.org/ff95/bms.

Thanks for trying this out!

Federico

Committer:
fmferrari
Date:
Fri 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?

UserRevisionLine numberNew contents of line
fmferrari 6:cb71171a7108 1 //
fmferrari 6:cb71171a7108 2 // Academic License - for use in teaching, academic research, and meeting
fmferrari 6:cb71171a7108 3 // course requirements at degree granting institutions only. Not for
fmferrari 6:cb71171a7108 4 // government, commercial, or other organizational use.
fmferrari 6:cb71171a7108 5 //
fmferrari 6:cb71171a7108 6 // File: EKF.cpp
fmferrari 6:cb71171a7108 7 //
fmferrari 6:cb71171a7108 8 // Code generated for Simulink model 'EKF'.
fmferrari 6:cb71171a7108 9 //
fmferrari 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 //