Federico Ferrari
A public repository for BMS algorithms for a NUCLEO BOARD.
Hi Everyone!
Welcome to this repository from Howey's Research Group at the University of Oxford.
The code published here incorporates BMS algorithms for diagnosis functions such as SOC, SOH and Power estimation on a Kokam 53Ah Li-ion battery. This code was designed to work with a NUCLEO F401-RE board and to be tested with a dSPACE HIL Simulator. A short guide on how the set up works is available at https://bitbucket.org/ff95/bms .
The code is made up of three key parts. "Headers" and "Source" folders and the "main.cpp" file. As the code was generated by converting a Simulink model ( available on the BitBucket page), the headers and source code files generated by the conversion are in the corresponding "Headers" and "Source" folders. The "main.cpp" file sets up the ADC, the USB data transmission and starts the estimation (once a character "y" has been received by the computer it is connected to). It also transmits the data from the estimation via USB. Explanation on how to set up the communication with the board is available at BitBucket webpage, from where a MATLAB file can be downloaded which allows real time communication.
For any questions you can contact the author at federicomariaferrari@gmail.com .
The Simulink and Matlab files, together with a short guide, are all available at: https://bitbucket.org/ff95/bms.
Thanks for trying this out!
Federico
Diff: source/Simulink/EKF.cpp
- Revision:
- 23:447ef1071e49
- Parent:
- 22:caf41c9bb9d0
--- a/source/Simulink/EKF.cpp Wed Mar 08 18:49:52 2017 +0000
+++ b/source/Simulink/EKF.cpp Thu Jul 13 10:50:13 2017 +0000
@@ -7,9 +7,9 @@
//
// Code generated for Simulink model 'EKF'.
//
-// Model version : 1.425
+// Model version : 1.439
// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016
-// C/C++ source code generated on : Wed Mar 8 10:23:12 2017
+// C/C++ source code generated on : Wed Mar 15 14:31:33 2017
//
// Target selection: ert.tlc
// Embedded hardware selection: STMicroelectronics->ST10/Super10
@@ -34,10 +34,12 @@
// Forward declaration for local functions
static int16_T EKF_bsearch(const real_T x[4], real_T xi);
-static real_T EKF_interp2(real_T varargin_4, real_T varargin_5);
-static real_T EKF_interp2_p(real_T varargin_4, real_T varargin_5);
-static real_T EKF_interp2_dispatch(const real_T V[76], real_T Xq, real_T Yq,
- real_T extrapval);
+static real_T EKF_interp2_local(const real_T V[64], real_T Xq, real_T Yq, real_T
+ extrapval, const real_T X[16], const real_T Y[4]);
+static real_T EKF_interp2_local_e(const real_T V[68], real_T Xq, real_T Yq,
+ real_T extrapval, const real_T X[17], const real_T Y[4]);
+static real_T EKF_interp2_local_el(const real_T V[76], real_T Xq, real_T Yq,
+ real_T extrapval, const real_T X[19], const real_T Y[4]);
static void EKF_InterpolateR0(real_T SOC, real_T MeasTemp, real_T XhatParam,
real_T *EstimatedT25R0, real_T *TrueT25R0, real_T *DegradationIndex);
static void EKF_inv(const real_T x[9], real_T y[9]);
@@ -70,54 +72,23 @@
}
// Function for MATLAB Function: '<Root>/MATLAB Function'
-static real_T EKF_interp2(real_T varargin_4, real_T varargin_5)
+static real_T EKF_interp2_local(const real_T V[64], real_T Xq, real_T Yq, real_T
+ extrapval, const real_T X[16], const real_T Y[4])
{
real_T Vq;
- int32_T b_y1;
- int32_T y2;
int16_T low_i;
int16_T low_ip1;
int16_T high_i;
int16_T mid_i;
real_T qx1;
real_T rx;
- static const real_T b[16] = { 0.05, 0.1100000000000001, 0.17000000000000004,
- 0.22999999999999998, 0.29000000000000004, 0.35, 0.41000000000000003,
- 0.47000000000000003, 0.53, 0.59, 0.64999999999999991, 0.71, 0.77, 0.83, 0.89,
- 0.95 };
-
- static const real_T V[64] = { 16576.9425034523, 20138.979107141495,
- 24421.239197254181, 28529.878780245781, 15374.498069286346,
- 21772.293746471405, 25693.107843399048, 32291.115447878838,
- 18555.804342031479, 20658.149644732475, 25824.466645717621,
- 31220.083832740784, 25359.847992658615, 27558.146119117737,
- 28088.040053844452, 32270.592749118805, 23069.943785667419,
- 32527.030557394028, 42899.709641933441, 47403.486669063568,
- 21542.409062385559, 27543.162256479263, 34497.639834880829,
- 38461.93790435791, 20651.820003986359, 25631.530284881592,
- 31862.854510545731, 33635.095953941345, 19963.297843933105,
- 24295.083433389664, 28222.531974315643, 30071.003437042236,
- 19400.93457698822, 23581.461608409882, 28570.601940155029,
- 29711.003303527832, 20319.71737742424, 24107.016921043396,
- 27998.240292072296, 30197.245478630066, 27554.730474948883,
- 30108.308792114258, 31208.517402410507, 34530.265778303146,
- 40256.447046995163, 47065.51194190979, 48255.230188369751,
- 42222.758531570435, 25791.702717542648, 26096.538305282593,
- 26316.764354705811, 28954.031467437744, 20904.739797115326,
- 22026.374936103821, 24867.414683103561, 29067.600816488266,
- 17860.728204250336, 21127.029955387115, 24830.964803695679,
- 28136.543035507202, 16835.392713546753, 21224.110424518585,
- 25584.730058908463, 28471.727073192596 };
-
- real_T tmp[4];
- if ((varargin_4 >= 0.05) && (varargin_4 <= 0.95) && (varargin_5 >= 15.0) &&
- (varargin_5 <= 45.0)) {
+ if ((Xq >= X[0]) && (Xq <= X[15]) && (Yq >= Y[0]) && (Yq <= Y[3])) {
low_i = 0;
low_ip1 = 2;
high_i = 16;
while (high_i > low_ip1) {
mid_i = ((low_i + high_i) + 1) >> 1;
- if (varargin_4 >= b[mid_i - 1]) {
+ if (Xq >= X[mid_i - 1]) {
low_i = mid_i - 1;
low_ip1 = mid_i + 1;
} else {
@@ -125,21 +96,15 @@
}
}
- tmp[0] = 15.0;
- tmp[1] = 25.0;
- tmp[2] = 35.0;
- tmp[3] = 45.0;
- low_ip1 = EKF_bsearch(tmp, varargin_5);
- b_y1 = (low_ip1 - 1) * 10L + 15L;
- y2 = 10L * low_ip1 + 15L;
- if (varargin_4 == b[low_i]) {
+ low_ip1 = EKF_bsearch(Y, Yq);
+ if (Xq == X[low_i]) {
qx1 = V[((low_i << 2) + low_ip1) - 1];
Vq = V[(low_i << 2) + low_ip1];
- } else if (b[low_i + 1] == varargin_4) {
+ } else if (X[low_i + 1] == Xq) {
qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1];
Vq = V[((low_i + 1) << 2) + low_ip1];
} else {
- rx = (varargin_4 - b[low_i]) / (b[low_i + 1] - b[low_i]);
+ rx = (Xq - X[low_i]) / (X[low_i + 1] - X[low_i]);
if (V[(((low_i + 1) << 2) + low_ip1) - 1] == V[((low_i << 2) + low_ip1) -
1]) {
qx1 = V[((low_i << 2) + low_ip1) - 1];
@@ -156,71 +121,39 @@
}
}
- if ((varargin_5 == b_y1) || (qx1 == Vq)) {
+ if ((Y[low_ip1 - 1] == Yq) || (qx1 == Vq)) {
Vq = qx1;
} else {
- if (!(varargin_5 == y2)) {
- rx = (varargin_5 - (real_T)b_y1) / (real_T)(y2 - b_y1);
+ if (!(Yq == Y[low_ip1])) {
+ rx = (Yq - Y[low_ip1 - 1]) / (Y[low_ip1] - Y[low_ip1 - 1]);
Vq = (1.0 - rx) * qx1 + rx * Vq;
}
}
} else {
- Vq = (rtNaN);
+ Vq = extrapval;
}
return Vq;
}
// Function for MATLAB Function: '<Root>/MATLAB Function'
-static real_T EKF_interp2_p(real_T varargin_4, real_T varargin_5)
+static real_T EKF_interp2_local_e(const real_T V[68], real_T Xq, real_T Yq,
+ real_T extrapval, const real_T X[17], const real_T Y[4])
{
real_T Vq;
- int32_T b_y1;
- int32_T y2;
int16_T low_i;
int16_T low_ip1;
int16_T high_i;
int16_T mid_i;
real_T qx1;
real_T rx;
- static const real_T b[17] = { 0.05, 0.10625000000000007, 0.16250000000000009,
- 0.21875, 0.275, 0.33125000000000004, 0.38749999999999996, 0.44375, 0.5,
- 0.55625, 0.6125, 0.66875, 0.725, 0.78125, 0.83749999999999991,
- 0.89374999999999993, 0.95 };
-
- static const real_T V[68] = { 0.003318750942826959, 0.0023001508295625922,
- 0.001810091258767631, 0.0014328808446455602, 0.0030545289991703731,
- 0.0021115342558727062, 0.0016216012369423278, 0.0012823414045409901,
- 0.0027529509371346668, 0.002036583066188943, 0.001545886433903926,
- 0.0012446724248481845, 0.0027524319432923596, 0.0017720469026882386,
- 0.0013951734539969804, 0.0011314777098891079, 0.0029413982954973922,
- 0.0018856539447880626, 0.0013197089099204458, 0.0010937617862261414,
- 0.0027527433161129741, 0.0020366598778004175, 0.001659312893615418,
- 0.0013955418096782679, 0.0025641025641025663, 0.0018852273584194189,
- 0.0015085800490288528, 0.0012819546037252021, 0.0023759239704329357,
- 0.0017728490060729568, 0.0014328268164850424, 0.0012065910033558322,
- 0.0022251555723175622, 0.0016591877521776855, 0.0012822446824558853,
- 0.0010559263868461751, 0.0021121713876211686, 0.0015462945502545701,
- 0.0012444846702115593, 0.0010182531301855535, 0.0019607843137254919,
- 0.0014709210228558553, 0.0011689731890342826, 0.00098024430704267071,
- 0.0022244844097575752, 0.0015838298514216688, 0.0012444846702115593,
- 0.0010559662090813104, 0.0027153416804947981, 0.0021879361726206297,
- 0.0018478711769808023, 0.0015460613145292029, 0.0025639092074504211,
- 0.001998717803673113, 0.0016216012369423445, 0.0013197089099204289,
- 0.002413455011690175, 0.001810023002375657, 0.0014330969980389297,
- 0.0011689291101055859, 0.0021869461935824378, 0.001659250320537012,
- 0.0012820029410655633, 0.0010935555639352886, 0.0020366598778004175,
- 0.0014707546102500505, 0.0012066820015837712, 0.0010181379388362777 };
-
- real_T tmp[4];
- if ((varargin_4 >= 0.05) && (varargin_4 <= 0.95) && (varargin_5 >= 15.0) &&
- (varargin_5 <= 45.0)) {
+ if ((Xq >= X[0]) && (Xq <= X[16]) && (Yq >= Y[0]) && (Yq <= Y[3])) {
low_i = 0;
low_ip1 = 2;
high_i = 17;
while (high_i > low_ip1) {
mid_i = ((low_i + high_i) + 1) >> 1;
- if (varargin_4 >= b[mid_i - 1]) {
+ if (Xq >= X[mid_i - 1]) {
low_i = mid_i - 1;
low_ip1 = mid_i + 1;
} else {
@@ -228,21 +161,15 @@
}
}
- tmp[0] = 15.0;
- tmp[1] = 25.0;
- tmp[2] = 35.0;
- tmp[3] = 45.0;
- low_ip1 = EKF_bsearch(tmp, varargin_5);
- b_y1 = (low_ip1 - 1) * 10L + 15L;
- y2 = 10L * low_ip1 + 15L;
- if (varargin_4 == b[low_i]) {
+ low_ip1 = EKF_bsearch(Y, Yq);
+ if (Xq == X[low_i]) {
qx1 = V[((low_i << 2) + low_ip1) - 1];
Vq = V[(low_i << 2) + low_ip1];
- } else if (b[low_i + 1] == varargin_4) {
+ } else if (X[low_i + 1] == Xq) {
qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1];
Vq = V[((low_i + 1) << 2) + low_ip1];
} else {
- rx = (varargin_4 - b[low_i]) / (b[low_i + 1] - b[low_i]);
+ rx = (Xq - X[low_i]) / (X[low_i + 1] - X[low_i]);
if (V[(((low_i + 1) << 2) + low_ip1) - 1] == V[((low_i << 2) + low_ip1) -
1]) {
qx1 = V[((low_i << 2) + low_ip1) - 1];
@@ -259,47 +186,39 @@
}
}
- if ((varargin_5 == b_y1) || (qx1 == Vq)) {
+ if ((Y[low_ip1 - 1] == Yq) || (qx1 == Vq)) {
Vq = qx1;
} else {
- if (!(varargin_5 == y2)) {
- rx = (varargin_5 - (real_T)b_y1) / (real_T)(y2 - b_y1);
+ if (!(Yq == Y[low_ip1])) {
+ rx = (Yq - Y[low_ip1 - 1]) / (Y[low_ip1] - Y[low_ip1 - 1]);
Vq = (1.0 - rx) * qx1 + rx * Vq;
}
}
} else {
- Vq = (rtNaN);
+ Vq = extrapval;
}
return Vq;
}
// Function for MATLAB Function: '<Root>/MATLAB Function'
-static real_T EKF_interp2_dispatch(const real_T V[76], real_T Xq, real_T Yq,
- real_T extrapval)
+static real_T EKF_interp2_local_el(const real_T V[76], real_T Xq, real_T Yq,
+ real_T extrapval, const real_T X[19], const real_T Y[4])
{
real_T Vq;
- int32_T b_y1;
- int32_T y2;
int16_T low_i;
int16_T low_ip1;
int16_T high_i;
int16_T mid_i;
real_T qx1;
real_T rx;
- static const real_T b[19] = { 0.05, 0.099999999999999978, 0.15000000000000002,
- 0.20000000000000007, 0.25000000000000011, 0.30000000000000004, 0.35, 0.4,
- 0.44999999999999996, 0.5, 0.55, 0.60000000000000009, 0.64999999999999991,
- 0.7, 0.75, 0.79999999999999993, 0.85, 0.89999999999999991, 0.95 };
-
- real_T tmp[4];
- if ((Xq >= 0.05) && (Xq <= 0.95) && (Yq >= 15.0) && (Yq <= 45.0)) {
+ if ((Xq >= X[0]) && (Xq <= X[18]) && (Yq >= Y[0]) && (Yq <= Y[3])) {
low_i = 0;
low_ip1 = 2;
high_i = 19;
while (high_i > low_ip1) {
mid_i = ((low_i + high_i) + 1) >> 1;
- if (Xq >= b[mid_i - 1]) {
+ if (Xq >= X[mid_i - 1]) {
low_i = mid_i - 1;
low_ip1 = mid_i + 1;
} else {
@@ -307,21 +226,15 @@
}
}
- tmp[0] = 15.0;
- tmp[1] = 25.0;
- tmp[2] = 35.0;
- tmp[3] = 45.0;
- low_ip1 = EKF_bsearch(tmp, Yq);
- b_y1 = (low_ip1 - 1) * 10L + 15L;
- y2 = 10L * low_ip1 + 15L;
- if (Xq == b[low_i]) {
+ low_ip1 = EKF_bsearch(Y, Yq);
+ if (Xq == X[low_i]) {
qx1 = V[((low_i << 2) + low_ip1) - 1];
Vq = V[(low_i << 2) + low_ip1];
- } else if (b[low_i + 1] == Xq) {
+ } else if (X[low_i + 1] == Xq) {
qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1];
Vq = V[((low_i + 1) << 2) + low_ip1];
} else {
- rx = (Xq - b[low_i]) / (b[low_i + 1] - b[low_i]);
+ rx = (Xq - X[low_i]) / (X[low_i + 1] - X[low_i]);
if (V[(((low_i + 1) << 2) + low_ip1) - 1] == V[((low_i << 2) + low_ip1) -
1]) {
qx1 = V[((low_i << 2) + low_ip1) - 1];
@@ -338,11 +251,11 @@
}
}
- if ((Yq == b_y1) || (qx1 == Vq)) {
+ if ((Y[low_ip1 - 1] == Yq) || (qx1 == Vq)) {
Vq = qx1;
} else {
- if (!(Yq == y2)) {
- rx = (Yq - (real_T)b_y1) / (real_T)(y2 - b_y1);
+ if (!(Yq == Y[low_ip1])) {
+ rx = (Yq - Y[low_ip1 - 1]) / (Y[low_ip1] - Y[low_ip1 - 1]);
Vq = (1.0 - rx) * qx1 + rx * Vq;
}
}
@@ -425,9 +338,9 @@
// IT ASSUMES IT KNOWS THE BEHAVIOUR OF THE DATA BUT NOT THE VALUE (I
// CALCULATE THE ESTIMATED R0 AND I CAN USE THE KNOWN PARAMETERS TO GET THE
// 25°C VALUE
- // '<S1>:1:274' zData=fliplr(ParamData.zR0);
- // '<S1>:1:275' SlopeData= fliplr(ParamData.SlopeR0);
- // '<S1>:1:277' InterpSlope=interp1(zData,SlopeData,SOC);
+ // '<S1>:1:344' zData=fliplr(ParamData.zR0);
+ // '<S1>:1:345' SlopeData= fliplr(ParamData.SlopeR0);
+ // '<S1>:1:347' InterpSlope=interp1(zData,SlopeData,SOC);
InterpSlope = (rtNaN);
if ((!rtIsNaN(SOC)) && (!((SOC > 1.0) || (SOC < 0.0)))) {
low_i = 0;
@@ -455,13 +368,13 @@
}
}
- // '<S1>:1:279' ParamOffset=XhatParam-InterpSlope*MeasTemp;
- // '<S1>:1:280' Param25Value=InterpSlope*25+ParamOffset;
+ // '<S1>:1:349' ParamOffset=XhatParam-InterpSlope*MeasTemp;
+ // '<S1>:1:350' Param25Value=InterpSlope*25+ParamOffset;
*EstimatedT25R0 = (XhatParam - InterpSlope * MeasTemp) + InterpSlope * 25.0;
- // '<S1>:1:283' EstimatedT25R0=Param25Value;
+ // '<S1>:1:353' EstimatedT25R0=Param25Value;
// DETERMINE THE CORRECT R0 AT 25 VALUE
- // '<S1>:1:287' TrueT25R0=interp2(ParamData.R0d_x,ParamData.R0d_y,ParamData.R0_map,SOC,25);
+ // '<S1>:1:357' TrueT25R0=interp2(ParamData.R0d_x,ParamData.R0d_y,ParamData.R0_map,SOC,25);
if ((SOC >= 0.05) && (SOC <= 0.95)) {
low_i = 0;
low_ip1 = 2;
@@ -519,7 +432,7 @@
*TrueT25R0 = (rtNaN);
}
- // '<S1>:1:289' DegradationIndex=EstimatedT25R0/TrueT25R0;
+ // '<S1>:1:359' DegradationIndex=EstimatedT25R0/TrueT25R0;
*DegradationIndex = *EstimatedT25R0 / *TrueT25R0;
}
@@ -940,8 +853,9 @@
real_T TerminalVoltage;
real_T PolynomialA[9];
real_T PredictedStates[3];
+ real_T OCVatSOC;
int16_T i;
- static const real_T b[76] = { 3.493, 3.464, 3.407, 3.412, 3.526, 3.495, 3.482,
+ static const real_T V[76] = { 3.493, 3.464, 3.407, 3.412, 3.526, 3.495, 3.482,
3.483, 3.556, 3.531, 3.517, 3.517, 3.578, 3.561, 3.549, 3.549, 3.604, 3.583,
3.574, 3.573, 3.629, 3.612, 3.6, 3.6, 3.643, 3.636, 3.633, 3.634, 3.658,
3.651, 3.649, 3.651, 3.675, 3.668, 3.666, 3.668, 3.696, 3.688, 3.685, 3.687,
@@ -950,48 +864,87 @@
3.946, 3.948, 4.002, 4.001, 4.0, 4.002, 4.056, 4.057, 4.058, 4.06, 4.114,
4.118, 4.12, 4.123 };
- real_T tmp[9];
+ static const real_T b[19] = { 0.05, 0.099999999999999978, 0.15000000000000002,
+ 0.20000000000000007, 0.25000000000000011, 0.30000000000000004, 0.35, 0.4,
+ 0.44999999999999996, 0.5, 0.55, 0.60000000000000009, 0.64999999999999991,
+ 0.7, 0.75, 0.79999999999999993, 0.85, 0.89999999999999991, 0.95 };
+
+ static const real_T b_V[76] = { 3.437, 3.46, 3.438, 3.394, 3.497, 3.501, 3.497,
+ 3.489, 3.532, 3.541, 3.536, 3.524, 3.571, 3.578, 3.575, 3.565, 3.6, 3.608,
+ 3.605, 3.597, 3.623, 3.63, 3.629, 3.623, 3.639, 3.645, 3.645, 3.643, 3.652,
+ 3.66, 3.66, 3.658, 3.667, 3.677, 3.677, 3.674, 3.685, 3.697, 3.698, 3.693,
+ 3.706, 3.722, 3.723, 3.717, 3.733, 3.753, 3.756, 3.748, 3.765, 3.795, 3.799,
+ 3.787, 3.812, 3.856, 3.86, 3.847, 3.868, 3.904, 3.909, 3.896, 3.914, 3.954,
+ 3.96, 3.946, 3.963, 4.007, 4.015, 3.999, 4.014, 4.064, 4.072, 4.055, 4.069,
+ 4.124, 4.133, 4.115 };
+
+ static const real_T c[19] = { 0.05, 0.1, 0.15, 0.2, 0.25, 0.3, 0.35,
+ 0.39999999999999991, 0.44999999999999996, 0.49999999999999994, 0.55, 0.6,
+ 0.65, 0.7, 0.74999999999999989, 0.79999999999999993, 0.85, 0.9, 0.95 };
+
+ real_T tmp[4];
+ real_T tmp_0[9];
int16_T i_0;
real_T Current_0[3];
- // '<S1>:1:242' LoadedData=coder.load('ParamData.mat');
- // '<S1>:1:243' ParamData=LoadedData.ParamData;
- // '<S1>:1:245' A=Power.Ahat;
- // '<S1>:1:246' Xhat=Power.Xhat_;
- // '<S1>:1:247' TempBattInt=Power.Temp;
- // '<S1>:1:248' Iterations=Power.Iterations;
- // '<S1>:1:249' B=Power.Bhat;
- // '<S1>:1:250' R0=Power.R0;
- // '<S1>:1:252' PolynomialA=0*A;
+ // '<S1>:1:308' LoadedDataDch=coder.load('ParamData.mat');
+ // '<S1>:1:309' LoadedDataCh=coder.load('ParamDataCh.mat');
+ // '<S1>:1:310' ParamData=LoadedDataDch.ParamData;
+ // '<S1>:1:311' ParamDataCh=LoadedDataCh.ParamDataCh;
+ // '<S1>:1:313' A=Power.Ahat;
+ // '<S1>:1:314' Xhat=Power.Xhat_;
+ // '<S1>:1:315' TempBattInt=Power.Temp;
+ // '<S1>:1:316' Iterations=Power.Iterations;
+ // '<S1>:1:317' B=Power.Bhat;
+ // '<S1>:1:318' R0=Power.R0;
+ // '<S1>:1:320' PolynomialA=0*A;
for (i_0 = 0; i_0 < 9; i_0++) {
PolynomialA[i_0] = 0.0 * Power_Ahat[i_0];
}
- // '<S1>:1:254' for i=0:1:(Iterations-1)
+ // '<S1>:1:322' for i=0:1:(Iterations-1)
for (i = 0; i < (int16_T)((Power_Iterations - 1.0) + 1.0); i++) {
- // '<S1>:1:255' PolynomialA=PolynomialA+A^i;
- EKF_mpower(Power_Ahat, (real_T)i, tmp);
+ // '<S1>:1:323' PolynomialA=PolynomialA+A^i;
+ EKF_mpower(Power_Ahat, (real_T)i, tmp_0);
for (i_0 = 0; i_0 < 9; i_0++) {
- PolynomialA[i_0] += tmp[i_0];
+ PolynomialA[i_0] += tmp_0[i_0];
}
}
- // '<S1>:1:258' PredictedStates=A^(Iterations)*Xhat+PolynomialA*B.*[Current Current 0]';
- EKF_mpower(Power_Ahat, Power_Iterations, tmp);
+ // '<S1>:1:326' PredictedStates=A^(Iterations)*Xhat+PolynomialA*B.*[Current Current 0]';
+ EKF_mpower(Power_Ahat, Power_Iterations, tmp_0);
Current_0[0] = Current;
Current_0[1] = Current;
Current_0[2] = 0.0;
for (i_0 = 0; i_0 < 3; i_0++) {
- PredictedStates[i_0] = ((tmp[i_0 + 3] * Power_Xhat_[1] + tmp[i_0] *
- Power_Xhat_[0]) + tmp[i_0 + 6] * Power_Xhat_[2]) + (PolynomialA[i_0 + 6] *
- Power_Bhat[2] + (PolynomialA[i_0 + 3] * Power_Bhat[1] + PolynomialA[i_0] *
- Power_Bhat[0])) * Current_0[i_0];
+ PredictedStates[i_0] = ((tmp_0[i_0 + 3] * Power_Xhat_[1] + tmp_0[i_0] *
+ Power_Xhat_[0]) + tmp_0[i_0 + 6] * Power_Xhat_[2]) + (PolynomialA[i_0 + 6]
+ * Power_Bhat[2] + (PolynomialA[i_0 + 3] * Power_Bhat[1] + PolynomialA[i_0]
+ * Power_Bhat[0])) * Current_0[i_0];
}
- // '<S1>:1:260' OCVatSOC= interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,PredictedStates(1),TempBattInt);
- // '<S1>:1:262' TerminalVoltage=OCVatSOC -PredictedStates(2) -R0*(Current);
- TerminalVoltage = (EKF_interp2_dispatch(b, PredictedStates[0], Power_Temp,
- (rtNaN)) - PredictedStates[1]) - Power_R0 * Current;
+ // '<S1>:1:327' if Current>0
+ if (Current > 0.0) {
+ // '<S1>:1:328' OCVatSOC= interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,PredictedStates(1),TempBattInt);
+ tmp[0] = 15.0;
+ tmp[1] = 25.0;
+ tmp[2] = 35.0;
+ tmp[3] = 45.0;
+ OCVatSOC = EKF_interp2_local_el(V, PredictedStates[0], Power_Temp, (rtNaN),
+ b, tmp);
+ } else {
+ // '<S1>:1:329' else
+ // '<S1>:1:330' OCVatSOC= interp2(ParamDataCh.OCVdatad_x,ParamDataCh.OCVdatad_y,ParamDataCh.OCVdata_map,PredictedStates(1),TempBattInt);
+ tmp[0] = 15.0;
+ tmp[1] = 25.0;
+ tmp[2] = 35.0;
+ tmp[3] = 45.0;
+ OCVatSOC = EKF_interp2_local_el(b_V, PredictedStates[0], Power_Temp, (rtNaN),
+ c, tmp);
+ }
+
+ // '<S1>:1:332' TerminalVoltage=OCVatSOC -PredictedStates(2) -R0*(Current);
+ TerminalVoltage = (OCVatSOC - PredictedStates[1]) - Power_R0 * Current;
return TerminalVoltage;
}
@@ -1048,17 +1001,51 @@
real_T tau;
real_T Ahat[9];
real_T Bhat[3];
+ int16_T UpdateNow;
real_T Cth;
real_T KGainTh;
+ real_T Qhat_old;
real_T C[3];
real_T KGain[3];
- real_T OCVatSOC_power;
real_T dOCVatSOC_power;
real_T A;
real_T PolynomialA;
- int16_T ixstart;
int16_T ix;
- static const real_T d[76] = { 1.2199999999999978, 1.2699999999999967,
+ static const real_T d[19] = { 0.05, 0.1, 0.15, 0.2, 0.25, 0.3, 0.35,
+ 0.39999999999999991, 0.44999999999999996, 0.49999999999999994, 0.55, 0.6,
+ 0.65, 0.7, 0.74999999999999989, 0.79999999999999993, 0.85, 0.9, 0.95 };
+
+ static const real_T V[76] = { 3.6399999999999988, 2.9899999999999993, 3.71,
+ 5.77, 0.950000000000002, 0.80999999999999961, 0.97999999999999865,
+ 1.2999999999999989, 0.74000000000000288, 0.76999999999999957,
+ 0.78000000000000291, 0.76000000000000068, 0.6800000000000006,
+ 0.67000000000000171, 0.6899999999999995, 0.72999999999999954,
+ 0.52000000000000046, 0.52000000000000046, 0.53999999999999826,
+ 0.58000000000000274, 0.389999999999997, 0.36999999999999922,
+ 0.40000000000000036, 0.45999999999999819, 0.28999999999999915,
+ 0.30000000000000249, 0.31000000000000139, 0.349999999999997,
+ 0.28000000000000025, 0.32000000000000028, 0.32000000000000028,
+ 0.31000000000000139, 0.32999999999999918, 0.36999999999999922,
+ 0.37999999999999812, 0.35000000000000142, 0.39000000000000146,
+ 0.44999999999999929, 0.45999999999999819, 0.43000000000000149,
+ 0.48000000000000043, 0.5600000000000005, 0.57999999999999829,
+ 0.5500000000000016, 0.59000000000000163, 0.72999999999999954,
+ 0.76000000000000068, 0.6999999999999984, 0.78999999999999737,
+ 1.0299999999999976, 1.0400000000000009, 0.98999999999999755,
+ 1.0299999999999976, 1.0899999999999999, 1.0999999999999988,
+ 1.0899999999999999, 1.0200000000000031, 0.98000000000000309,
+ 1.0000000000000009, 0.990000000000002, 0.950000000000002, 1.0299999999999976,
+ 1.0599999999999987, 1.030000000000002, 1.0000000000000009,
+ 1.0999999999999988, 1.120000000000001, 1.0899999999999954,
+ 1.0599999999999987, 1.17, 1.1800000000000033, 1.160000000000001,
+ 1.6500000000000004, 1.2099999999999955, 1.1899999999999977, 1.33 };
+
+ static const real_T e[19] = { 0.05, 0.099999999999999978, 0.15000000000000002,
+ 0.20000000000000007, 0.25000000000000011, 0.30000000000000004, 0.35, 0.4,
+ 0.44999999999999996, 0.5, 0.55, 0.60000000000000009, 0.64999999999999991,
+ 0.7, 0.75, 0.79999999999999993, 0.85, 0.89999999999999991, 0.95 };
+
+ static const real_T b_V[76] = { 1.2199999999999978, 1.2699999999999967,
1.2900000000000034, 1.3100000000000012, 1.12, 1.169999999999999, 1.2,
1.2100000000000033, 1.04, 1.0900000000000034, 1.1199999999999957,
1.1199999999999957, 0.97999999999999776, 1.0300000000000011,
@@ -1080,15 +1067,139 @@
1.0499999999999989, 1.3099999999999967, 2.8099999999999987,
4.1499999999999968, 4.3099999999999969 };
- static const real_T e[76] = { 3.493, 3.464, 3.407, 3.412, 3.526, 3.495, 3.482,
- 3.483, 3.556, 3.531, 3.517, 3.517, 3.578, 3.561, 3.549, 3.549, 3.604, 3.583,
- 3.574, 3.573, 3.629, 3.612, 3.6, 3.6, 3.643, 3.636, 3.633, 3.634, 3.658,
- 3.651, 3.649, 3.651, 3.675, 3.668, 3.666, 3.668, 3.696, 3.688, 3.685, 3.687,
- 3.722, 3.713, 3.71, 3.712, 3.754, 3.744, 3.74, 3.743, 3.801, 3.787, 3.781,
- 3.785, 3.859, 3.85, 3.846, 3.848, 3.904, 3.898, 3.895, 3.897, 3.952, 3.948,
- 3.946, 3.948, 4.002, 4.001, 4.0, 4.002, 4.056, 4.057, 4.058, 4.06, 4.114,
- 4.118, 4.12, 4.123 };
+ static const real_T c_V[76] = { 3.437, 3.46, 3.438, 3.394, 3.497, 3.501, 3.497,
+ 3.489, 3.532, 3.541, 3.536, 3.524, 3.571, 3.578, 3.575, 3.565, 3.6, 3.608,
+ 3.605, 3.597, 3.623, 3.63, 3.629, 3.623, 3.639, 3.645, 3.645, 3.643, 3.652,
+ 3.66, 3.66, 3.658, 3.667, 3.677, 3.677, 3.674, 3.685, 3.697, 3.698, 3.693,
+ 3.706, 3.722, 3.723, 3.717, 3.733, 3.753, 3.756, 3.748, 3.765, 3.795, 3.799,
+ 3.787, 3.812, 3.856, 3.86, 3.847, 3.868, 3.904, 3.909, 3.896, 3.914, 3.954,
+ 3.96, 3.946, 3.963, 4.007, 4.015, 3.999, 4.014, 4.064, 4.072, 4.055, 4.069,
+ 4.124, 4.133, 4.115 };
+
+ static const real_T d_V[76] = { 3.493, 3.464, 3.407, 3.412, 3.526, 3.495,
+ 3.482, 3.483, 3.556, 3.531, 3.517, 3.517, 3.578, 3.561, 3.549, 3.549, 3.604,
+ 3.583, 3.574, 3.573, 3.629, 3.612, 3.6, 3.6, 3.643, 3.636, 3.633, 3.634,
+ 3.658, 3.651, 3.649, 3.651, 3.675, 3.668, 3.666, 3.668, 3.696, 3.688, 3.685,
+ 3.687, 3.722, 3.713, 3.71, 3.712, 3.754, 3.744, 3.74, 3.743, 3.801, 3.787,
+ 3.781, 3.785, 3.859, 3.85, 3.846, 3.848, 3.904, 3.898, 3.895, 3.897, 3.952,
+ 3.948, 3.946, 3.948, 4.002, 4.001, 4.0, 4.002, 4.056, 4.057, 4.058, 4.06,
+ 4.114, 4.118, 4.12, 4.123 };
+
+ static const real_T e_V[68] = { 0.0032053699374010093, 0.0021497265698661111,
+ 0.0016214177978883919, 0.0012820512820512916, 0.0030545289991703731,
+ 0.0019988685649632259, 0.001546003016591249, 0.0013199577613516422,
+ 0.0025639092074504041, 0.0018101595203077287, 0.001395278678633378,
+ 0.0011690613568654123, 0.0022627847337456652, 0.0015837701270786915,
+ 0.0012820512820512749, 0.001131307036729761, 0.0020364294603461863,
+ 0.0014705882352941233, 0.0011690172712874327, 0.0010180611590814709,
+ 0.00211185277369235, 0.0015837104072398121, 0.0012443908141332599,
+ 0.00098039215686275441, 0.0023006713434411989, 0.0017344745673240004,
+ 0.0013577732518669395, 0.0010935555639352886, 0.0024130910187768668,
+ 0.0018475228112510345, 0.00143293487688072, 0.0011692377324331511,
+ 0.0025264904408160084, 0.0019608582525736285, 0.0015454202789295108,
+ 0.0012446254808780237, 0.0026399155227032674, 0.0020739064856711976,
+ 0.0016968325791855178, 0.0013952260643312314, 0.0026024968883189359,
+ 0.0021493212669683234, 0.0017345399698340974, 0.0014705882352941064,
+ 0.0026393183017871897, 0.0019979643382214323, 0.0016968325791855178,
+ 0.0015082956259426861, 0.0023004978126414388, 0.0013951734539969804,
+ 0.001131221719457023, 0.00098061401523724234, 0.0018480105600603405,
+ 0.0015084093823063591, 0.0012446254808780404, 0.00098042912628682247,
+ 0.0021494023153210886, 0.0017346053772766628, 0.0013951208476301769,
+ 0.0010935143288084432, 0.0024886877828054236, 0.0019232219624406121,
+ 0.00147058823529414, 0.0012066365007541323, 0.0027904521286624628,
+ 0.0020742193392668472, 0.001621723552706021, 0.0012821479749604305 };
+
+ static const real_T h[17] = { 0.05, 0.10625000000000007, 0.16250000000000009,
+ 0.21875, 0.275, 0.33125000000000004, 0.38749999999999996, 0.44375, 0.5,
+ 0.55625, 0.6125, 0.66875, 0.725, 0.78125, 0.83749999999999991,
+ 0.89374999999999993, 0.95 };
+ static const real_T f_V[68] = { 0.003318750942826959, 0.0023001508295625922,
+ 0.001810091258767631, 0.0014328808446455602, 0.0030545289991703731,
+ 0.0021115342558727062, 0.0016216012369423278, 0.0012823414045409901,
+ 0.0027529509371346668, 0.002036583066188943, 0.001545886433903926,
+ 0.0012446724248481845, 0.0027524319432923596, 0.0017720469026882386,
+ 0.0013951734539969804, 0.0011314777098891079, 0.0029413982954973922,
+ 0.0018856539447880626, 0.0013197089099204458, 0.0010937617862261414,
+ 0.0027527433161129741, 0.0020366598778004175, 0.001659312893615418,
+ 0.0013955418096782679, 0.0025641025641025663, 0.0018852273584194189,
+ 0.0015085800490288528, 0.0012819546037252021, 0.0023759239704329357,
+ 0.0017728490060729568, 0.0014328268164850424, 0.0012065910033558322,
+ 0.0022251555723175622, 0.0016591877521776855, 0.0012822446824558853,
+ 0.0010559263868461751, 0.0021121713876211686, 0.0015462945502545701,
+ 0.0012444846702115593, 0.0010182531301855535, 0.0019607843137254919,
+ 0.0014709210228558553, 0.0011689731890342826, 0.00098024430704267071,
+ 0.0022244844097575752, 0.0015838298514216688, 0.0012444846702115593,
+ 0.0010559662090813104, 0.0027153416804947981, 0.0021879361726206297,
+ 0.0018478711769808023, 0.0015460613145292029, 0.0025639092074504211,
+ 0.001998717803673113, 0.0016216012369423445, 0.0013197089099204289,
+ 0.002413455011690175, 0.001810023002375657, 0.0014330969980389297,
+ 0.0011689291101055859, 0.0021869461935824378, 0.001659250320537012,
+ 0.0012820029410655633, 0.0010935555639352886, 0.0020366598778004175,
+ 0.0014707546102500505, 0.0012066820015837712, 0.0010181379388362777 };
+
+ static const real_T j[16] = { 0.05, 0.10999999999999999, 0.16999999999999998,
+ 0.22999999999999998, 0.29, 0.35, 0.41, 0.46999999999999992,
+ 0.52999999999999992, 0.59, 0.65, 0.71, 0.77, 0.83, 0.8899999999999999, 0.95
+ };
+
+ static const real_T g_V[64] = { 12952.58454978466, 16721.245646476746,
+ 18094.454854726791, 19519.349336624146, 16455.746591091156,
+ 19678.233414888382, 22328.75183224678, 28442.932069301605,
+ 16769.467890262604, 20145.526826381683, 21402.607858181, 26433.236002922058,
+ 15587.565451860428, 20140.820145606995, 25156.97255730629,
+ 27427.809238433838, 13005.107939243317, 16267.156600952148,
+ 20800.438523292542, 28211.558312177658, 14418.013095855713,
+ 17023.013830184937, 20969.395637512207, 24025.956839323044,
+ 19510.320052504539, 16737.51026391983, 20584.938079118729,
+ 21689.800769090652, 25105.454325675964, 20594.489276409149,
+ 20420.795679092407, 24547.557830810547, 30244.764536619186,
+ 28689.835667610168, 21030.941307544708, 25378.375649452209,
+ 38914.605975151062, 39606.031626462936, 27208.808213472366,
+ 25108.094215393066, 50986.163169145584, 61406.42374753952,
+ 44493.357837200165, 28249.984309077263, 67713.165283203125,
+ 79396.185725927353, 76527.339890599251, 54358.5991859436, 36749.007776379585,
+ 20649.336278438568, 25218.520164489746, 27854.651063680649,
+ 16192.848384380341, 20641.534104943275, 24964.586496353149,
+ 26407.085955142975, 16436.644196510315, 21475.938558578491,
+ 26011.685431003571, 28530.018329620361, 16578.286588191986,
+ 22495.884746313095, 26096.648871898651, 29256.966412067413 };
+
+ static const real_T k[16] = { 0.05, 0.1100000000000001, 0.17000000000000004,
+ 0.22999999999999998, 0.29000000000000004, 0.35, 0.41000000000000003,
+ 0.47000000000000003, 0.53, 0.59, 0.64999999999999991, 0.71, 0.77, 0.83, 0.89,
+ 0.95 };
+
+ static const real_T h_V[64] = { 16576.9425034523, 20138.979107141495,
+ 24421.239197254181, 28529.878780245781, 15374.498069286346,
+ 21772.293746471405, 25693.107843399048, 32291.115447878838,
+ 18555.804342031479, 20658.149644732475, 25824.466645717621,
+ 31220.083832740784, 25359.847992658615, 27558.146119117737,
+ 28088.040053844452, 32270.592749118805, 23069.943785667419,
+ 32527.030557394028, 42899.709641933441, 47403.486669063568,
+ 21542.409062385559, 27543.162256479263, 34497.639834880829,
+ 38461.93790435791, 20651.820003986359, 25631.530284881592,
+ 31862.854510545731, 33635.095953941345, 19963.297843933105,
+ 24295.083433389664, 28222.531974315643, 30071.003437042236,
+ 19400.93457698822, 23581.461608409882, 28570.601940155029,
+ 29711.003303527832, 20319.71737742424, 24107.016921043396,
+ 27998.240292072296, 30197.245478630066, 27554.730474948883,
+ 30108.308792114258, 31208.517402410507, 34530.265778303146,
+ 40256.447046995163, 47065.51194190979, 48255.230188369751,
+ 42222.758531570435, 25791.702717542648, 26096.538305282593,
+ 26316.764354705811, 28954.031467437744, 20904.739797115326,
+ 22026.374936103821, 24867.414683103561, 29067.600816488266,
+ 17860.728204250336, 21127.029955387115, 24830.964803695679,
+ 28136.543035507202, 16835.392713546753, 21224.110424518585,
+ 25584.730058908463, 28471.727073192596 };
+
+ real_T tmp[4];
+ real_T tmp_0[4];
+ real_T tmp_1[4];
+ real_T tmp_2[4];
+ real_T tmp_3[17];
+ real_T tmp_4[4];
+ real_T tmp_5[4];
real_T Ahat_0[3];
real_T Ahat_1[9];
real_T Ahat_2[9];
@@ -1108,80 +1219,80 @@
// MATLAB Function 'MATLAB Function': '<S1>:1'
// '<S1>:1:13' LoadedData=coder.load('ParamData.mat');
// '<S1>:1:14' ParamData=LoadedData.ParamData;
+ // '<S1>:1:15' LoadedData=coder.load('ParamDataCh.mat');
+ // '<S1>:1:16' ParamDataCh=LoadedData.ParamDataCh;
//
- // '<S1>:1:18' if isempty(Count)
+ // '<S1>:1:21' if isempty(Count)
if (!EKF_DW.Count_not_empty) {
- // '<S1>:1:20' Count=0;
+ // '<S1>:1:23' Count=0;
EKF_DW.Count = 0.0;
EKF_DW.Count_not_empty = true;
// States
- // '<S1>:1:23' Xhat=[0.8 0.1 -6.76]';
- // '<S1>:1:24' P0=[0.1 0 0;
- // '<S1>:1:25' 0 0.05 0;
- // '<S1>:1:26' 0 0 .1];
- // '<S1>:1:27' Current_prev=0;
- // '<S1>:1:28' Q=1;
+ // '<S1>:1:26' Xhat=[0.8 0.1 -6.76]';
+ // '<S1>:1:27' P0=[0.1 0 0;
+ // '<S1>:1:28' 0 0.05 0;
+ // '<S1>:1:29' 0 0 .1];
+ // '<S1>:1:30' Current_prev=0;
+ // '<S1>:1:31' Q=1;
// PROCESS NOISE (CURRENT)
- // '<S1>:1:29' R=sqrt(0.1);
- EKF_DW.R = 0.1;
- EKF_DW.R = std::sqrt(EKF_DW.R);
-
+ // '<S1>:1:32' R=0.01;
// SENSOR NOISE (VOLTAGE) THIS NEEDS TO BE FAIRLY LARGE TO ALLOW TO CONVERGE
// Capacity
- // '<S1>:1:32' Qhat=53*3600;
- // '<S1>:1:33' XhatTh=0;
- // '<S1>:1:34' CthPrev=0;
- // '<S1>:1:35' iQ=1;
- // '<S1>:1:36' SumCC=0;
- // '<S1>:1:37' TimeToUpdateQ=0;
- // '<S1>:1:38' AbsSumCC=0;
- // '<S1>:1:39' UpdateQ=60*20;
- // '<S1>:1:40' Qth=(14400)*1000;
+ // '<S1>:1:35' Qhat=53*3600;
+ // '<S1>:1:36' XhatTh=0;
+ // '<S1>:1:37' XhatTh_Update=0;
+ // '<S1>:1:38' CthPrev=0;
+ // '<S1>:1:39' iQ=1;
+ // '<S1>:1:40' SumCC=0;
+ // '<S1>:1:41' TimeToUpdateQ=0;
+ // '<S1>:1:42' AbsSumCC=0;
+ // '<S1>:1:43' UpdateQ=60*20;
+ // '<S1>:1:45' Qth=(14400)*1000;
// Process Noise
- // '<S1>:1:41' Rth=1e-6;
+ // '<S1>:1:46' Rth=1e-6;
// SENSOR NOISE
- // '<S1>:1:42' P0th=10*Qth;
+ // '<S1>:1:47' P0th=10*Qth;
EKF_DW.P0th = 10.0 * EKF_DW.Qth;
// Resistance
- // '<S1>:1:45' R0_filt_prev=exp(Xhat(3));
+ // '<S1>:1:50' R0_filt_prev=exp(Xhat(3));
EKF_DW.R0_filt_prev = EKF_DW.Xhat[2];
EKF_DW.R0_filt_prev = std::exp(EKF_DW.R0_filt_prev);
- // '<S1>:1:46' Current_prev_R=0;
- // '<S1>:1:47' alpha=0.999;
+ // '<S1>:1:51' Current_prev_R=0;
+ // '<S1>:1:52' alpha=0.999;
// Power
- // '<S1>:1:51' Power.iMaxDisch=424;
+ // '<S1>:1:56' Power.iMaxDisch=424;
EKF_DW.Power.iMaxDisch = 424.0;
// for max 10s otherwise mac is 265
- // '<S1>:1:52' Power.iMaxCh=-100;
+ // '<S1>:1:57' Power.iMaxCh=-100;
EKF_DW.Power.iMaxCh = -100.0;
- // '<S1>:1:53' Power.MaxSOC=0.95;
+ // '<S1>:1:58' Power.MaxSOC=0.95;
EKF_DW.Power.MaxSOC = 0.95;
- // '<S1>:1:54' Power.MinSOC=0.05;
+ // '<S1>:1:59' Power.MinSOC=0.05;
EKF_DW.Power.MinSOC = 0.05;
- // '<S1>:1:55' Power.Vmax=4.2;
+ // '<S1>:1:60' Power.Vmax=4.2;
EKF_DW.Power.Vmax = 4.2;
- // '<S1>:1:56' Power.Vmin=2.7;
+ // '<S1>:1:61' Power.Vmin=2.7;
EKF_DW.Power.Vmin = 2.7;
- // '<S1>:1:57' Power.Time=0;
+ // '<S1>:1:62' Power.Time=0;
EKF_DW.Power.Time = 0.0;
- // '<S1>:1:58' Power.Ahat=zeros(3,3);
+ // '<S1>:1:63' Power.Ahat=zeros(3,3);
memset(&EKF_DW.Power.Ahat[0], 0, 9U * sizeof(real_T));
- // '<S1>:1:59' Power.Bhat=[0 0 0]';
- // '<S1>:1:60' Power.Temp=0;
+ // '<S1>:1:64' Power.Bhat=[0 0 0]';
+ // '<S1>:1:65' Power.Temp=0;
EKF_DW.Power.Temp = 0.0;
- // '<S1>:1:61' Power.Xhat_=Xhat;
+ // '<S1>:1:66' Power.Xhat_=Xhat;
EKF_DW.Power.Bhat[0] = 0.0;
EKF_DW.Power.Xhat_[0] = EKF_DW.Xhat[0];
EKF_DW.Power.Bhat[1] = 0.0;
@@ -1189,43 +1300,78 @@
EKF_DW.Power.Bhat[2] = 0.0;
EKF_DW.Power.Xhat_[2] = EKF_DW.Xhat[2];
- // '<S1>:1:62' Power.TimeStep=0;
+ // '<S1>:1:67' Power.TimeStep=0;
EKF_DW.Power.TimeStep = 0.0;
- // '<S1>:1:63' Power.Iterations=0;
+ // '<S1>:1:68' Power.Iterations=0;
EKF_DW.Power.Iterations = 0.0;
- // '<S1>:1:64' Power.R0=0;
+ // '<S1>:1:69' Power.R0=0;
EKF_DW.Power.R0 = 0.0;
- // '<S1>:1:65' PowerDischHat=0;
- // '<S1>:1:66' PowerChHat=0;
- // '<S1>:1:67' iP=1;
- // '<S1>:1:68' Power.TimeWindow=10;
+ // '<S1>:1:70' PowerDischHat=0;
+ // '<S1>:1:71' PowerChHat=0;
+ // '<S1>:1:72' iP=1;
+ // '<S1>:1:73' Power.TimeWindow=10;
EKF_DW.Power.TimeWindow = 10.0;
- // '<S1>:1:69' UpdatePower=1;
+ // '<S1>:1:74' UpdatePower=1;
} else {
- // '<S1>:1:70' else
- // '<S1>:1:71' Count=Count+1;
+ // '<S1>:1:75' else
+ // '<S1>:1:76' Count=Count+1;
EKF_DW.Count++;
}
//
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// INTERPOLATE PARAMETRES
- // '<S1>:1:81' C1=interp2(ParamData.C1d_x,ParamData.C1d_y,ParamData.C1_map,Xhat(1),TemperatureIn);
- C1 = EKF_interp2(EKF_DW.Xhat[0], EKF_U.TemperatureIn);
+ // '<S1>:1:87' if Current>0
+ if (EKF_U.Current > 0.0) {
+ // '<S1>:1:88' C1=interp2(ParamData.C1d_x,ParamData.C1d_y,ParamData.C1_map,Xhat(1),TemperatureIn);
+ tmp_5[0] = 15.0;
+ tmp_5[1] = 25.0;
+ tmp_5[2] = 35.0;
+ tmp_5[3] = 45.0;
+ C1 = EKF_interp2_local(h_V, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN), k,
+ tmp_5);
- // '<S1>:1:82' R1=interp2(ParamData.R1d_x,ParamData.R1d_y,ParamData.R1_map,Xhat(1),TemperatureIn);
- // '<S1>:1:83' tau=C1*R1;
- tau = C1 * EKF_interp2_p(EKF_DW.Xhat[0], EKF_U.TemperatureIn);
+ // '<S1>:1:89' R1=interp2(ParamData.R1d_x,ParamData.R1d_y,ParamData.R1_map,Xhat(1),TemperatureIn);
+ // '<S1>:1:90' tau=C1*R1;
+ tmp_4[0] = 15.0;
+ tmp_4[1] = 25.0;
+ tmp_4[2] = 35.0;
+ tmp_4[3] = 45.0;
+ tau = C1 * EKF_interp2_local_e(f_V, EKF_DW.Xhat[0], EKF_U.TemperatureIn,
+ (rtNaN), h, tmp_4);
+ } else {
+ // '<S1>:1:91' else
+ // '<S1>:1:92' C1=interp2(ParamDataCh.C1d_x,ParamDataCh.C1d_y,ParamDataCh.C1_map,Xhat(1),TemperatureIn);
+ tmp_5[0] = 15.0;
+ tmp_5[1] = 25.0;
+ tmp_5[2] = 35.0;
+ tmp_5[3] = 45.0;
+ C1 = EKF_interp2_local(g_V, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN), j,
+ tmp_5);
+
+ // '<S1>:1:93' R1=interp2(ParamDataCh.R1d_x,ParamDataCh.R1d_y,ParamDataCh.R1_map,Xhat(1),TemperatureIn);
+ // '<S1>:1:94' tau=C1*R1;
+ for (UpdateNow = 0; UpdateNow < 17; UpdateNow++) {
+ tmp_3[UpdateNow] = 0.056249999999999994 * (real_T)UpdateNow + 0.05;
+ }
+
+ tmp_4[0] = 15.0;
+ tmp_4[1] = 25.0;
+ tmp_4[2] = 35.0;
+ tmp_4[3] = 45.0;
+ tau = C1 * EKF_interp2_local_e(e_V, EKF_DW.Xhat[0], EKF_U.TemperatureIn,
+ (rtNaN), tmp_3, tmp_4);
+ }
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// DISCRETE TIME SS MATRICES
- // '<S1>:1:87' Ahat=[1 0 0;
- // '<S1>:1:88' 0 exp(-TimeStep/tau) 0;
- // '<S1>:1:89' 0 0 1;];
+ // '<S1>:1:100' Ahat=[1 0 0;
+ // '<S1>:1:101' 0 exp(-TimeStep/tau) 0;
+ // '<S1>:1:102' 0 0 1;];
Ahat[1] = 0.0;
Ahat[4] = std::exp(-EKF_U.TimeStep / tau);
Ahat[7] = 0.0;
@@ -1236,9 +1382,9 @@
Ahat[6] = 0.0;
Ahat[8] = 1.0;
- // '<S1>:1:92' Bhat=[-TimeStep/(Qhat)
- // '<S1>:1:93' TimeStep/C1
- // '<S1>:1:94' 1];
+ // '<S1>:1:105' Bhat=[-TimeStep/(Qhat)
+ // '<S1>:1:106' TimeStep/C1
+ // '<S1>:1:107' 1];
Bhat[0] = -EKF_U.TimeStep / EKF_DW.Qhat;
Bhat[1] = EKF_U.TimeStep / C1;
Bhat[2] = 1.0;
@@ -1247,186 +1393,281 @@
// PREDICTION STAGE
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// SOC ESTIMATION FOR CAPACITY ESTIMATION
- // '<S1>:1:101' if MacroTime>10*60
- if (EKF_U.MacroTime > 600.0) {
- // '<S1>:1:102' SumCC=(Current_prev*TimeStep)+SumCC;
+ // '<S1>:1:114' if MacroTime>15*60
+ if (EKF_U.MacroTime > 900.0) {
+ // '<S1>:1:115' XhatTh_Update=XhatTh_Update-TimeStep/(Qhat)*Current_prev;
+ EKF_DW.XhatTh_Update -= EKF_U.TimeStep / EKF_DW.Qhat * EKF_DW.Current_prev;
+
+ // '<S1>:1:116' SumCC=(Current_prev*TimeStep)+SumCC;
EKF_DW.SumCC += EKF_DW.Current_prev * EKF_U.TimeStep;
- // '<S1>:1:103' AbsSumCC=abs(Current_prev*TimeStep)+AbsSumCC;
+ // '<S1>:1:117' AbsSumCC=abs(Current_prev*TimeStep)+AbsSumCC;
EKF_DW.AbsSumCC += std::abs(EKF_DW.Current_prev * EKF_U.TimeStep);
- // '<S1>:1:104' TimeToUpdateQ=TimeToUpdateQ+TimeStep;
+ // '<S1>:1:118' TimeToUpdateQ=TimeToUpdateQ+TimeStep;
EKF_DW.TimeToUpdateQ += EKF_U.TimeStep;
} else {
- // '<S1>:1:105' else
- // '<S1>:1:106' XhatTh=Xhat(1);
+ // '<S1>:1:119' else
+ // '<S1>:1:120' XhatTh=Xhat(1);
EKF_DW.XhatTh = EKF_DW.Xhat[0];
+
+ // '<S1>:1:121' XhatTh_Update=Xhat(1);
+ EKF_DW.XhatTh_Update = EKF_DW.Xhat[0];
}
- // '<S1>:1:110' if TimeToUpdateQ>UpdateQ
- if (EKF_DW.TimeToUpdateQ > EKF_DW.UpdateQ) {
- // if AbsSumCC>1.5e4
- // '<S1>:1:113' XhatTh=XhatTh-SumCC/(Qhat);
- EKF_DW.XhatTh -= EKF_DW.SumCC / EKF_DW.Qhat;
-
- // '<S1>:1:115' P0th=P0th+Qth;
- EKF_DW.P0th += EKF_DW.Qth;
-
- // '<S1>:1:117' Cth=+SumCC/((Qhat)^2)+ CthPrev;
- Cth = EKF_DW.SumCC / (EKF_DW.Qhat * EKF_DW.Qhat) + EKF_DW.CthPrev;
-
- // '<S1>:1:119' KGainTh=P0th*(Cth')*(Cth*P0th*Cth'+Rth)^-1;
- KGainTh = 1.0 / (Cth * EKF_DW.P0th * Cth + EKF_DW.Rth) * (EKF_DW.P0th * Cth);
+ // '<S1>:1:124' MeasError_Update=Xhat(1)-XhatTh_Update(1);
+ // ONLY UPDATE IF ERROR IS ABOVE THRESHOLD AND ENOUGH TIME HAS ELAPSED
+ // '<S1>:1:127' if abs(MeasError_Update)<1.1e-3
+ if (std::abs(EKF_DW.Xhat[0] - EKF_DW.XhatTh_Update) < 0.0011) {
+ // '<S1>:1:129' UpdateNow=0;
+ UpdateNow = 0;
- // '<S1>:1:121' MeasError=Xhat(1)-XhatTh(1);
- // '<S1>:1:123' Qhat=Qhat+KGainTh*(MeasError);
- EKF_DW.Qhat += (EKF_DW.Xhat[0] - EKF_DW.XhatTh) * KGainTh;
-
- // '<S1>:1:124' P0th=P0th-KGainTh*Cth*KGainTh';
- EKF_DW.P0th -= KGainTh * Cth * KGainTh;
-
- // '<S1>:1:125' CthPrev=Cth;
- EKF_DW.CthPrev = Cth;
+ // '<S1>:1:130' if TimeToUpdateQ>60*30
+ if (EKF_DW.TimeToUpdateQ > 1800.0) {
+ // '<S1>:1:132' SumCC=0;
+ EKF_DW.SumCC = 0.0;
- // '<S1>:1:127' Qhat/3600;
- // '<S1>:1:129' iQ=iQ+1;
- EKF_DW.iQ++;
+ // '<S1>:1:133' AbsSumCC=0;
+ EKF_DW.AbsSumCC = 0.0;
- // '<S1>:1:131' XhatTh=Xhat(1);
- EKF_DW.XhatTh = EKF_DW.Xhat[0];
-
- // '<S1>:1:133' AbsSumCC=0;
- EKF_DW.AbsSumCC = 0.0;
+ // '<S1>:1:134' XhatTh=Xhat(1);
+ EKF_DW.XhatTh = EKF_DW.Xhat[0];
- // '<S1>:1:134' SumCC=0;
- EKF_DW.SumCC = 0.0;
+ // '<S1>:1:135' TimeToUpdateQ=0;
+ EKF_DW.TimeToUpdateQ = 0.0;
- // '<S1>:1:135' TimeToUpdateQ=0;
- EKF_DW.TimeToUpdateQ = 0.0;
- }
+ // '<S1>:1:136' XhatTh_Update=Xhat(1);
+ EKF_DW.XhatTh_Update = EKF_DW.Xhat[0];
+ }
+ } else {
+ // '<S1>:1:139' else
+ // '<S1>:1:141' UpdateNow=0;
+ UpdateNow = 0;
- //
- // PREDICTION FOR STATES
- // '<S1>:1:140' Xhat=Ahat*Xhat+Bhat.*[Current_prev Current_prev 0]';
- KGain[0] = EKF_DW.Current_prev;
- KGain[1] = EKF_DW.Current_prev;
- KGain[2] = 0.0;
- for (ixstart = 0; ixstart < 3; ixstart++) {
- Ahat_0[ixstart] = ((Ahat[ixstart + 3] * EKF_DW.Xhat[1] + Ahat[ixstart] *
- EKF_DW.Xhat[0]) + Ahat[ixstart + 6] * EKF_DW.Xhat[2]) +
- Bhat[ixstart] * KGain[ixstart];
- }
+ // '<S1>:1:142' if TimeToUpdateQ>UpdateQ
+ if (EKF_DW.TimeToUpdateQ > EKF_DW.UpdateQ) {
+ // '<S1>:1:143' TimeToUpdateQ=0;
+ EKF_DW.TimeToUpdateQ = 0.0;
- // '<S1>:1:141' P0=Ahat*P0*Ahat' + Bhat*Q*Bhat';
- for (ixstart = 0; ixstart < 3; ixstart++) {
- EKF_DW.Xhat[ixstart] = Ahat_0[ixstart];
- for (ix = 0; ix < 3; ix++) {
- Ahat_1[ixstart + 3 * ix] = 0.0;
- Ahat_1[ixstart + 3 * ix] += EKF_DW.P0[3 * ix] * Ahat[ixstart];
- Ahat_1[ixstart + 3 * ix] += EKF_DW.P0[3 * ix + 1] * Ahat[ixstart + 3];
- Ahat_1[ixstart + 3 * ix] += EKF_DW.P0[3 * ix + 2] * Ahat[ixstart + 6];
- }
+ // '<S1>:1:144' UpdateNow=1;
+ UpdateNow = 1;
- for (ix = 0; ix < 3; ix++) {
- Ahat_2[ixstart + 3 * ix] = 0.0;
- Ahat_2[ixstart + 3 * ix] += Ahat_1[ixstart] * Ahat[ix];
- Ahat_2[ixstart + 3 * ix] += Ahat_1[ixstart + 3] * Ahat[ix + 3];
- Ahat_2[ixstart + 3 * ix] += Ahat_1[ixstart + 6] * Ahat[ix + 6];
- Bhat_0[ixstart + 3 * ix] = Bhat[ixstart] * EKF_DW.Q * Bhat[ix];
+ // '<S1>:1:145' XhatTh_Update=Xhat(1);
+ EKF_DW.XhatTh_Update = EKF_DW.Xhat[0];
}
}
- // '<S1>:1:143' Current_prev=Current;
+ // '<S1>:1:151' if UpdateNow
+ if (UpdateNow != 0) {
+ // if AbsSumCC>1.5e4
+ // MACROSCALE EKF FOR CAPACITY ESTIMATION
+ // '<S1>:1:155' XhatTh=XhatTh-SumCC/(Qhat);
+ EKF_DW.XhatTh -= EKF_DW.SumCC / EKF_DW.Qhat;
+
+ // '<S1>:1:157' P0th=P0th+Qth;
+ EKF_DW.P0th += EKF_DW.Qth;
+
+ // '<S1>:1:159' Cth=+SumCC/((Qhat)^2)+ CthPrev;
+ Cth = EKF_DW.SumCC / (EKF_DW.Qhat * EKF_DW.Qhat) + EKF_DW.CthPrev;
+
+ // '<S1>:1:161' KGainTh=P0th*(Cth')*(Cth*P0th*Cth'+Rth)^-1;
+ KGainTh = 1.0 / (Cth * EKF_DW.P0th * Cth + EKF_DW.Rth) * (EKF_DW.P0th * Cth);
+
+ // '<S1>:1:163' MeasError=Xhat(1)-XhatTh(1);
+ // STORE PREVIOUS VALUES
+ // '<S1>:1:166' Qhat_old=Qhat;
+ Qhat_old = EKF_DW.Qhat;
+
+ // '<S1>:1:167' Qrated=53*3600;
+ // '<S1>:1:169' Qhat=Qhat+KGainTh*(MeasError);
+ EKF_DW.Qhat += (EKF_DW.Xhat[0] - EKF_DW.XhatTh) * KGainTh;
+
+ // '<S1>:1:170' P0th=P0th-KGainTh*Cth*KGainTh';
+ EKF_DW.P0th -= KGainTh * Cth * KGainTh;
+
+ // '<S1>:1:171' CthPrev=Cth;
+ EKF_DW.CthPrev = Cth;
+
+ // MAKRE SURE THE CAPACITY DOESN'T GO OUT OF SAFETY BOUNDS
+ // '<S1>:1:174' ErrorQ=abs(Qrated-Qhat)/Qrated;
+ // '<S1>:1:175' if ErrorQ>0.5
+ if (std::abs(190800.0 - EKF_DW.Qhat) / 190800.0 > 0.5) {
+ // '<S1>:1:176' Qhat=Qhat_old;
+ EKF_DW.Qhat = Qhat_old;
+ }
+
+ // '<S1>:1:180' QPrint=Qhat/3600
+ // '<S1>:1:181' XhatTh=Xhat(1);
+ EKF_DW.XhatTh = EKF_DW.Xhat[0];
+
+ // '<S1>:1:182' iQ=iQ+1;
+ EKF_DW.iQ++;
+
+ // '<S1>:1:184' AbsSumCC=0;
+ EKF_DW.AbsSumCC = 0.0;
+
+ // '<S1>:1:185' SumCC=0;
+ EKF_DW.SumCC = 0.0;
+
+ // '<S1>:1:186' TimeToUpdateQ=0;
+ EKF_DW.TimeToUpdateQ = 0.0;
+
+ // '<S1>:1:187' UpdateNow=0;
+ // '<S1>:1:188' disp(' ' )
+ }
+
+ // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ // PREDICTION FOR STATES
+ // '<S1>:1:194' Xhat=Ahat*Xhat+Bhat.*[Current_prev Current_prev 0]';
+ KGain[0] = EKF_DW.Current_prev;
+ KGain[1] = EKF_DW.Current_prev;
+ KGain[2] = 0.0;
+ for (UpdateNow = 0; UpdateNow < 3; UpdateNow++) {
+ Ahat_0[UpdateNow] = ((Ahat[UpdateNow + 3] * EKF_DW.Xhat[1] + Ahat[UpdateNow]
+ * EKF_DW.Xhat[0]) + Ahat[UpdateNow + 6] * EKF_DW.Xhat
+ [2]) + Bhat[UpdateNow] * KGain[UpdateNow];
+ }
+
+ // '<S1>:1:195' P0=Ahat*P0*Ahat' + Bhat*Q*Bhat';
+ for (UpdateNow = 0; UpdateNow < 3; UpdateNow++) {
+ EKF_DW.Xhat[UpdateNow] = Ahat_0[UpdateNow];
+ for (ix = 0; ix < 3; ix++) {
+ Ahat_1[UpdateNow + 3 * ix] = 0.0;
+ Ahat_1[UpdateNow + 3 * ix] += EKF_DW.P0[3 * ix] * Ahat[UpdateNow];
+ Ahat_1[UpdateNow + 3 * ix] += EKF_DW.P0[3 * ix + 1] * Ahat[UpdateNow + 3];
+ Ahat_1[UpdateNow + 3 * ix] += EKF_DW.P0[3 * ix + 2] * Ahat[UpdateNow + 6];
+ }
+
+ for (ix = 0; ix < 3; ix++) {
+ Ahat_2[UpdateNow + 3 * ix] = 0.0;
+ Ahat_2[UpdateNow + 3 * ix] += Ahat_1[UpdateNow] * Ahat[ix];
+ Ahat_2[UpdateNow + 3 * ix] += Ahat_1[UpdateNow + 3] * Ahat[ix + 3];
+ Ahat_2[UpdateNow + 3 * ix] += Ahat_1[UpdateNow + 6] * Ahat[ix + 6];
+ Bhat_0[UpdateNow + 3 * ix] = Bhat[UpdateNow] * EKF_DW.Q * Bhat[ix];
+ }
+ }
+
+ for (UpdateNow = 0; UpdateNow < 3; UpdateNow++) {
+ EKF_DW.P0[3 * UpdateNow] = Ahat_2[3 * UpdateNow] + Bhat_0[3 * UpdateNow];
+ EKF_DW.P0[1 + 3 * UpdateNow] = Ahat_2[3 * UpdateNow + 1] + Bhat_0[3 *
+ UpdateNow + 1];
+ EKF_DW.P0[2 + 3 * UpdateNow] = Ahat_2[3 * UpdateNow + 2] + Bhat_0[3 *
+ UpdateNow + 2];
+ }
+
+ // '<S1>:1:197' Current_prev=Current;
EKF_DW.Current_prev = EKF_U.Current;
- // '<S1>:1:145' Ytrue=Voltage;
+ // '<S1>:1:199' Ytrue=Voltage;
// ESTIMATION STAGE
// INTERPOLATE CORRECT VALUES
- // '<S1>:1:151' OCVatSOC=interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,Xhat(1),TemperatureIn);
- // '<S1>:1:152' dOCVatSOC=interp2(ParamData.dOCVdatad_x,ParamData.dOCVdatad_y,ParamData.dOCVdata_map,Xhat(1),TemperatureIn);
- // '<S1>:1:155' Yhat=OCVatSOC -Xhat(2) -exp(Xhat(3))*Current;
- // '<S1>:1:156' C=[dOCVatSOC -1 -exp(Xhat(3))*Current];
- KGainTh = EKF_interp2_dispatch(d, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN));
- C[0] = EKF_interp2_dispatch(d, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN));
+ // '<S1>:1:205' if Current>0
+ if (EKF_U.Current > 0.0) {
+ // '<S1>:1:206' OCVatSOC=interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,Xhat(1),TemperatureIn);
+ tmp_2[0] = 15.0;
+ tmp_2[1] = 25.0;
+ tmp_2[2] = 35.0;
+ tmp_2[3] = 45.0;
+ Cth = EKF_interp2_local_el(d_V, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN),
+ e, tmp_2);
+
+ // '<S1>:1:207' dOCVatSOC=interp2(ParamData.dOCVdatad_x,ParamData.dOCVdatad_y,ParamData.dOCVdata_map,Xhat(1),TemperatureIn);
+ tmp_1[0] = 15.0;
+ tmp_1[1] = 25.0;
+ tmp_1[2] = 35.0;
+ tmp_1[3] = 45.0;
+ KGainTh = EKF_interp2_local_el(b_V, EKF_DW.Xhat[0], EKF_U.TemperatureIn,
+ (rtNaN), e, tmp_1);
+ } else {
+ // '<S1>:1:208' else
+ // '<S1>:1:209' OCVatSOC=interp2(ParamDataCh.OCVdatad_x,ParamDataCh.OCVdatad_y,ParamDataCh.OCVdata_map,Xhat(1),TemperatureIn);
+ tmp_2[0] = 15.0;
+ tmp_2[1] = 25.0;
+ tmp_2[2] = 35.0;
+ tmp_2[3] = 45.0;
+ Cth = EKF_interp2_local_el(c_V, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN),
+ d, tmp_2);
+
+ // '<S1>:1:210' dOCVatSOC=interp2(ParamDataCh.dOCVdatad_x,ParamDataCh.dOCVdatad_y,ParamDataCh.dOCVdata_map,Xhat(1),TemperatureIn);
+ tmp_1[0] = 15.0;
+ tmp_1[1] = 25.0;
+ tmp_1[2] = 35.0;
+ tmp_1[3] = 45.0;
+ KGainTh = EKF_interp2_local_el(V, EKF_DW.Xhat[0], EKF_U.TemperatureIn,
+ (rtNaN), d, tmp_1);
+ }
+
+ // '<S1>:1:214' Yhat=OCVatSOC -Xhat(2) -exp(Xhat(3))*Current;
+ // '<S1>:1:215' C=[dOCVatSOC -1 -exp(Xhat(3))*Current];
+ C[0] = KGainTh;
C[1] = -1.0;
C[2] = -std::exp(EKF_DW.Xhat[2]) * EKF_U.Current;
- // '<S1>:1:157' KGain=P0*C'*(C*P0*C'+R)^-1;
- OCVatSOC_power = 0.0;
- for (ixstart = 0; ixstart < 3; ixstart++) {
- EKF_DW.P0[3 * ixstart] = Ahat_2[3 * ixstart] + Bhat_0[3 * ixstart];
- Cth = EKF_DW.P0[3 * ixstart] * KGainTh;
- EKF_DW.P0[1 + 3 * ixstart] = Ahat_2[3 * ixstart + 1] + Bhat_0[3 * ixstart +
- 1];
- Cth += -EKF_DW.P0[3 * ixstart + 1];
- EKF_DW.P0[2 + 3 * ixstart] = Ahat_2[3 * ixstart + 2] + Bhat_0[3 * ixstart +
- 2];
- Cth += EKF_DW.P0[3 * ixstart + 2] * C[2];
- OCVatSOC_power += Cth * C[ixstart];
+ // '<S1>:1:216' KGain=P0*C'*(C*P0*C'+R)^-1;
+ Qhat_old = 0.0;
+ for (UpdateNow = 0; UpdateNow < 3; UpdateNow++) {
+ Qhat_old += (EKF_DW.P0[3 * UpdateNow + 2] * C[2] + (-EKF_DW.P0[3 * UpdateNow
+ + 1] + EKF_DW.P0[3 * UpdateNow] * KGainTh)) * C[UpdateNow];
}
- Cth = 1.0 / (OCVatSOC_power + EKF_DW.R);
- for (ixstart = 0; ixstart < 3; ixstart++) {
- KGain[ixstart] = (EKF_DW.P0[ixstart + 6] * C[2] + (-EKF_DW.P0[ixstart + 3] +
- EKF_DW.P0[ixstart] * KGainTh)) * Cth;
- }
+ KGainTh = 1.0 / (Qhat_old + EKF_DW.R);
- // '<S1>:1:158' Xhat=Xhat+KGain*(Ytrue-Yhat);
- Cth = EKF_U.Voltage - ((EKF_interp2_dispatch(e, EKF_DW.Xhat[0],
- EKF_U.TemperatureIn, (rtNaN)) - EKF_DW.Xhat[1]) - std::exp(EKF_DW.Xhat[2]) *
+ // '<S1>:1:217' Xhat=Xhat+KGain*(Ytrue-Yhat);
+ Cth = EKF_U.Voltage - ((Cth - EKF_DW.Xhat[1]) - std::exp(EKF_DW.Xhat[2]) *
EKF_U.Current);
- // '<S1>:1:159' P0=P0-KGain*(C*P0*C'+R)*KGain';
- OCVatSOC_power = 0.0;
- for (ixstart = 0; ixstart < 3; ixstart++) {
- EKF_DW.Xhat[ixstart] += KGain[ixstart] * Cth;
- OCVatSOC_power += (EKF_DW.P0[3 * ixstart + 2] * C[2] + (-EKF_DW.P0[3 *
- ixstart + 1] + EKF_DW.P0[3 * ixstart] * KGainTh)) * C[ixstart];
+ // '<S1>:1:218' P0=P0-KGain*(C*P0*C'+R)*KGain';
+ Qhat_old = 0.0;
+ for (UpdateNow = 0; UpdateNow < 3; UpdateNow++) {
+ dOCVatSOC_power = (EKF_DW.P0[UpdateNow + 6] * C[2] + (-EKF_DW.P0[UpdateNow +
+ 3] + EKF_DW.P0[UpdateNow] * C[0])) * KGainTh;
+ EKF_DW.Xhat[UpdateNow] += dOCVatSOC_power * Cth;
+ Qhat_old += (EKF_DW.P0[3 * UpdateNow + 2] * C[2] + (-EKF_DW.P0[3 * UpdateNow
+ + 1] + EKF_DW.P0[3 * UpdateNow] * C[0])) * C[UpdateNow];
+ KGain[UpdateNow] = dOCVatSOC_power;
}
- Cth = OCVatSOC_power + EKF_DW.R;
- for (ixstart = 0; ixstart < 3; ixstart++) {
- EKF_DW.P0[ixstart] -= KGain[ixstart] * Cth * KGain[0];
- EKF_DW.P0[ixstart + 3] -= KGain[ixstart] * Cth * KGain[1];
- EKF_DW.P0[ixstart + 6] -= KGain[ixstart] * Cth * KGain[2];
+ Cth = Qhat_old + EKF_DW.R;
+ for (UpdateNow = 0; UpdateNow < 3; UpdateNow++) {
+ EKF_DW.P0[UpdateNow] -= KGain[UpdateNow] * Cth * KGain[0];
+ EKF_DW.P0[UpdateNow + 3] -= KGain[UpdateNow] * Cth * KGain[1];
+ EKF_DW.P0[UpdateNow + 6] -= KGain[UpdateNow] * Cth * KGain[2];
}
// R0 FILTER
- // '<S1>:1:164' if abs(Current-Current_prev_R)>5
+ // '<S1>:1:223' if abs(Current-Current_prev_R)>5
if (std::abs(EKF_U.Current - EKF_DW.Current_prev_R) > 5.0) {
- // '<S1>:1:165' 1;
- // '<S1>:1:166' R0_filt=R0_filt_prev*alpha + (1-alpha)*exp(Xhat(3));
+ // '<S1>:1:225' R0_filt=R0_filt_prev*alpha + (1-alpha)*exp(Xhat(3));
Cth = (1.0 - EKF_DW.alpha) * std::exp(EKF_DW.Xhat[2]) + EKF_DW.R0_filt_prev *
EKF_DW.alpha;
- // '<S1>:1:167' R0_filt_prev=R0_filt;
+ // '<S1>:1:226' R0_filt_prev=R0_filt;
EKF_DW.R0_filt_prev = Cth;
} else {
- // '<S1>:1:168' else
- // '<S1>:1:169' R0_filt=R0_filt_prev;
+ // '<S1>:1:227' else
+ // '<S1>:1:228' R0_filt=R0_filt_prev;
Cth = EKF_DW.R0_filt_prev;
}
- // '<S1>:1:174' [~,~,DegradationIndex]=InterpolateR0(Xhat(1),TemperatureIn, R0_filt,ParamData);
- EKF_InterpolateR0(EKF_DW.Xhat[0], EKF_U.TemperatureIn, Cth, &OCVatSOC_power,
+ // '<S1>:1:233' [~,~,DegradationIndex]=InterpolateR0(Xhat(1),TemperatureIn, R0_filt,ParamData);
+ EKF_InterpolateR0(EKF_DW.Xhat[0], EKF_U.TemperatureIn, Cth, &Qhat_old,
&dOCVatSOC_power, &KGainTh);
- // '<S1>:1:174' ~
- //
+ // '<S1>:1:233' ~
// POWER ESTIMATION
- // '<S1>:1:178' if MacroTime>UpdatePower*iP
+ // '<S1>:1:237' if MacroTime>UpdatePower*iP
if (EKF_U.MacroTime > EKF_DW.UpdatePower * EKF_DW.iP) {
- // '<S1>:1:180' Current_prev_R=Current;
+ // '<S1>:1:239' Current_prev_R=Current;
EKF_DW.Current_prev_R = EKF_U.Current;
- // '<S1>:1:182' Power.Ahat=Ahat;
+ // '<S1>:1:241' Power.Ahat=Ahat;
memcpy(&EKF_DW.Power.Ahat[0], &Ahat[0], 9U * sizeof(real_T));
- // '<S1>:1:183' Power.Bhat=Bhat;
- // '<S1>:1:184' Power.Temp=TemperatureIn;
+ // '<S1>:1:242' Power.Bhat=Bhat;
+ // '<S1>:1:243' Power.Temp=TemperatureIn;
EKF_DW.Power.Temp = EKF_U.TemperatureIn;
- // '<S1>:1:185' Power.Xhat_=Xhat;
+ // '<S1>:1:244' Power.Xhat_=Xhat;
EKF_DW.Power.Bhat[0] = Bhat[0];
EKF_DW.Power.Xhat_[0] = EKF_DW.Xhat[0];
EKF_DW.Power.Bhat[1] = Bhat[1];
@@ -1434,62 +1675,90 @@
EKF_DW.Power.Bhat[2] = 1.0;
EKF_DW.Power.Xhat_[2] = EKF_DW.Xhat[2];
- // '<S1>:1:186' Power.TimeStep=TimeStep;
+ // '<S1>:1:245' Power.TimeStep=TimeStep;
EKF_DW.Power.TimeStep = EKF_U.TimeStep;
- // '<S1>:1:187' Power.Iterations=50;
+ // '<S1>:1:246' Power.Iterations=50;
EKF_DW.Power.Iterations = 50.0;
// 50 iterations at 0.2s time step is 10seconds
- // '<S1>:1:188' Power.R0=R0_filt;
+ // '<S1>:1:247' Power.R0=R0_filt;
EKF_DW.Power.R0 = Cth;
- // '<S1>:1:190' iChSOC=(Xhat(1)-Power.MaxSOC)*(Qhat)/(Power.TimeWindow);
- // '<S1>:1:191' iDischSOC=(Xhat(1)-Power.MinSOC)*(Qhat)/(Power.TimeWindow);
- // '<S1>:1:193' OCVatSOC_power=interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,Xhat(1),TemperatureIn);
- OCVatSOC_power = EKF_interp2_dispatch(e, EKF_DW.Xhat[0], EKF_U.TemperatureIn,
- (rtNaN));
+ // '<S1>:1:249' iChSOC=(Xhat(1)-Power.MaxSOC)*(Qhat)/(Power.TimeWindow);
+ // '<S1>:1:250' iDischSOC=(Xhat(1)-Power.MinSOC)*(Qhat)/(Power.TimeWindow);
+ // '<S1>:1:252' if Current>0
+ if (EKF_U.Current > 0.0) {
+ // '<S1>:1:253' OCVatSOC_power=interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,Xhat(1),TemperatureIn);
+ tmp_0[0] = 15.0;
+ tmp_0[1] = 25.0;
+ tmp_0[2] = 35.0;
+ tmp_0[3] = 45.0;
+ Qhat_old = EKF_interp2_local_el(d_V, EKF_DW.Xhat[0], EKF_U.TemperatureIn,
+ (rtNaN), e, tmp_0);
- // '<S1>:1:194' dOCVatSOC_power=interp2(ParamData.dOCVdatad_x,ParamData.dOCVdatad_y,ParamData.dOCVdata_map,Xhat(1),TemperatureIn);
- dOCVatSOC_power = EKF_interp2_dispatch(d, EKF_DW.Xhat[0],
- EKF_U.TemperatureIn, (rtNaN));
+ // '<S1>:1:254' dOCVatSOC_power=interp2(ParamData.dOCVdatad_x,ParamData.dOCVdatad_y,ParamData.dOCVdata_map,Xhat(1),TemperatureIn);
+ tmp[0] = 15.0;
+ tmp[1] = 25.0;
+ tmp[2] = 35.0;
+ tmp[3] = 45.0;
+ dOCVatSOC_power = EKF_interp2_local_el(b_V, EKF_DW.Xhat[0],
+ EKF_U.TemperatureIn, (rtNaN), e, tmp);
+ } else {
+ // '<S1>:1:255' else
+ // '<S1>:1:256' OCVatSOC_power=interp2(ParamDataCh.OCVdatad_x,ParamDataCh.OCVdatad_y,ParamDataCh.OCVdata_map,Xhat(1),TemperatureIn);
+ tmp_0[0] = 15.0;
+ tmp_0[1] = 25.0;
+ tmp_0[2] = 35.0;
+ tmp_0[3] = 45.0;
+ Qhat_old = EKF_interp2_local_el(c_V, EKF_DW.Xhat[0], EKF_U.TemperatureIn,
+ (rtNaN), d, tmp_0);
- // '<S1>:1:196' A=exp(-Power.TimeWindow/tau);
+ // '<S1>:1:257' dOCVatSOC_power=interp2(ParamDataCh.dOCVdatad_x,ParamDataCh.dOCVdatad_y,ParamDataCh.dOCVdata_map,Xhat(1),TemperatureIn);
+ tmp[0] = 15.0;
+ tmp[1] = 25.0;
+ tmp[2] = 35.0;
+ tmp[3] = 45.0;
+ dOCVatSOC_power = EKF_interp2_local_el(V, EKF_DW.Xhat[0],
+ EKF_U.TemperatureIn, (rtNaN), d, tmp);
+ }
+
+ // '<S1>:1:260' A=exp(-Power.TimeWindow/tau);
A = std::exp(-EKF_DW.Power.TimeWindow / tau);
- // '<S1>:1:197' PolynomialA=0;
+ // '<S1>:1:261' PolynomialA=0;
PolynomialA = 0.0;
- // '<S1>:1:198' for i=0:1:(Power.Iterations-1)
- for (ixstart = 0; ixstart < (int16_T)((EKF_DW.Power.Iterations - 1.0) + 1.0);
- ixstart++) {
- // '<S1>:1:199' PolynomialA=PolynomialA+A^i;
- PolynomialA += rt_powd_snf(A, (real_T)ixstart);
+ // '<S1>:1:262' for i=0:1:(Power.Iterations-1)
+ for (UpdateNow = 0; UpdateNow < (int16_T)((EKF_DW.Power.Iterations - 1.0) +
+ 1.0); UpdateNow++) {
+ // '<S1>:1:263' PolynomialA=PolynomialA+A^i;
+ PolynomialA += rt_powd_snf(A, (real_T)UpdateNow);
}
- // '<S1>:1:202' iDischOCV=(OCVatSOC_power-Xhat(2)*exp(-Power.TimeWindow/tau)-Power.Vmin)/...
- // '<S1>:1:203' ((Power.TimeWindow/(Qhat))*dOCVatSOC_power+...
- // '<S1>:1:204' (Power.TimeStep/C1)*PolynomialA+...
- // '<S1>:1:205' Power.R0);
- // '<S1>:1:207' iChOCV=(OCVatSOC_power-Xhat(2)*exp(-Power.TimeWindow/tau)-Power.Vmax)/...
- // '<S1>:1:208' ((Power.TimeWindow/(Qhat))*dOCVatSOC_power+...
- // '<S1>:1:209' (Power.TimeStep/C1)*PolynomialA+...
- // '<S1>:1:210' Power.R0);
- // '<S1>:1:212' IMaxDisch=min([Power.iMaxDisch; iDischOCV; iDischSOC]);
+ // '<S1>:1:266' iDischOCV=(OCVatSOC_power-Xhat(2)*exp(-Power.TimeWindow/tau)-Power.Vmin)/...
+ // '<S1>:1:267' ((Power.TimeWindow/(Qhat))*dOCVatSOC_power+...
+ // '<S1>:1:268' (Power.TimeStep/C1)*PolynomialA+...
+ // '<S1>:1:269' Power.R0);
+ // '<S1>:1:271' iChOCV=(OCVatSOC_power-Xhat(2)*exp(-Power.TimeWindow/tau)-Power.Vmax)/...
+ // '<S1>:1:272' ((Power.TimeWindow/(Qhat))*dOCVatSOC_power+...
+ // '<S1>:1:273' (Power.TimeStep/C1)*PolynomialA+...
+ // '<S1>:1:274' Power.R0);
+ // '<S1>:1:276' IMaxDisch=min([Power.iMaxDisch; iDischOCV; iDischSOC]);
Bhat[0] = EKF_DW.Power.iMaxDisch;
- Bhat[1] = ((OCVatSOC_power - std::exp(-EKF_DW.Power.TimeWindow / tau) *
+ Bhat[1] = ((Qhat_old - std::exp(-EKF_DW.Power.TimeWindow / tau) *
EKF_DW.Xhat[1]) - EKF_DW.Power.Vmin) / ((EKF_DW.Power.TimeWindow
/ EKF_DW.Qhat * dOCVatSOC_power + EKF_DW.Power.TimeStep / C1 * PolynomialA)
+ EKF_DW.Power.R0);
Bhat[2] = (EKF_DW.Xhat[0] - EKF_DW.Power.MinSOC) * EKF_DW.Qhat /
EKF_DW.Power.TimeWindow;
- ixstart = 1;
+ UpdateNow = 1;
A = EKF_DW.Power.iMaxDisch;
if (rtIsNaN(EKF_DW.Power.iMaxDisch)) {
ix = 2;
exitg2 = false;
while ((!exitg2) && (ix < 4)) {
- ixstart = ix;
+ UpdateNow = ix;
if (!rtIsNaN(Bhat[ix - 1])) {
A = Bhat[ix - 1];
exitg2 = true;
@@ -1499,31 +1768,31 @@
}
}
- if (ixstart < 3) {
- while (ixstart + 1 < 4) {
- if (Bhat[ixstart] < A) {
- A = Bhat[ixstart];
+ if (UpdateNow < 3) {
+ while (UpdateNow + 1 < 4) {
+ if (Bhat[UpdateNow] < A) {
+ A = Bhat[UpdateNow];
}
- ixstart++;
+ UpdateNow++;
}
}
- // '<S1>:1:213' IMaxCh=max([Power.iMaxCh;iChOCV;iChSOC]);
+ // '<S1>:1:277' IMaxCh=max([Power.iMaxCh;iChOCV;iChSOC]);
Bhat[0] = EKF_DW.Power.iMaxCh;
- Bhat[1] = ((OCVatSOC_power - std::exp(-EKF_DW.Power.TimeWindow / tau) *
+ Bhat[1] = ((Qhat_old - std::exp(-EKF_DW.Power.TimeWindow / tau) *
EKF_DW.Xhat[1]) - EKF_DW.Power.Vmax) / ((EKF_DW.Power.TimeWindow
/ EKF_DW.Qhat * dOCVatSOC_power + EKF_DW.Power.TimeStep / C1 * PolynomialA)
+ EKF_DW.Power.R0);
Bhat[2] = (EKF_DW.Xhat[0] - EKF_DW.Power.MaxSOC) * EKF_DW.Qhat /
EKF_DW.Power.TimeWindow;
- ixstart = 1;
+ UpdateNow = 1;
C1 = EKF_DW.Power.iMaxCh;
if (rtIsNaN(EKF_DW.Power.iMaxCh)) {
ix = 2;
exitg1 = false;
while ((!exitg1) && (ix < 4)) {
- ixstart = ix;
+ UpdateNow = ix;
if (!rtIsNaN(Bhat[ix - 1])) {
C1 = Bhat[ix - 1];
exitg1 = true;
@@ -1533,43 +1802,43 @@
}
}
- if (ixstart < 3) {
- while (ixstart + 1 < 4) {
- if (Bhat[ixstart] > C1) {
- C1 = Bhat[ixstart];
+ if (UpdateNow < 3) {
+ while (UpdateNow + 1 < 4) {
+ if (Bhat[UpdateNow] > C1) {
+ C1 = Bhat[UpdateNow];
}
- ixstart++;
+ UpdateNow++;
}
}
- // '<S1>:1:215' VoltTermDisch=PredictTerminalVoltage(IMaxDisch,Power);
- // '<S1>:1:216' VoltTermCh=PredictTerminalVoltage(IMaxCh,Power);
- // '<S1>:1:218' PowerDischHat=abs(IMaxDisch*VoltTermDisch);
+ // '<S1>:1:279' VoltTermDisch=PredictTerminalVoltage(IMaxDisch,Power);
+ // '<S1>:1:280' VoltTermCh=PredictTerminalVoltage(IMaxCh,Power);
+ // '<S1>:1:282' PowerDischHat=abs(IMaxDisch*VoltTermDisch);
EKF_DW.PowerDischHat = std::abs(A * EKF_PredictTerminalVoltage(A,
EKF_DW.Power.Ahat, EKF_DW.Power.Bhat, EKF_DW.Power.Temp,
EKF_DW.Power.Xhat_, EKF_DW.Power.Iterations, EKF_DW.Power.R0));
- // '<S1>:1:219' PowerChHat=abs(IMaxCh*VoltTermCh);
+ // '<S1>:1:283' PowerChHat=abs(IMaxCh*VoltTermCh);
EKF_DW.PowerChHat = std::abs(C1 * EKF_PredictTerminalVoltage(C1,
EKF_DW.Power.Ahat, EKF_DW.Power.Bhat, EKF_DW.Power.Temp,
EKF_DW.Power.Xhat_, EKF_DW.Power.Iterations, EKF_DW.Power.R0));
- // '<S1>:1:220' iP=iP+1;
+ // '<S1>:1:284' iP=iP+1;
EKF_DW.iP++;
}
- //
+ // '<S1>:1:288' if isnan(Xhat(1))
// MAKE SURE SOC IS WITHIN LIMITS
- // '<S1>:1:226' if Xhat(1)>1
+ // '<S1>:1:292' if Xhat(1)>1
if (EKF_DW.Xhat[0] > 1.0) {
- // '<S1>:1:227' Xhat(1)=1;
+ // '<S1>:1:293' Xhat(1)=1;
EKF_DW.Xhat[0] = 1.0;
}
- // '<S1>:1:230' if Xhat(1)<0
+ // '<S1>:1:296' if Xhat(1)<0
if (EKF_DW.Xhat[0] < 0.0) {
- // '<S1>:1:231' Xhat(1)=0;
+ // '<S1>:1:297' Xhat(1)=0;
EKF_DW.Xhat[0] = 0.0;
}
@@ -1577,11 +1846,11 @@
// MATLAB Function: '<Root>/MATLAB Function'
// OUTPUT THE VALUES
- // '<S1>:1:235' SOC=Xhat(1);
- // '<S1>:1:236' Q0=Qhat/3600;
- // '<S1>:1:237' R0=R0_filt;
- // '<S1>:1:238' PowerDisch=PowerDischHat;
- // '<S1>:1:239' PowerCh=PowerChHat;
+ // '<S1>:1:301' SOC=Xhat(1);
+ // '<S1>:1:302' Q0=Qhat/3600;
+ // '<S1>:1:303' R0=R0_filt;
+ // '<S1>:1:304' PowerDisch=PowerDischHat;
+ // '<S1>:1:305' PowerCh=PowerChHat;
EKF_Y.SOC = EKF_DW.Xhat[0];
// Outport: '<Root>/Q0' incorporates:
@@ -1637,15 +1906,17 @@
};
// SystemInitialize for MATLAB Function: '<Root>/MATLAB Function'
- EKF_DW.Xhat[0] = 0.85;
+ EKF_DW.Xhat[0] = 0.8;
EKF_DW.Xhat[1] = 0.1;
EKF_DW.Xhat[2] = -6.76;
memcpy(&EKF_DW.P0[0], &tmp[0], 9U * sizeof(real_T));
EKF_DW.Count_not_empty = false;
EKF_DW.Current_prev = 0.0;
EKF_DW.Q = 1.0;
+ EKF_DW.R = 0.01;
EKF_DW.Qhat = 190800.0;
EKF_DW.XhatTh = 0.0;
+ EKF_DW.XhatTh_Update = 0.0;
EKF_DW.CthPrev = 0.0;
EKF_DW.iQ = 1.0;
EKF_DW.SumCC = 0.0;