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:
- 22:caf41c9bb9d0
- Parent:
- 21:534cd02b6bc4
- Child:
- 23:447ef1071e49
--- a/source/Simulink/EKF.cpp Fri Mar 03 19:00:02 2017 +0000
+++ b/source/Simulink/EKF.cpp Wed Mar 08 18:49:52 2017 +0000
@@ -7,9 +7,9 @@
//
// Code generated for Simulink model 'EKF'.
//
-// Model version : 1.408
+// Model version : 1.425
// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016
-// C/C++ source code generated on : Wed Mar 1 17:25:41 2017
+// C/C++ source code generated on : Wed Mar 8 10:23:12 2017
//
// Target selection: ert.tlc
// Embedded hardware selection: STMicroelectronics->ST10/Super10
@@ -35,6 +35,7 @@
// 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 void EKF_InterpolateR0(real_T SOC, real_T MeasTemp, real_T XhatParam,
@@ -171,6 +172,109 @@
}
// Function for MATLAB Function: '<Root>/MATLAB Function'
+static real_T EKF_interp2_p(real_T varargin_4, real_T varargin_5)
+{
+ 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)) {
+ 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]) {
+ low_i = mid_i - 1;
+ low_ip1 = mid_i + 1;
+ } else {
+ high_i = mid_i;
+ }
+ }
+
+ 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]) {
+ qx1 = V[((low_i << 2) + low_ip1) - 1];
+ Vq = V[(low_i << 2) + low_ip1];
+ } else if (b[low_i + 1] == varargin_4) {
+ 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]);
+ if (V[(((low_i + 1) << 2) + low_ip1) - 1] == V[((low_i << 2) + low_ip1) -
+ 1]) {
+ qx1 = V[((low_i << 2) + low_ip1) - 1];
+ } else {
+ qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1] * rx + V[((low_i << 2) +
+ low_ip1) - 1] * (1.0 - rx);
+ }
+
+ if (V[((low_i + 1) << 2) + low_ip1] == V[(low_i << 2) + low_ip1]) {
+ Vq = V[(low_i << 2) + low_ip1];
+ } else {
+ Vq = V[((low_i + 1) << 2) + low_ip1] * rx + V[(low_i << 2) + low_ip1] *
+ (1.0 - rx);
+ }
+ }
+
+ if ((varargin_5 == b_y1) || (qx1 == Vq)) {
+ Vq = qx1;
+ } else {
+ if (!(varargin_5 == y2)) {
+ rx = (varargin_5 - (real_T)b_y1) / (real_T)(y2 - b_y1);
+ Vq = (1.0 - rx) * qx1 + rx * Vq;
+ }
+ }
+ } else {
+ Vq = (rtNaN);
+ }
+
+ 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)
{
@@ -321,9 +425,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:261' zData=fliplr(ParamData.zR0);
- // '<S1>:1:262' SlopeData= fliplr(ParamData.SlopeR0);
- // '<S1>:1:264' InterpSlope=interp1(zData,SlopeData,SOC);
+ // '<S1>:1:274' zData=fliplr(ParamData.zR0);
+ // '<S1>:1:275' SlopeData= fliplr(ParamData.SlopeR0);
+ // '<S1>:1:277' InterpSlope=interp1(zData,SlopeData,SOC);
InterpSlope = (rtNaN);
if ((!rtIsNaN(SOC)) && (!((SOC > 1.0) || (SOC < 0.0)))) {
low_i = 0;
@@ -351,13 +455,13 @@
}
}
- // '<S1>:1:266' ParamOffset=XhatParam-InterpSlope*MeasTemp;
- // '<S1>:1:267' Param25Value=InterpSlope*25+ParamOffset;
+ // '<S1>:1:279' ParamOffset=XhatParam-InterpSlope*MeasTemp;
+ // '<S1>:1:280' Param25Value=InterpSlope*25+ParamOffset;
*EstimatedT25R0 = (XhatParam - InterpSlope * MeasTemp) + InterpSlope * 25.0;
- // '<S1>:1:270' EstimatedT25R0=Param25Value;
+ // '<S1>:1:283' EstimatedT25R0=Param25Value;
// DETERMINE THE CORRECT R0 AT 25 VALUE
- // '<S1>:1:274' TrueT25R0=interp2(ParamData.R0d_x,ParamData.R0d_y,ParamData.R0_map,SOC,25);
+ // '<S1>:1:287' 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;
@@ -415,7 +519,7 @@
*TrueT25R0 = (rtNaN);
}
- // '<S1>:1:276' DegradationIndex=EstimatedT25R0/TrueT25R0;
+ // '<S1>:1:289' DegradationIndex=EstimatedT25R0/TrueT25R0;
*DegradationIndex = *EstimatedT25R0 / *TrueT25R0;
}
@@ -850,29 +954,29 @@
int16_T i_0;
real_T Current_0[3];
- // '<S1>:1:229' LoadedData=coder.load('ParamData.mat');
- // '<S1>:1:230' ParamData=LoadedData.ParamData;
- // '<S1>:1:232' A=Power.Ahat;
- // '<S1>:1:233' Xhat=Power.Xhat_;
- // '<S1>:1:234' TempBattInt=Power.Temp;
- // '<S1>:1:235' Iterations=Power.Iterations;
- // '<S1>:1:236' B=Power.Bhat;
- // '<S1>:1:237' R0=Power.R0;
- // '<S1>:1:239' PolynomialA=0*A;
+ // '<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;
for (i_0 = 0; i_0 < 9; i_0++) {
PolynomialA[i_0] = 0.0 * Power_Ahat[i_0];
}
- // '<S1>:1:241' for i=0:1:(Iterations-1)
+ // '<S1>:1:254' for i=0:1:(Iterations-1)
for (i = 0; i < (int16_T)((Power_Iterations - 1.0) + 1.0); i++) {
- // '<S1>:1:242' PolynomialA=PolynomialA+A^i;
+ // '<S1>:1:255' PolynomialA=PolynomialA+A^i;
EKF_mpower(Power_Ahat, (real_T)i, tmp);
for (i_0 = 0; i_0 < 9; i_0++) {
PolynomialA[i_0] += tmp[i_0];
}
}
- // '<S1>:1:245' PredictedStates=A^(Iterations)*Xhat+PolynomialA*B.*[Current Current 0]';
+ // '<S1>:1:258' PredictedStates=A^(Iterations)*Xhat+PolynomialA*B.*[Current Current 0]';
EKF_mpower(Power_Ahat, Power_Iterations, tmp);
Current_0[0] = Current;
Current_0[1] = Current;
@@ -884,8 +988,8 @@
Power_Bhat[0])) * Current_0[i_0];
}
- // '<S1>:1:247' OCVatSOC= interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,PredictedStates(1),TempBattInt);
- // '<S1>:1:249' TerminalVoltage=OCVatSOC -PredictedStates(2) -R0*(Current);
+ // '<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;
return TerminalVoltage;
@@ -941,22 +1045,19 @@
void EKF_step(void)
{
real_T C1;
+ real_T tau;
real_T Ahat[9];
real_T Bhat[3];
+ real_T Cth;
+ real_T KGainTh;
real_T C[3];
real_T KGain[3];
+ real_T OCVatSOC_power;
real_T dOCVatSOC_power;
real_T A;
real_T PolynomialA;
- 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;
- real_T ry;
+ int16_T ixstart;
+ int16_T ix;
static const real_T d[76] = { 1.2199999999999978, 1.2699999999999967,
1.2900000000000034, 1.3100000000000012, 1.12, 1.169999999999999, 1.2,
1.2100000000000033, 1.04, 1.0900000000000034, 1.1199999999999957,
@@ -988,36 +1089,6 @@
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 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 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 };
-
- real_T tmp[4];
real_T Ahat_0[3];
real_T Ahat_1[9];
real_T Ahat_2[9];
@@ -1032,76 +1103,85 @@
// Inport: '<Root>/TimeStep'
// Inport: '<Root>/Voltage'
- // DECIDE WHAT CAPACITY TO USE IN BATTERY SIMULATION AND WHAT IN ESTIMATION
+ // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ // INITIALISE VARIABLES
// MATLAB Function 'MATLAB Function': '<S1>:1'
- // DECIDE ON PROCESS AND MEASUREMENT NOISE COVARIANCES
- // '<S1>:1:8' Q=1;
- // PROCESS NOISE (CURRENT)
- // '<S1>:1:9' R=0.01;
- // SENSOR NOISE (VOLTAGE) THIS NEEDS TO BE FAIRLY LARGE TO ALLOW TO CONVERGE
- // '<S1>:1:12' InitialSOC=0.8;
- // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- // INITIALISE ESTIMATION VECTORS AND P0 MATRICES
- // '<S1>:1:22' LoadedData=coder.load('ParamData.mat');
- // '<S1>:1:23' ParamData=LoadedData.ParamData;
- // '<S1>:1:26' if isempty(Count)
+ // '<S1>:1:13' LoadedData=coder.load('ParamData.mat');
+ // '<S1>:1:14' ParamData=LoadedData.ParamData;
+ //
+ // '<S1>:1:18' if isempty(Count)
if (!EKF_DW.Count_not_empty) {
- // '<S1>:1:27' Count=0;
+ // '<S1>:1:20' Count=0;
EKF_DW.Count = 0.0;
EKF_DW.Count_not_empty = true;
- // '<S1>:1:29' Xhat=[0.5 0.1 -6.76]';
- // '<S1>:1:30' P0=[0.1 0 0;
- // '<S1>:1:31' 0 0.05 0;
- // '<S1>:1:32' 0 0 .1];
- // '<S1>:1:34' Qhat=53;
- // '<S1>:1:35' ThetaHat=Qhat;
- EKF_DW.ThetaHat = EKF_DW.Qhat;
+ // 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;
+ // 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:36' P0th=2;
- // '<S1>:1:37' XhatTh=InitialSOC;
- // '<S1>:1:38' CthPrev=0;
- // '<S1>:1:39' iQ=1;
- // '<S1>:1:40' UpdateMacro=60*20;
- // '<S1>:1:42' R0_filt=exp(Xhat(3));
- EKF_DW.R0_filt = EKF_DW.Xhat[2];
- EKF_DW.R0_filt = std::exp(EKF_DW.R0_filt);
+ // 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;
+ // Process Noise
+ // '<S1>:1:41' Rth=1e-6;
+ // SENSOR NOISE
+ // '<S1>:1:42' P0th=10*Qth;
+ EKF_DW.P0th = 10.0 * EKF_DW.Qth;
- // '<S1>:1:43' R0_filt_prev=exp(Xhat(3));
+ // Resistance
+ // '<S1>:1:45' 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:45' Current_prev=0;
- // '<S1>:1:48' Power.iMaxDisch=424;
+ // '<S1>:1:46' Current_prev_R=0;
+ // '<S1>:1:47' alpha=0.999;
+ // Power
+ // '<S1>:1:51' Power.iMaxDisch=424;
EKF_DW.Power.iMaxDisch = 424.0;
// for max 10s otherwise mac is 265
- // '<S1>:1:49' Power.iMaxCh=-100;
+ // '<S1>:1:52' Power.iMaxCh=-100;
EKF_DW.Power.iMaxCh = -100.0;
- // '<S1>:1:50' Power.MaxSOC=0.95;
+ // '<S1>:1:53' Power.MaxSOC=0.95;
EKF_DW.Power.MaxSOC = 0.95;
- // '<S1>:1:51' Power.MinSOC=0.05;
+ // '<S1>:1:54' Power.MinSOC=0.05;
EKF_DW.Power.MinSOC = 0.05;
- // '<S1>:1:52' Power.Vmax=4.2;
+ // '<S1>:1:55' Power.Vmax=4.2;
EKF_DW.Power.Vmax = 4.2;
- // '<S1>:1:53' Power.Vmin=2.7;
+ // '<S1>:1:56' Power.Vmin=2.7;
EKF_DW.Power.Vmin = 2.7;
- // '<S1>:1:54' Power.Time=0;
+ // '<S1>:1:57' Power.Time=0;
EKF_DW.Power.Time = 0.0;
- // '<S1>:1:55' Power.Ahat=zeros(3,3);
+ // '<S1>:1:58' Power.Ahat=zeros(3,3);
memset(&EKF_DW.Power.Ahat[0], 0, 9U * sizeof(real_T));
- // '<S1>:1:56' Power.Bhat=[0 0 0]';
- // '<S1>:1:57' Power.Temp=0;
+ // '<S1>:1:59' Power.Bhat=[0 0 0]';
+ // '<S1>:1:60' Power.Temp=0;
EKF_DW.Power.Temp = 0.0;
- // '<S1>:1:58' Power.Xhat_=Xhat;
+ // '<S1>:1:61' 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;
@@ -1109,109 +1189,45 @@
EKF_DW.Power.Bhat[2] = 0.0;
EKF_DW.Power.Xhat_[2] = EKF_DW.Xhat[2];
- // '<S1>:1:59' Power.TimeStep=0;
+ // '<S1>:1:62' Power.TimeStep=0;
EKF_DW.Power.TimeStep = 0.0;
- // '<S1>:1:60' Power.Iterations=0;
+ // '<S1>:1:63' Power.Iterations=0;
EKF_DW.Power.Iterations = 0.0;
- // '<S1>:1:61' Power.R0=0;
+ // '<S1>:1:64' Power.R0=0;
EKF_DW.Power.R0 = 0.0;
- // '<S1>:1:62' PowerDischHat=0;
- // '<S1>:1:63' PowerChHat=0;
- // '<S1>:1:64' iP=1;
+ // '<S1>:1:65' PowerDischHat=0;
+ // '<S1>:1:66' PowerChHat=0;
+ // '<S1>:1:67' iP=1;
+ // '<S1>:1:68' Power.TimeWindow=10;
+ EKF_DW.Power.TimeWindow = 10.0;
+
+ // '<S1>:1:69' UpdatePower=1;
} else {
- // '<S1>:1:66' else
- // '<S1>:1:67' Count=Count+1;
+ // '<S1>:1:70' else
+ // '<S1>:1:71' Count=Count+1;
EKF_DW.Count++;
}
- // '<S1>:1:71' Power.TimeWindow=10;
- EKF_DW.Power.TimeWindow = 10.0;
-
- // '<S1>:1:72' UpdatePower=1;
- // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- // INITIALISE VARIABLE FOR CAPACITY ESTIMATION
- // IN SECONDS
- // Qth=Qhat*1/100*4;
- // '<S1>:1:80' Qth=5;
- // '<S1>:1:81' Rth=1e-6;
- // SENSOR NOISE
+ //
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// INTERPOLATE PARAMETRES
- // '<S1>:1:85' C1=interp2(ParamData.C1d_x,ParamData.C1d_y,ParamData.C1_map,Xhat(1),TemperatureIn);
+ // '<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:86' R1=interp2(ParamData.R1d_x,ParamData.R1d_y,ParamData.R1_map,Xhat(1),TemperatureIn);
- if ((EKF_DW.Xhat[0] >= 0.05) && (EKF_DW.Xhat[0] <= 0.95) &&
- ((EKF_U.TemperatureIn >= 15.0) && (EKF_U.TemperatureIn <= 45.0))) {
- low_i = 0;
- low_ip1 = 2;
- high_i = 17;
- while (high_i > low_ip1) {
- mid_i = ((low_i + high_i) + 1) >> 1;
- if (EKF_DW.Xhat[0] >= h[mid_i - 1]) {
- low_i = mid_i - 1;
- low_ip1 = mid_i + 1;
- } else {
- high_i = mid_i;
- }
- }
-
- tmp[0] = 15.0;
- tmp[1] = 25.0;
- tmp[2] = 35.0;
- tmp[3] = 45.0;
- low_ip1 = EKF_bsearch(tmp, EKF_U.TemperatureIn);
- b_y1 = (low_ip1 - 1) * 10L + 15L;
- y2 = 10L * low_ip1 + 15L;
- if (EKF_DW.Xhat[0] == h[low_i]) {
- qx1 = V[((low_i << 2) + low_ip1) - 1];
- rx = V[(low_i << 2) + low_ip1];
- } else if (h[low_i + 1] == EKF_DW.Xhat[0]) {
- qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1];
- rx = V[((low_i + 1) << 2) + low_ip1];
- } else {
- rx = (EKF_DW.Xhat[0] - h[low_i]) / (h[low_i + 1] - h[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];
- } else {
- qx1 = V[(((low_i + 1) << 2) + low_ip1) - 1] * rx + V[((low_i << 2) +
- low_ip1) - 1] * (1.0 - rx);
- }
-
- if (V[((low_i + 1) << 2) + low_ip1] == V[(low_i << 2) + low_ip1]) {
- rx = V[(low_i << 2) + low_ip1];
- } else {
- rx = V[((low_i + 1) << 2) + low_ip1] * rx + V[(low_i << 2) + low_ip1] *
- (1.0 - rx);
- }
- }
-
- if ((EKF_U.TemperatureIn == b_y1) || (qx1 == rx)) {
- rx = qx1;
- } else {
- if (!(EKF_U.TemperatureIn == y2)) {
- ry = (EKF_U.TemperatureIn - (real_T)b_y1) / (real_T)(y2 - b_y1);
- rx = (1.0 - ry) * qx1 + ry * rx;
- }
- }
- } else {
- rx = (rtNaN);
- }
-
- // '<S1>:1:87' tau=C1*R1;
- qx1 = C1 * rx;
+ // '<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);
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// DISCRETE TIME SS MATRICES
- // '<S1>:1:91' Ahat=[1 0 0;
- // '<S1>:1:92' 0 exp(-TimeStep/tau) 0;
- // '<S1>:1:93' 0 0 1;];
+ // '<S1>:1:87' Ahat=[1 0 0;
+ // '<S1>:1:88' 0 exp(-TimeStep/tau) 0;
+ // '<S1>:1:89' 0 0 1;];
Ahat[1] = 0.0;
- Ahat[4] = std::exp(-EKF_U.TimeStep / qx1);
+ Ahat[4] = std::exp(-EKF_U.TimeStep / tau);
Ahat[7] = 0.0;
Ahat[0] = 1.0;
Ahat[2] = 0.0;
@@ -1220,10 +1236,10 @@
Ahat[6] = 0.0;
Ahat[8] = 1.0;
- // '<S1>:1:96' Bhat=[-TimeStep/(Qhat*3600)
- // '<S1>:1:97' TimeStep/C1
- // '<S1>:1:98' 1];
- Bhat[0] = -EKF_U.TimeStep / (EKF_DW.Qhat * 3600.0);
+ // '<S1>:1:92' Bhat=[-TimeStep/(Qhat)
+ // '<S1>:1:93' TimeStep/C1
+ // '<S1>:1:94' 1];
+ Bhat[0] = -EKF_U.TimeStep / EKF_DW.Qhat;
Bhat[1] = EKF_U.TimeStep / C1;
Bhat[2] = 1.0;
@@ -1231,162 +1247,186 @@
// PREDICTION STAGE
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// SOC ESTIMATION FOR CAPACITY ESTIMATION
- // '<S1>:1:105' if MacroTime>5*60
- if (EKF_U.MacroTime > 300.0) {
- // '<S1>:1:106' XhatTh=XhatTh-TimeStep/(Qhat*3600)*Current_prev;
- EKF_DW.XhatTh -= EKF_U.TimeStep / (EKF_DW.Qhat * 3600.0) *
- EKF_DW.Current_prev;
+ // '<S1>:1:101' if MacroTime>10*60
+ if (EKF_U.MacroTime > 600.0) {
+ // '<S1>:1:102' SumCC=(Current_prev*TimeStep)+SumCC;
+ EKF_DW.SumCC += EKF_DW.Current_prev * EKF_U.TimeStep;
+
+ // '<S1>:1:103' AbsSumCC=abs(Current_prev*TimeStep)+AbsSumCC;
+ EKF_DW.AbsSumCC += std::abs(EKF_DW.Current_prev * EKF_U.TimeStep);
+
+ // '<S1>:1:104' TimeToUpdateQ=TimeToUpdateQ+TimeStep;
+ EKF_DW.TimeToUpdateQ += EKF_U.TimeStep;
} else {
- // '<S1>:1:107' else
- // '<S1>:1:108' XhatTh=Xhat(1);
+ // '<S1>:1:105' else
+ // '<S1>:1:106' XhatTh=Xhat(1);
EKF_DW.XhatTh = EKF_DW.Xhat[0];
}
- // '<S1>:1:112' if MacroTime>UpdateMacro*iQ
- if (EKF_U.MacroTime > EKF_DW.UpdateMacro * EKF_DW.iQ) {
- // MACROSCALE EKF FOR CAPACITY ESTIMATION
- // '<S1>:1:114' P0th=P0th+Qth;
- EKF_DW.P0th += 5.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' Cth=+Current_prev*TimeStep/(ThetaHat^2)+ CthPrev;
- rx = EKF_DW.Current_prev * EKF_U.TimeStep / (EKF_DW.ThetaHat *
- EKF_DW.ThetaHat) + EKF_DW.CthPrev;
+ // '<S1>:1:115' P0th=P0th+Qth;
+ EKF_DW.P0th += EKF_DW.Qth;
- // '<S1>:1:116' KGainTh=P0th*(Cth')*(Cth*P0th*Cth'+Rth)^-1;
- ry = 1.0 / (rx * EKF_DW.P0th * rx + 1.0E-6) * (EKF_DW.P0th * rx);
+ // '<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:117' ThetaHat=ThetaHat+KGainTh*(Xhat(1)-XhatTh(1));
- EKF_DW.ThetaHat += (EKF_DW.Xhat[0] - EKF_DW.XhatTh) * ry;
+ // '<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:118' P0th=P0th-KGainTh*Cth*KGainTh';
- EKF_DW.P0th -= ry * rx * ry;
+ // '<S1>:1:124' P0th=P0th-KGainTh*Cth*KGainTh';
+ EKF_DW.P0th -= KGainTh * Cth * KGainTh;
- // '<S1>:1:119' CthPrev=Cth;
- EKF_DW.CthPrev = rx;
+ // '<S1>:1:125' CthPrev=Cth;
+ EKF_DW.CthPrev = Cth;
- // '<S1>:1:120' Qhat=ThetaHat
- EKF_DW.Qhat = EKF_DW.ThetaHat;
+ // '<S1>:1:127' Qhat/3600;
+ // '<S1>:1:129' iQ=iQ+1;
+ EKF_DW.iQ++;
- // THIS HAS TO BE DONE TO AVOID OVERSHOOTING
- // '<S1>:1:122' XhatTh=Xhat(1);
+ // '<S1>:1:131' XhatTh=Xhat(1);
EKF_DW.XhatTh = EKF_DW.Xhat[0];
- // '<S1>:1:123' iQ=iQ+1;
- EKF_DW.iQ++;
+ // '<S1>:1:133' AbsSumCC=0;
+ EKF_DW.AbsSumCC = 0.0;
+
+ // '<S1>:1:134' SumCC=0;
+ EKF_DW.SumCC = 0.0;
+
+ // '<S1>:1:135' TimeToUpdateQ=0;
+ EKF_DW.TimeToUpdateQ = 0.0;
}
- // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ //
// PREDICTION FOR STATES
- // '<S1>:1:129' Xhat=Ahat*Xhat+Bhat.*[Current_prev Current_prev 0]';
+ // '<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 (low_i = 0; low_i < 3; low_i++) {
- Ahat_0[low_i] = ((Ahat[low_i + 3] * EKF_DW.Xhat[1] + Ahat[low_i] *
- EKF_DW.Xhat[0]) + Ahat[low_i + 6] * EKF_DW.Xhat[2]) +
- Bhat[low_i] * KGain[low_i];
+ 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:130' P0=Ahat*P0*Ahat' + Bhat*Q*Bhat';
- for (low_i = 0; low_i < 3; low_i++) {
- EKF_DW.Xhat[low_i] = Ahat_0[low_i];
- for (low_ip1 = 0; low_ip1 < 3; low_ip1++) {
- Ahat_1[low_i + 3 * low_ip1] = 0.0;
- Ahat_1[low_i + 3 * low_ip1] += EKF_DW.P0[3 * low_ip1] * Ahat[low_i];
- Ahat_1[low_i + 3 * low_ip1] += EKF_DW.P0[3 * low_ip1 + 1] * Ahat[low_i + 3];
- Ahat_1[low_i + 3 * low_ip1] += EKF_DW.P0[3 * low_ip1 + 2] * Ahat[low_i + 6];
+ // '<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];
}
- for (low_ip1 = 0; low_ip1 < 3; low_ip1++) {
- Ahat_2[low_i + 3 * low_ip1] = 0.0;
- Ahat_2[low_i + 3 * low_ip1] += Ahat_1[low_i] * Ahat[low_ip1];
- Ahat_2[low_i + 3 * low_ip1] += Ahat_1[low_i + 3] * Ahat[low_ip1 + 3];
- Ahat_2[low_i + 3 * low_ip1] += Ahat_1[low_i + 6] * Ahat[low_ip1 + 6];
- Bhat_0[low_i + 3 * low_ip1] = Bhat[low_i] * Bhat[low_ip1];
+ 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:133' Ytrue=Voltage;
+ // '<S1>:1:143' Current_prev=Current;
+ EKF_DW.Current_prev = EKF_U.Current;
+
+ // '<S1>:1:145' Ytrue=Voltage;
// ESTIMATION STAGE
// INTERPOLATE CORRECT VALUES
- // '<S1>:1:139' OCVatSOC=interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,Xhat(1),TemperatureIn);
- // '<S1>:1:140' dOCVatSOC=interp2(ParamData.dOCVdatad_x,ParamData.dOCVdatad_y,ParamData.dOCVdata_map,Xhat(1),TemperatureIn);
- // '<S1>:1:143' Yhat=OCVatSOC -Xhat(2) -exp(Xhat(3))*Current;
- // '<S1>:1:144' C=[dOCVatSOC -1 -exp(Xhat(3))*Current];
- ry = EKF_interp2_dispatch(d, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN));
+ // '<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));
C[1] = -1.0;
C[2] = -std::exp(EKF_DW.Xhat[2]) * EKF_U.Current;
- // '<S1>:1:145' KGain=P0*C'*(C*P0*C'+R)^-1;
- dOCVatSOC_power = 0.0;
- for (low_i = 0; low_i < 3; low_i++) {
- EKF_DW.P0[3 * low_i] = Ahat_2[3 * low_i] + Bhat_0[3 * low_i];
- rx = EKF_DW.P0[3 * low_i] * ry;
- EKF_DW.P0[1 + 3 * low_i] = Ahat_2[3 * low_i + 1] + Bhat_0[3 * low_i + 1];
- rx += -EKF_DW.P0[3 * low_i + 1];
- EKF_DW.P0[2 + 3 * low_i] = Ahat_2[3 * low_i + 2] + Bhat_0[3 * low_i + 2];
- rx += EKF_DW.P0[3 * low_i + 2] * C[2];
- dOCVatSOC_power += rx * C[low_i];
- }
-
- rx = 1.0 / (dOCVatSOC_power + 0.01);
- for (low_i = 0; low_i < 3; low_i++) {
- KGain[low_i] = (EKF_DW.P0[low_i + 6] * C[2] + (-EKF_DW.P0[low_i + 3] +
- EKF_DW.P0[low_i] * ry)) * rx;
+ // '<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:146' Xhat=Xhat+KGain*(Ytrue-Yhat);
- rx = 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]) *
- EKF_U.Current);
-
- // '<S1>:1:147' P0=P0-KGain*(C*P0*C'+R)*KGain';
- dOCVatSOC_power = 0.0;
- for (low_i = 0; low_i < 3; low_i++) {
- EKF_DW.Xhat[low_i] += KGain[low_i] * rx;
- dOCVatSOC_power += (EKF_DW.P0[3 * low_i + 2] * C[2] + (-EKF_DW.P0[3 * low_i
- + 1] + EKF_DW.P0[3 * low_i] * ry)) * C[low_i];
+ 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;
}
- for (low_i = 0; low_i < 3; low_i++) {
- EKF_DW.P0[low_i] -= (dOCVatSOC_power + 0.01) * KGain[low_i] * KGain[0];
- EKF_DW.P0[low_i + 3] -= (dOCVatSOC_power + 0.01) * KGain[low_i] * KGain[1];
- EKF_DW.P0[low_i + 6] -= (dOCVatSOC_power + 0.01) * KGain[low_i] * KGain[2];
+ // '<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]) *
+ 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];
+ }
+
+ 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];
}
// R0 FILTER
- // '<S1>:1:152' DI=Current-Current_prev;
- // '<S1>:1:153' Current_prev=Current;
- EKF_DW.Current_prev = EKF_U.Current;
+ // '<S1>:1:164' 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));
+ Cth = (1.0 - EKF_DW.alpha) * std::exp(EKF_DW.Xhat[2]) + EKF_DW.R0_filt_prev *
+ EKF_DW.alpha;
- // '<S1>:1:155' alpha=0.995;
- // '<S1>:1:156' if abs(Current)>20
- if (std::abs(EKF_U.Current) > 5.0) {
- // '<S1>:1:158' R0_filt=R0_filt_prev*alpha + (1-alpha)*exp(Xhat(3));
- EKF_DW.R0_filt = EKF_DW.R0_filt_prev * 0.995 + 0.0050000000000000044 * std::
- exp(EKF_DW.Xhat[2]);
-
- // '<S1>:1:159' R0_filt_prev=R0_filt;
- EKF_DW.R0_filt_prev = EKF_DW.R0_filt;
+ // '<S1>:1:167' R0_filt_prev=R0_filt;
+ EKF_DW.R0_filt_prev = Cth;
+ } else {
+ // '<S1>:1:168' else
+ // '<S1>:1:169' R0_filt=R0_filt_prev;
+ Cth = EKF_DW.R0_filt_prev;
}
- // '<S1>:1:162' [~,~,DegradationIndex]=InterpolateR0(Xhat(1),TemperatureIn, R0_filt,ParamData);
- EKF_InterpolateR0(EKF_DW.Xhat[0], EKF_U.TemperatureIn, EKF_DW.R0_filt, &ry,
- &dOCVatSOC_power, &rx);
+ // '<S1>:1:174' [~,~,DegradationIndex]=InterpolateR0(Xhat(1),TemperatureIn, R0_filt,ParamData);
+ EKF_InterpolateR0(EKF_DW.Xhat[0], EKF_U.TemperatureIn, Cth, &OCVatSOC_power,
+ &dOCVatSOC_power, &KGainTh);
- // '<S1>:1:162' ~
+ // '<S1>:1:174' ~
+ //
// POWER ESTIMATION
- // '<S1>:1:166' if MacroTime>UpdatePower*iP
- if (EKF_U.MacroTime > EKF_DW.iP) {
- // '<S1>:1:168' Power.Ahat=Ahat;
+ // '<S1>:1:178' if MacroTime>UpdatePower*iP
+ if (EKF_U.MacroTime > EKF_DW.UpdatePower * EKF_DW.iP) {
+ // '<S1>:1:180' Current_prev_R=Current;
+ EKF_DW.Current_prev_R = EKF_U.Current;
+
+ // '<S1>:1:182' Power.Ahat=Ahat;
memcpy(&EKF_DW.Power.Ahat[0], &Ahat[0], 9U * sizeof(real_T));
- // '<S1>:1:169' Power.Bhat=Bhat;
- // '<S1>:1:170' Power.Temp=TemperatureIn;
+ // '<S1>:1:183' Power.Bhat=Bhat;
+ // '<S1>:1:184' Power.Temp=TemperatureIn;
EKF_DW.Power.Temp = EKF_U.TemperatureIn;
- // '<S1>:1:171' Power.Xhat_=Xhat;
+ // '<S1>:1:185' 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];
@@ -1394,140 +1434,142 @@
EKF_DW.Power.Bhat[2] = 1.0;
EKF_DW.Power.Xhat_[2] = EKF_DW.Xhat[2];
- // '<S1>:1:172' Power.TimeStep=TimeStep;
+ // '<S1>:1:186' Power.TimeStep=TimeStep;
EKF_DW.Power.TimeStep = EKF_U.TimeStep;
- // '<S1>:1:173' Power.Iterations=50;
+ // '<S1>:1:187' Power.Iterations=50;
EKF_DW.Power.Iterations = 50.0;
// 50 iterations at 0.2s time step is 10seconds
- // '<S1>:1:174' Power.R0=R0_filt;
- EKF_DW.Power.R0 = EKF_DW.R0_filt;
+ // '<S1>:1:188' Power.R0=R0_filt;
+ EKF_DW.Power.R0 = Cth;
- // '<S1>:1:176' iChSOC=(Xhat(1)-Power.MaxSOC)*(Qhat*3600)/(Power.TimeWindow);
- // '<S1>:1:177' iDischSOC=(Xhat(1)-Power.MinSOC)*(Qhat*3600)/(Power.TimeWindow);
- // '<S1>:1:179' OCVatSOC_power=interp2(ParamData.OCVdatad_x,ParamData.OCVdatad_y,ParamData.OCVdata_map,Xhat(1),TemperatureIn);
- ry = EKF_interp2_dispatch(e, EKF_DW.Xhat[0], EKF_U.TemperatureIn, (rtNaN));
+ // '<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:180' dOCVatSOC_power=interp2(ParamData.dOCVdatad_x,ParamData.dOCVdatad_y,ParamData.dOCVdata_map,Xhat(1),TemperatureIn);
+ // '<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:182' A=exp(-Power.TimeWindow/tau);
- A = std::exp(-EKF_DW.Power.TimeWindow / qx1);
+ // '<S1>:1:196' A=exp(-Power.TimeWindow/tau);
+ A = std::exp(-EKF_DW.Power.TimeWindow / tau);
- // '<S1>:1:183' PolynomialA=0;
+ // '<S1>:1:197' PolynomialA=0;
PolynomialA = 0.0;
- // '<S1>:1:184' for i=0:1:(Power.Iterations-1)
- for (low_i = 0; low_i < (int16_T)((EKF_DW.Power.Iterations - 1.0) + 1.0);
- low_i++) {
- // '<S1>:1:185' PolynomialA=PolynomialA+A^i;
- PolynomialA += rt_powd_snf(A, (real_T)low_i);
+ // '<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:188' iDischOCV=(OCVatSOC_power-Xhat(2)*exp(-Power.TimeWindow/tau)-Power.Vmin)/...
- // '<S1>:1:189' ((Power.TimeWindow/(Qhat*3600))*dOCVatSOC_power+...
- // '<S1>:1:190' (Power.TimeStep/C1)*PolynomialA+...
- // '<S1>:1:191' Power.R0);
- // '<S1>:1:193' iChOCV=(OCVatSOC_power-Xhat(2)*exp(-Power.TimeWindow/tau)-Power.Vmax)/...
- // '<S1>:1:194' ((Power.TimeWindow/(Qhat*3600))*dOCVatSOC_power+...
- // '<S1>:1:195' (Power.TimeStep/C1)*PolynomialA+...
- // '<S1>:1:196' Power.R0);
- // '<S1>:1:198' IMaxDisch=min([Power.iMaxDisch; iDischOCV; iDischSOC]);
+ // '<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]);
Bhat[0] = EKF_DW.Power.iMaxDisch;
- Bhat[1] = ((ry - std::exp(-EKF_DW.Power.TimeWindow / qx1) * EKF_DW.Xhat[1])
- - EKF_DW.Power.Vmin) / ((EKF_DW.Power.TimeWindow / (EKF_DW.Qhat *
- 3600.0) * 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 * 3600.0) /
+ Bhat[1] = ((OCVatSOC_power - 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;
- low_i = 1;
+ ixstart = 1;
A = EKF_DW.Power.iMaxDisch;
if (rtIsNaN(EKF_DW.Power.iMaxDisch)) {
- low_ip1 = 2;
+ ix = 2;
exitg2 = false;
- while ((!exitg2) && (low_ip1 < 4)) {
- low_i = low_ip1;
- if (!rtIsNaN(Bhat[low_ip1 - 1])) {
- A = Bhat[low_ip1 - 1];
+ while ((!exitg2) && (ix < 4)) {
+ ixstart = ix;
+ if (!rtIsNaN(Bhat[ix - 1])) {
+ A = Bhat[ix - 1];
exitg2 = true;
} else {
- low_ip1++;
+ ix++;
}
}
}
- if (low_i < 3) {
- while (low_i + 1 < 4) {
- if (Bhat[low_i] < A) {
- A = Bhat[low_i];
+ if (ixstart < 3) {
+ while (ixstart + 1 < 4) {
+ if (Bhat[ixstart] < A) {
+ A = Bhat[ixstart];
}
- low_i++;
+ ixstart++;
}
}
- // '<S1>:1:199' IMaxCh=max([Power.iMaxCh;iChOCV;iChSOC]);
+ // '<S1>:1:213' IMaxCh=max([Power.iMaxCh;iChOCV;iChSOC]);
Bhat[0] = EKF_DW.Power.iMaxCh;
- Bhat[1] = ((ry - std::exp(-EKF_DW.Power.TimeWindow / qx1) * EKF_DW.Xhat[1])
- - EKF_DW.Power.Vmax) / ((EKF_DW.Power.TimeWindow / (EKF_DW.Qhat *
- 3600.0) * 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 * 3600.0) /
+ Bhat[1] = ((OCVatSOC_power - 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;
- low_i = 1;
+ ixstart = 1;
C1 = EKF_DW.Power.iMaxCh;
if (rtIsNaN(EKF_DW.Power.iMaxCh)) {
- low_ip1 = 2;
+ ix = 2;
exitg1 = false;
- while ((!exitg1) && (low_ip1 < 4)) {
- low_i = low_ip1;
- if (!rtIsNaN(Bhat[low_ip1 - 1])) {
- C1 = Bhat[low_ip1 - 1];
+ while ((!exitg1) && (ix < 4)) {
+ ixstart = ix;
+ if (!rtIsNaN(Bhat[ix - 1])) {
+ C1 = Bhat[ix - 1];
exitg1 = true;
} else {
- low_ip1++;
+ ix++;
}
}
}
- if (low_i < 3) {
- while (low_i + 1 < 4) {
- if (Bhat[low_i] > C1) {
- C1 = Bhat[low_i];
+ if (ixstart < 3) {
+ while (ixstart + 1 < 4) {
+ if (Bhat[ixstart] > C1) {
+ C1 = Bhat[ixstart];
}
- low_i++;
+ ixstart++;
}
}
- // '<S1>:1:201' VoltTermDisch=PredictTerminalVoltage(IMaxDisch,Power);
- // '<S1>:1:202' VoltTermCh=PredictTerminalVoltage(IMaxCh,Power);
- // '<S1>:1:204' PowerDischHat=abs(IMaxDisch*VoltTermDisch);
+ // '<S1>:1:215' VoltTermDisch=PredictTerminalVoltage(IMaxDisch,Power);
+ // '<S1>:1:216' VoltTermCh=PredictTerminalVoltage(IMaxCh,Power);
+ // '<S1>:1:218' 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:205' PowerChHat=abs(IMaxCh*VoltTermCh);
+ // '<S1>:1:219' 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:206' iP=iP+1;
+ // '<S1>:1:220' iP=iP+1;
EKF_DW.iP++;
}
+ //
// MAKE SURE SOC IS WITHIN LIMITS
- // '<S1>:1:212' if Xhat(1)>1
+ // '<S1>:1:226' if Xhat(1)>1
if (EKF_DW.Xhat[0] > 1.0) {
- // '<S1>:1:213' Xhat(1)=1;
+ // '<S1>:1:227' Xhat(1)=1;
EKF_DW.Xhat[0] = 1.0;
}
- // '<S1>:1:216' if Xhat(1)<0
+ // '<S1>:1:230' if Xhat(1)<0
if (EKF_DW.Xhat[0] < 0.0) {
- // '<S1>:1:217' Xhat(1)=0;
+ // '<S1>:1:231' Xhat(1)=0;
EKF_DW.Xhat[0] = 0.0;
}
@@ -1535,22 +1577,22 @@
// MATLAB Function: '<Root>/MATLAB Function'
// OUTPUT THE VALUES
- // '<S1>:1:221' SOC=Xhat(1);
- // '<S1>:1:222' Q0=ThetaHat;
- // '<S1>:1:223' R0=R0_filt;
- // '<S1>:1:225' PowerDisch=PowerDischHat;
- // '<S1>:1:226' PowerCh=PowerChHat;
+ // '<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;
EKF_Y.SOC = EKF_DW.Xhat[0];
// Outport: '<Root>/Q0' incorporates:
// MATLAB Function: '<Root>/MATLAB Function'
- EKF_Y.Q0 = EKF_DW.ThetaHat;
+ EKF_Y.Q0 = EKF_DW.Qhat / 3600.0;
// Outport: '<Root>/R0' incorporates:
// MATLAB Function: '<Root>/MATLAB Function'
- EKF_Y.R0 = EKF_DW.R0_filt;
+ EKF_Y.R0 = Cth;
// Outport: '<Root>/PowerDisch' incorporates:
// MATLAB Function: '<Root>/MATLAB Function'
@@ -1565,7 +1607,7 @@
// Outport: '<Root>/DegradationIndex' incorporates:
// MATLAB Function: '<Root>/MATLAB Function'
- EKF_Y.DegradationIndex = rx;
+ EKF_Y.DegradationIndex = KGainTh;
}
// Model initialize function
@@ -1595,21 +1637,29 @@
};
// SystemInitialize for MATLAB Function: '<Root>/MATLAB Function'
- EKF_DW.Xhat[0] = 0.5;
+ EKF_DW.Xhat[0] = 0.85;
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.Qhat = 53.0;
- EKF_DW.P0th = 2.0;
- EKF_DW.XhatTh = 0.8;
+ EKF_DW.Current_prev = 0.0;
+ EKF_DW.Q = 1.0;
+ EKF_DW.Qhat = 190800.0;
+ EKF_DW.XhatTh = 0.0;
EKF_DW.CthPrev = 0.0;
EKF_DW.iQ = 1.0;
- EKF_DW.UpdateMacro = 1200.0;
- EKF_DW.Current_prev = 0.0;
+ EKF_DW.SumCC = 0.0;
+ EKF_DW.TimeToUpdateQ = 0.0;
+ EKF_DW.AbsSumCC = 0.0;
+ EKF_DW.UpdateQ = 1200.0;
+ EKF_DW.Qth = 1.44E+7;
+ EKF_DW.Rth = 1.0E-6;
+ EKF_DW.Current_prev_R = 0.0;
+ EKF_DW.alpha = 0.999;
EKF_DW.PowerDischHat = 0.0;
EKF_DW.PowerChHat = 0.0;
EKF_DW.iP = 1.0;
+ EKF_DW.UpdatePower = 1.0;
}
}