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
Revision 23:447ef1071e49, committed 2017-07-13
- Comitter:
- fmferrari
- Date:
- Thu Jul 13 10:50:13 2017 +0000
- Parent:
- 22:caf41c9bb9d0
- Commit message:
- A working version of the code needed to run an mbed micro controller.
Changed in this revision
--- a/headers/Simulink/EKF.h Wed Mar 08 18:49:52 2017 +0000 +++ b/headers/Simulink/EKF.h Thu Jul 13 10:50:13 2017 +0000 @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'EKF'. // -// Model version : 1.423 +// Model version : 1.439 // Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Mar 6 09:47:51 2017 +// C/C++ source code generated on : Wed Mar 15 14:31:33 2017 // // Target selection: ert.tlc // Embedded hardware selection: STMicroelectronics->ST10/Super10 @@ -60,6 +60,7 @@ real_T Qth; // '<Root>/MATLAB Function' real_T Rth; // '<Root>/MATLAB Function' real_T AbsSumCC; // '<Root>/MATLAB Function' + real_T XhatTh_Update; // '<Root>/MATLAB Function' real_T R0_filt_prev; // '<Root>/MATLAB Function' real_T Current_prev_R; // '<Root>/MATLAB Function' real_T alpha; // '<Root>/MATLAB Function'
--- a/headers/Simulink/EKF_types.h Wed Mar 08 18:49:52 2017 +0000
+++ b/headers/Simulink/EKF_types.h Thu Jul 13 10:50:13 2017 +0000
@@ -7,9 +7,9 @@
//
// Code generated for Simulink model 'EKF'.
//
-// Model version : 1.423
+// Model version : 1.439
// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016
-// C/C++ source code generated on : Mon Mar 6 09:47:51 2017
+// C/C++ source code generated on : Wed Mar 15 14:31:33 2017
//
// Target selection: ert.tlc
// Embedded hardware selection: STMicroelectronics->ST10/Super10
@@ -21,10 +21,10 @@
#include "rtwtypes.h"
// Custom Type definition for MATLAB Function: '<Root>/MATLAB Function'
-#ifndef struct_tag_s0brPt3zjJpLPSuz9oveYcF
-#define struct_tag_s0brPt3zjJpLPSuz9oveYcF
+#ifndef struct_tag_se9QnY1xn68SkdPhpEjQa5B
+#define struct_tag_se9QnY1xn68SkdPhpEjQa5B
-struct tag_s0brPt3zjJpLPSuz9oveYcF
+struct tag_se9QnY1xn68SkdPhpEjQa5B
{
real_T SlopeR0[20];
real_T OffsetR0[20];
@@ -51,33 +51,95 @@
real_T dOCVdata_map[76];
real_T dOCVdatad_x[19];
real_T dOCVdatad_y[4];
+ real_T R0MeanMap[72];
+ real_T R0Mean_map[72];
+ real_T SlopeR0Mean[18];
+ real_T OffsetR0Mean[18];
+};
+
+#endif //struct_tag_se9QnY1xn68SkdPhpEjQa5B
+
+#ifndef typedef_se9QnY1xn68SkdPhpEjQa5B_EKF_T
+#define typedef_se9QnY1xn68SkdPhpEjQa5B_EKF_T
+
+typedef struct tag_se9QnY1xn68SkdPhpEjQa5B se9QnY1xn68SkdPhpEjQa5B_EKF_T;
+
+#endif //typedef_se9QnY1xn68SkdPhpEjQa5B_EKF_T
+
+#ifndef struct_tag_sThWfC4MPcx9gI1J4ylavCH
+#define struct_tag_sThWfC4MPcx9gI1J4ylavCH
+
+struct tag_sThWfC4MPcx9gI1J4ylavCH
+{
+ se9QnY1xn68SkdPhpEjQa5B_EKF_T ParamData;
};
-#endif //struct_tag_s0brPt3zjJpLPSuz9oveYcF
+#endif //struct_tag_sThWfC4MPcx9gI1J4ylavCH
+
+#ifndef typedef_sThWfC4MPcx9gI1J4ylavCH_EKF_T
+#define typedef_sThWfC4MPcx9gI1J4ylavCH_EKF_T
-#ifndef typedef_s0brPt3zjJpLPSuz9oveYcF_EKF_T
-#define typedef_s0brPt3zjJpLPSuz9oveYcF_EKF_T
+typedef struct tag_sThWfC4MPcx9gI1J4ylavCH sThWfC4MPcx9gI1J4ylavCH_EKF_T;
-typedef struct tag_s0brPt3zjJpLPSuz9oveYcF s0brPt3zjJpLPSuz9oveYcF_EKF_T;
+#endif //typedef_sThWfC4MPcx9gI1J4ylavCH_EKF_T
+
+#ifndef struct_tag_sLLlhL5ZgC7fy1VKGqCCQcD
+#define struct_tag_sLLlhL5ZgC7fy1VKGqCCQcD
-#endif //typedef_s0brPt3zjJpLPSuz9oveYcF_EKF_T
-
-#ifndef struct_tag_sVTaynHTAzP4nxJ5g2XE8gG
-#define struct_tag_sVTaynHTAzP4nxJ5g2XE8gG
-
-struct tag_sVTaynHTAzP4nxJ5g2XE8gG
+struct tag_sLLlhL5ZgC7fy1VKGqCCQcD
{
- s0brPt3zjJpLPSuz9oveYcF_EKF_T ParamData;
+ real_T SlopeR0[20];
+ real_T OffsetR0[20];
+ real_T SlopeR1[19];
+ real_T OffsetR1[19];
+ real_T SlopeC1[18];
+ real_T OffsetC1[18];
+ real_T R0_map[72];
+ real_T R0d_x[18];
+ real_T R0d_y[4];
+ real_T zR0[18];
+ real_T R1_map[68];
+ real_T R1d_x[17];
+ real_T R1d_y[4];
+ real_T zR1[17];
+ real_T C1_map[64];
+ real_T C1d_x[16];
+ real_T C1d_y[4];
+ real_T zC1[16];
+ real_T OCVdata_map[76];
+ real_T OCVdatad_x[19];
+ real_T OCVdatad_y[4];
+ real_T zOCV[21];
+ real_T dOCVdata_map[76];
+ real_T dOCVdatad_x[19];
+ real_T dOCVdatad_y[4];
};
-#endif //struct_tag_sVTaynHTAzP4nxJ5g2XE8gG
+#endif //struct_tag_sLLlhL5ZgC7fy1VKGqCCQcD
+
+#ifndef typedef_sLLlhL5ZgC7fy1VKGqCCQcD_EKF_T
+#define typedef_sLLlhL5ZgC7fy1VKGqCCQcD_EKF_T
+
+typedef struct tag_sLLlhL5ZgC7fy1VKGqCCQcD sLLlhL5ZgC7fy1VKGqCCQcD_EKF_T;
+
+#endif //typedef_sLLlhL5ZgC7fy1VKGqCCQcD_EKF_T
+
+#ifndef struct_tag_sFtBan7A404IaGwOPGmStGB
+#define struct_tag_sFtBan7A404IaGwOPGmStGB
-#ifndef typedef_sVTaynHTAzP4nxJ5g2XE8gG_EKF_T
-#define typedef_sVTaynHTAzP4nxJ5g2XE8gG_EKF_T
+struct tag_sFtBan7A404IaGwOPGmStGB
+{
+ sLLlhL5ZgC7fy1VKGqCCQcD_EKF_T ParamDataCh;
+};
+
+#endif //struct_tag_sFtBan7A404IaGwOPGmStGB
-typedef struct tag_sVTaynHTAzP4nxJ5g2XE8gG sVTaynHTAzP4nxJ5g2XE8gG_EKF_T;
+#ifndef typedef_sFtBan7A404IaGwOPGmStGB_EKF_T
+#define typedef_sFtBan7A404IaGwOPGmStGB_EKF_T
-#endif //typedef_sVTaynHTAzP4nxJ5g2XE8gG_EKF_T
+typedef struct tag_sFtBan7A404IaGwOPGmStGB sFtBan7A404IaGwOPGmStGB_EKF_T;
+
+#endif //typedef_sFtBan7A404IaGwOPGmStGB_EKF_T
#ifndef struct_tag_slcl1F8eFYfz9OcJekEB9zC
#define struct_tag_slcl1F8eFYfz9OcJekEB9zC
--- a/headers/Simulink/rtwtypes.h Wed Mar 08 18:49:52 2017 +0000 +++ b/headers/Simulink/rtwtypes.h Thu Jul 13 10:50:13 2017 +0000 @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'EKF'. // -// Model version : 1.423 +// Model version : 1.439 // Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Mar 6 09:47:51 2017 +// C/C++ source code generated on : Wed Mar 15 14:31:33 2017 // // Target selection: ert.tlc // Embedded hardware selection: STMicroelectronics->ST10/Super10
--- a/main.cpp Wed Mar 08 18:49:52 2017 +0000
+++ b/main.cpp Thu Jul 13 10:50:13 2017 +0000
@@ -97,27 +97,13 @@
// INPUT THE DATA TO THE EKF_STEP
- //ADJUST FOR THE CIRCUIT GAIN
- // StoreVoltage[iter]=Result_V/CircuitGain;
- //StoreCurrent[iter]=(Result_I/CircuitGain-MaxdSpaceOut/2)*(2*MaxCurrentAmp/MaxdSpaceOut);
- // StoreCurrent[iter]=(Result_I/0.54725-MaxdSpaceOut/2)*(2*MaxCurrentAmp/MaxdSpaceOut)-0.7;
- // StoreTemperature[iter]=Result_T*(MaxTemperature/MaxdSpaceOut)/CircuitGain;
- // StoreVoltage[iter]=Result_V*1.8263;
-
+
StoreVoltage[iter]=Result_V*1.8272;
StoreCurrent[iter]=(Result_I*1.8225-MaxdSpaceOut/2)*(2*MaxCurrentAmp/MaxdSpaceOut);
StoreTemperature[iter]=Result_T*1.8245*(MaxTemperature/MaxdSpaceOut);
}
- // MeanVoltage=GetMean(StoreVoltage)/0.9925/1.0121;
- //MeanCurrent=GetMean(StoreCurrent)/0.994/1.0001/1.0562;
- // MeanCurrent=GetMean(StoreCurrent)/1.0384/1.0602/0.894/0.9512;
- // MeanTemperature=GetMean(StoreTemperature)/1.0064/1.0003/0.9995;
-
-
- // MeanVoltage=GetMean(StoreVoltage)/1.0049/1.0001/1.0001/0.9953;
- //MeanCurrent=GetMean(StoreCurrent)/0.9679/0.8893/1.16/1.0592/0.9231/1.0794;
- // MeanTemperature=GetMean(StoreTemperature)/1.008/0.9956/1.043/0.9569/1.0063/0.998/1.0019/0.9927;
+
MeanVoltage=GetMean(StoreVoltage)/1.0005;
MeanCurrent=GetMean(StoreCurrent)/1.003;
--- 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;