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
source/Simulink/EKF.cpp
- Committer:
- fmferrari
- Date:
- 2017-07-13
- Revision:
- 23:447ef1071e49
- Parent:
- 22:caf41c9bb9d0
File content as of revision 23:447ef1071e49:
//
// Academic License - for use in teaching, academic research, and meeting
// course requirements at degree granting institutions only. Not for
// government, commercial, or other organizational use.
//
// File: EKF.cpp
//
// Code generated for Simulink model 'EKF'.
//
// Model version : 1.439
// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016
// C/C++ source code generated on : Wed Mar 15 14:31:33 2017
//
// Target selection: ert.tlc
// Embedded hardware selection: STMicroelectronics->ST10/Super10
// Code generation objectives: Unspecified
// Validation result: Not run
//
#include "EKF.h"
#include "EKF_private.h"
// Block states (auto storage)
DW_EKF_T EKF_DW;
// External inputs (root inport signals with auto storage)
ExtU_EKF_T EKF_U;
// External outputs (root outports fed by signals with auto storage)
ExtY_EKF_T EKF_Y;
// Real-time model
RT_MODEL_EKF_T EKF_M_;
RT_MODEL_EKF_T *const EKF_M = &EKF_M_;
// Forward declaration for local functions
static int16_T EKF_bsearch(const real_T x[4], real_T xi);
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]);
static void EKF_mpower(const real_T a[9], real_T b, real_T c[9]);
static real_T EKF_PredictTerminalVoltage(real_T Current, const real_T
Power_Ahat[9], const real_T Power_Bhat[3], real_T Power_Temp, const real_T
Power_Xhat_[3], real_T Power_Iterations, real_T Power_R0);
// Function for MATLAB Function: '<Root>/MATLAB Function'
static int16_T EKF_bsearch(const real_T x[4], real_T xi)
{
int16_T n;
int16_T low_ip1;
int16_T high_i;
int16_T mid_i;
n = 1;
low_ip1 = 2;
high_i = 4;
while (high_i > low_ip1) {
mid_i = (n + high_i) >> 1;
if (xi >= x[mid_i - 1]) {
n = mid_i;
low_ip1 = mid_i + 1;
} else {
high_i = mid_i;
}
}
return n;
}
// Function for MATLAB Function: '<Root>/MATLAB Function'
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;
int16_T low_i;
int16_T low_ip1;
int16_T high_i;
int16_T mid_i;
real_T qx1;
real_T rx;
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 (Xq >= X[mid_i - 1]) {
low_i = mid_i - 1;
low_ip1 = mid_i + 1;
} else {
high_i = mid_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 (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 - 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];
} 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 ((Y[low_ip1 - 1] == Yq) || (qx1 == Vq)) {
Vq = qx1;
} else {
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 = extrapval;
}
return Vq;
}
// Function for MATLAB Function: '<Root>/MATLAB Function'
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;
int16_T low_i;
int16_T low_ip1;
int16_T high_i;
int16_T mid_i;
real_T qx1;
real_T rx;
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 (Xq >= X[mid_i - 1]) {
low_i = mid_i - 1;
low_ip1 = mid_i + 1;
} else {
high_i = mid_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 (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 - 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];
} 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 ((Y[low_ip1 - 1] == Yq) || (qx1 == Vq)) {
Vq = qx1;
} else {
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 = extrapval;
}
return Vq;
}
// Function for MATLAB Function: '<Root>/MATLAB Function'
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;
int16_T low_i;
int16_T low_ip1;
int16_T high_i;
int16_T mid_i;
real_T qx1;
real_T rx;
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 >= X[mid_i - 1]) {
low_i = mid_i - 1;
low_ip1 = mid_i + 1;
} else {
high_i = mid_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 (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 - 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];
} 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 ((Y[low_ip1 - 1] == Yq) || (qx1 == Vq)) {
Vq = qx1;
} else {
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 = extrapval;
}
return Vq;
}
//
// Function for MATLAB Function: '<Root>/MATLAB Function'
// function [EstimatedT25R0,TrueT25R0,DegradationIndex]=InterpolateR0(SOC,MeasTemp,XhatParam,ParamData)
// THIS SCRIPT INTERPOLATES THE DATA CORRESPONDING TO THE CORRECT SLOPE
// IT NORMALIZES THE RESULT BY DETERMINING THE CORRESPONDING VALUE AT 25°C AT
// THE CURRENT SOC VALUE
//
static void EKF_InterpolateR0(real_T SOC, real_T MeasTemp, real_T XhatParam,
real_T *EstimatedT25R0, real_T *TrueT25R0, real_T *DegradationIndex)
{
real_T InterpSlope;
int16_T low_i;
int16_T low_ip1;
int16_T high_i;
int16_T mid_i;
int32_T b_y1;
int32_T y2;
real_T rx;
static const real_T V[72] = { 0.0017736518359183257, 0.0015094909241858197,
0.0013207048790611728, 0.00113203275348101, 0.0017734510602973419,
0.0014341246178812793, 0.0012453300124533138, 0.001094257037204736,
0.0017359800739678564, 0.0014338540487510473, 0.0012074560410535065,
0.0010943809200347151, 0.0017357180590144073, 0.0013963317986263083,
0.0011697671785970392, 0.0010565240359218181, 0.001735718059014424,
0.00139643719806763, 0.0011321181931393564, 0.00094332503207306454,
0.0016981132075471839, 0.001358593101366142, 0.0010945461407812762,
0.00094328943893145513, 0.0016604400166044017, 0.0013585418317672378,
0.0011321181931393564, 0.00090566037735849132, 0.0016603773584905675,
0.0013586956521739143, 0.0011321181931393564, 0.000943396226415091,
0.0016225190551656535, 0.0013207048790611728, 0.0010566436469300739,
0.00090552369453667447, 0.001584666465439172, 0.0012830188679245377,
0.0010942983283649642, 0.00094336062790083058, 0.0015850851039740279,
0.0013205055649877435, 0.0010565639032489348, 0.00090559203079012987,
0.0015849654703951025, 0.0012830672855579553, 0.0010563247444071386,
0.00090552369453667447, 0.001584786053882719, 0.0012451890423364244,
0.00098116910072079113, 0.00086789177766877213, 0.0015470530525998011,
0.0012075927393486558, 0.00098116910072079113, 0.00083015735255272615,
0.0015093200513168832, 0.0012075927393486558, 0.00098109505301685975,
0.000867859029507212, 0.0014715870500339485, 0.0011698113207547055,
0.00094350303807977929, 0.00083022000830219249, 0.0015094339622641524,
0.0011698996150652763, 0.0009810580333559824, 0.00083009470625966108,
0.0014718091931466412, 0.0012075927393486391, 0.00094346743150424436,
0.00083009470625967788 };
static const real_T b[18] = { 0.05, 0.10294117647058831, 0.15588235294117658,
0.20882352941176474, 0.2617647058823529, 0.31470588235294117,
0.36764705882352944, 0.4205882352941176, 0.47352941176470587,
0.52647058823529413, 0.57941176470588229, 0.63235294117647056,
0.68529411764705883, 0.7382352941176471, 0.79117647058823526,
0.84411764705882353, 0.89705882352941169, 0.95 };
static const real_T c[20] = { 0.0, 0.052631578947368474, 0.10526315789473684,
0.15789473684210531, 0.21052631578947367, 0.26315789473684215,
0.31578947368421051, 0.368421052631579, 0.42105263157894735,
0.47368421052631582, 0.52631578947368429, 0.57894736842105265,
0.631578947368421, 0.68421052631578949, 0.736842105263158,
0.78947368421052633, 0.84210526315789469, 0.89473684210526316,
0.94736842105263164, 1.0 };
static const real_T d[20] = { 7.5474530734005014E-7, -2.1136432924365931E-5,
-2.2263766747057817E-5, -2.151195469496965E-5, -2.2641466893070361E-5,
-2.6414980857523514E-5, -2.5285182664320518E-5, -2.490762556365612E-5,
-2.3775208552609869E-5, -2.4150473140180349E-5, -2.112638052174597E-5,
-2.3024208812905025E-5, -2.2650678687261015E-5, -2.4147027702574732E-5,
-2.3771107387690893E-5, -2.1508807517608091E-5, -2.1504094078701937E-5,
-2.2268593497227676E-5, -2.1892687685052847E-5, -2.0001423964217564E-5 };
real_T tmp[4];
// 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: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;
low_ip1 = 2;
high_i = 20;
while (high_i > low_ip1) {
mid_i = ((low_i + high_i) + 1) >> 1;
if (SOC >= c[mid_i - 1]) {
low_i = mid_i - 1;
low_ip1 = mid_i + 1;
} else {
high_i = mid_i;
}
}
InterpSlope = (SOC - c[low_i]) / (c[low_i + 1] - c[low_i]);
if (InterpSlope == 0.0) {
InterpSlope = d[low_i];
} else if (InterpSlope == 1.0) {
InterpSlope = d[low_i + 1];
} else if (d[low_i + 1] == d[low_i]) {
InterpSlope = d[low_i];
} else {
InterpSlope = (1.0 - InterpSlope) * d[low_i] + d[low_i + 1] * InterpSlope;
}
}
// '<S1>:1:349' ParamOffset=XhatParam-InterpSlope*MeasTemp;
// '<S1>:1:350' Param25Value=InterpSlope*25+ParamOffset;
*EstimatedT25R0 = (XhatParam - InterpSlope * MeasTemp) + InterpSlope * 25.0;
// '<S1>:1:353' EstimatedT25R0=Param25Value;
// DETERMINE THE CORRECT R0 AT 25 VALUE
// '<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;
high_i = 18;
while (high_i > low_ip1) {
mid_i = ((low_i + high_i) + 1) >> 1;
if (SOC >= 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, 25.0);
b_y1 = (low_ip1 - 1) * 10L + 15L;
y2 = 10L * low_ip1 + 15L;
if (SOC == b[low_i]) {
InterpSlope = V[((low_i << 2) + low_ip1) - 1];
*TrueT25R0 = V[(low_i << 2) + low_ip1];
} else if (b[low_i + 1] == SOC) {
InterpSlope = V[(((low_i + 1) << 2) + low_ip1) - 1];
*TrueT25R0 = V[((low_i + 1) << 2) + low_ip1];
} else {
rx = (SOC - 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]) {
InterpSlope = V[((low_i << 2) + low_ip1) - 1];
} else {
InterpSlope = 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]) {
*TrueT25R0 = V[(low_i << 2) + low_ip1];
} else {
*TrueT25R0 = V[((low_i + 1) << 2) + low_ip1] * rx + V[(low_i << 2) +
low_ip1] * (1.0 - rx);
}
}
if ((25L == b_y1) || (InterpSlope == *TrueT25R0)) {
*TrueT25R0 = InterpSlope;
} else {
if (25L != y2) {
rx = (25.0 - (real_T)b_y1) / (real_T)(y2 - b_y1);
*TrueT25R0 = (1.0 - rx) * InterpSlope + rx * *TrueT25R0;
}
}
} else {
*TrueT25R0 = (rtNaN);
}
// '<S1>:1:359' DegradationIndex=EstimatedT25R0/TrueT25R0;
*DegradationIndex = *EstimatedT25R0 / *TrueT25R0;
}
// Function for MATLAB Function: '<Root>/MATLAB Function'
static void EKF_inv(const real_T x[9], real_T y[9])
{
real_T b_x[9];
int16_T p1;
int16_T p2;
int16_T p3;
real_T absx11;
real_T absx21;
real_T absx31;
int16_T itmp;
memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
p1 = 0;
p2 = 3;
p3 = 6;
absx11 = std::abs(x[0]);
absx21 = std::abs(x[1]);
absx31 = std::abs(x[2]);
if ((absx21 > absx11) && (absx21 > absx31)) {
p1 = 3;
p2 = 0;
b_x[0] = x[1];
b_x[1] = x[0];
b_x[3] = x[4];
b_x[4] = x[3];
b_x[6] = x[7];
b_x[7] = x[6];
} else {
if (absx31 > absx11) {
p1 = 6;
p3 = 0;
b_x[0] = x[2];
b_x[2] = x[0];
b_x[3] = x[5];
b_x[5] = x[3];
b_x[6] = x[8];
b_x[8] = x[6];
}
}
absx11 = b_x[1] / b_x[0];
b_x[1] /= b_x[0];
absx21 = b_x[2] / b_x[0];
b_x[2] /= b_x[0];
b_x[4] -= absx11 * b_x[3];
b_x[5] -= absx21 * b_x[3];
b_x[7] -= absx11 * b_x[6];
b_x[8] -= absx21 * b_x[6];
if (std::abs(b_x[5]) > std::abs(b_x[4])) {
itmp = p2;
p2 = p3;
p3 = itmp;
b_x[1] = absx21;
b_x[2] = absx11;
absx11 = b_x[4];
b_x[4] = b_x[5];
b_x[5] = absx11;
absx11 = b_x[7];
b_x[7] = b_x[8];
b_x[8] = absx11;
}
absx11 = b_x[5] / b_x[4];
b_x[5] /= b_x[4];
b_x[8] -= absx11 * b_x[7];
absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8];
absx21 = -(b_x[7] * absx11 + b_x[1]) / b_x[4];
y[p1] = ((1.0 - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0];
y[p1 + 1] = absx21;
y[p1 + 2] = absx11;
absx11 = -b_x[5] / b_x[8];
absx21 = (1.0 - b_x[7] * absx11) / b_x[4];
y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
y[p2 + 1] = absx21;
y[p2 + 2] = absx11;
absx11 = 1.0 / b_x[8];
absx21 = -b_x[7] * absx11 / b_x[4];
y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
y[p3 + 1] = absx21;
y[p3 + 2] = absx11;
}
// Function for MATLAB Function: '<Root>/MATLAB Function'
static void EKF_mpower(const real_T a[9], real_T b, real_T c[9])
{
real_T b_a[9];
real_T e;
boolean_T firstmult;
real_T ed2;
int16_T n;
real_T cBuffer[9];
real_T aBuffer[9];
boolean_T aBufferInUse;
boolean_T lsb;
int16_T nb;
int16_T nbitson;
int16_T b_n;
real_T tmp[9];
real_T tmp_0[9];
real_T aBufferInUse_0[9];
real_T c_0[9];
int16_T i;
real_T c_1[9];
int32_T exitg1;
if (std::floor(b) == b) {
if (32767.0 >= std::abs(b)) {
memcpy(&b_a[0], &a[0], 9U * sizeof(real_T));
e = std::abs(b);
n = (int16_T)std::abs(b);
b_n = (int16_T)e;
nbitson = 0;
nb = -1;
while (b_n > 0) {
nb++;
if ((b_n & 1) != 0) {
nbitson++;
}
b_n >>= 1;
}
if ((int16_T)e <= 2) {
if (b == 2.0) {
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
c[i + 3 * nbitson] = 0.0;
c[i + 3 * nbitson] += a[3 * nbitson] * a[i];
c[i + 3 * nbitson] += a[3 * nbitson + 1] * a[i + 3];
c[i + 3 * nbitson] += a[3 * nbitson + 2] * a[i + 6];
}
}
} else if (b == 1.0) {
memcpy(&c[0], &a[0], 9U * sizeof(real_T));
} else if (b == -1.0) {
EKF_inv(a, c);
} else if (b == -2.0) {
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
b_a[i + 3 * nbitson] = 0.0;
b_a[i + 3 * nbitson] += a[3 * nbitson] * a[i];
b_a[i + 3 * nbitson] += a[3 * nbitson + 1] * a[i + 3];
b_a[i + 3 * nbitson] += a[3 * nbitson + 2] * a[i + 6];
}
}
EKF_inv(b_a, c);
} else {
memset(&c[0], 0, 9U * sizeof(real_T));
c[0] = 1.0;
c[4] = 1.0;
c[8] = 1.0;
}
} else {
firstmult = true;
aBufferInUse = false;
lsb = ((nbitson & 1) != 0);
lsb = ((lsb && (b < 0.0)) || ((!lsb) && (b >= 0.0)));
for (b_n = 1; b_n <= nb; b_n++) {
if ((n & 1) != 0) {
if (firstmult) {
firstmult = false;
if (lsb) {
if (aBufferInUse) {
memcpy(&cBuffer[0], &aBuffer[0], 9U * sizeof(real_T));
} else {
memcpy(&cBuffer[0], &b_a[0], 9U * sizeof(real_T));
}
} else if (aBufferInUse) {
memcpy(&c[0], &aBuffer[0], 9U * sizeof(real_T));
} else {
memcpy(&c[0], &b_a[0], 9U * sizeof(real_T));
}
} else {
if (aBufferInUse) {
if (lsb) {
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
c[nbitson + 3 * i] = 0.0;
c[nbitson + 3 * i] += aBuffer[3 * i] * cBuffer[nbitson];
c[nbitson + 3 * i] += aBuffer[3 * i + 1] * cBuffer[nbitson
+ 3];
c[nbitson + 3 * i] += aBuffer[3 * i + 2] * cBuffer[nbitson
+ 6];
}
}
} else {
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
cBuffer[nbitson + 3 * i] = 0.0;
cBuffer[nbitson + 3 * i] += aBuffer[3 * i] * c[nbitson];
cBuffer[nbitson + 3 * i] += aBuffer[3 * i + 1] * c[nbitson
+ 3];
cBuffer[nbitson + 3 * i] += aBuffer[3 * i + 2] * c[nbitson
+ 6];
}
}
}
} else if (lsb) {
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
c[nbitson + 3 * i] = 0.0;
c[nbitson + 3 * i] += b_a[3 * i] * cBuffer[nbitson];
c[nbitson + 3 * i] += b_a[3 * i + 1] * cBuffer[nbitson + 3];
c[nbitson + 3 * i] += b_a[3 * i + 2] * cBuffer[nbitson + 6];
}
}
} else {
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
cBuffer[nbitson + 3 * i] = 0.0;
cBuffer[nbitson + 3 * i] += b_a[3 * i] * c[nbitson];
cBuffer[nbitson + 3 * i] += b_a[3 * i + 1] * c[nbitson + 3];
cBuffer[nbitson + 3 * i] += b_a[3 * i + 2] * c[nbitson + 6];
}
}
}
lsb = !lsb;
}
}
n >>= 1;
if (aBufferInUse) {
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
b_a[nbitson + 3 * i] = 0.0;
b_a[nbitson + 3 * i] += aBuffer[3 * i] * aBuffer[nbitson];
b_a[nbitson + 3 * i] += aBuffer[3 * i + 1] * aBuffer[nbitson + 3];
b_a[nbitson + 3 * i] += aBuffer[3 * i + 2] * aBuffer[nbitson + 6];
}
}
} else {
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
aBuffer[nbitson + 3 * i] = 0.0;
aBuffer[nbitson + 3 * i] += b_a[3 * i] * b_a[nbitson];
aBuffer[nbitson + 3 * i] += b_a[3 * i + 1] * b_a[nbitson + 3];
aBuffer[nbitson + 3 * i] += b_a[3 * i + 2] * b_a[nbitson + 6];
}
}
}
aBufferInUse = !aBufferInUse;
}
EKF_inv(aBuffer, tmp);
EKF_inv(b_a, tmp_0);
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
c_0[i + 3 * nbitson] = 0.0;
c_1[i + 3 * nbitson] = 0.0;
c_0[i + 3 * nbitson] += aBuffer[3 * nbitson] * c[i];
c_1[i + 3 * nbitson] += b_a[3 * nbitson] * c[i];
c_0[i + 3 * nbitson] += aBuffer[3 * nbitson + 1] * c[i + 3];
c_1[i + 3 * nbitson] += b_a[3 * nbitson + 1] * c[i + 3];
c_0[i + 3 * nbitson] += aBuffer[3 * nbitson + 2] * c[i + 6];
c_1[i + 3 * nbitson] += b_a[3 * nbitson + 2] * c[i + 6];
}
}
if (aBufferInUse) {
for (nbitson = 0; nbitson < 3; nbitson++) {
aBufferInUse_0[3 * nbitson] = c_0[3 * nbitson];
aBufferInUse_0[1 + 3 * nbitson] = c_0[3 * nbitson + 1];
aBufferInUse_0[2 + 3 * nbitson] = c_0[3 * nbitson + 2];
}
} else {
for (nbitson = 0; nbitson < 3; nbitson++) {
aBufferInUse_0[3 * nbitson] = c_1[3 * nbitson];
aBufferInUse_0[1 + 3 * nbitson] = c_1[3 * nbitson + 1];
aBufferInUse_0[2 + 3 * nbitson] = c_1[3 * nbitson + 2];
}
}
EKF_inv(aBufferInUse_0, c_0);
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
c_1[i + 3 * nbitson] = 0.0;
aBufferInUse_0[i + 3 * nbitson] = 0.0;
c_1[i + 3 * nbitson] += aBuffer[3 * nbitson] * cBuffer[i];
aBufferInUse_0[i + 3 * nbitson] += b_a[3 * nbitson] * cBuffer[i];
c_1[i + 3 * nbitson] += aBuffer[3 * nbitson + 1] * cBuffer[i + 3];
aBufferInUse_0[i + 3 * nbitson] += b_a[3 * nbitson + 1] * cBuffer[i
+ 3];
c_1[i + 3 * nbitson] += aBuffer[3 * nbitson + 2] * cBuffer[i + 6];
aBufferInUse_0[i + 3 * nbitson] += b_a[3 * nbitson + 2] * cBuffer[i
+ 6];
}
}
if (firstmult) {
if (b < 0.0) {
if (aBufferInUse) {
for (nbitson = 0; nbitson < 3; nbitson++) {
c[3 * nbitson] = tmp[3 * nbitson];
c[1 + 3 * nbitson] = tmp[3 * nbitson + 1];
c[2 + 3 * nbitson] = tmp[3 * nbitson + 2];
}
} else {
for (nbitson = 0; nbitson < 3; nbitson++) {
c[3 * nbitson] = tmp_0[3 * nbitson];
c[1 + 3 * nbitson] = tmp_0[3 * nbitson + 1];
c[2 + 3 * nbitson] = tmp_0[3 * nbitson + 2];
}
}
} else if (aBufferInUse) {
for (nbitson = 0; nbitson < 3; nbitson++) {
c[3 * nbitson] = aBuffer[3 * nbitson];
c[1 + 3 * nbitson] = aBuffer[3 * nbitson + 1];
c[2 + 3 * nbitson] = aBuffer[3 * nbitson + 2];
}
} else {
for (nbitson = 0; nbitson < 3; nbitson++) {
c[3 * nbitson] = b_a[3 * nbitson];
c[1 + 3 * nbitson] = b_a[3 * nbitson + 1];
c[2 + 3 * nbitson] = b_a[3 * nbitson + 2];
}
}
} else if (b < 0.0) {
for (nbitson = 0; nbitson < 3; nbitson++) {
c[3 * nbitson] = c_0[3 * nbitson];
c[1 + 3 * nbitson] = c_0[3 * nbitson + 1];
c[2 + 3 * nbitson] = c_0[3 * nbitson + 2];
}
} else if (aBufferInUse) {
for (nbitson = 0; nbitson < 3; nbitson++) {
c[3 * nbitson] = c_1[3 * nbitson];
c[1 + 3 * nbitson] = c_1[3 * nbitson + 1];
c[2 + 3 * nbitson] = c_1[3 * nbitson + 2];
}
} else {
for (nbitson = 0; nbitson < 3; nbitson++) {
c[3 * nbitson] = aBufferInUse_0[3 * nbitson];
c[1 + 3 * nbitson] = aBufferInUse_0[3 * nbitson + 1];
c[2 + 3 * nbitson] = aBufferInUse_0[3 * nbitson + 2];
}
}
}
} else {
memcpy(&b_a[0], &a[0], 9U * sizeof(real_T));
if ((!rtIsInf(b)) && (!rtIsNaN(b))) {
e = std::abs(b);
firstmult = true;
do {
exitg1 = 0L;
ed2 = std::floor(e / 2.0);
if (2.0 * ed2 != e) {
if (firstmult) {
memcpy(&c[0], &b_a[0], 9U * sizeof(real_T));
firstmult = false;
} else {
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
c_0[nbitson + 3 * i] = 0.0;
c_0[nbitson + 3 * i] += b_a[3 * i] * c[nbitson];
c_0[nbitson + 3 * i] += b_a[3 * i + 1] * c[nbitson + 3];
c_0[nbitson + 3 * i] += b_a[3 * i + 2] * c[nbitson + 6];
}
}
for (nbitson = 0; nbitson < 3; nbitson++) {
c[3 * nbitson] = c_0[3 * nbitson];
c[1 + 3 * nbitson] = c_0[3 * nbitson + 1];
c[2 + 3 * nbitson] = c_0[3 * nbitson + 2];
}
}
}
if (ed2 == 0.0) {
exitg1 = 1L;
} else {
e = ed2;
for (nbitson = 0; nbitson < 3; nbitson++) {
for (i = 0; i < 3; i++) {
cBuffer[nbitson + 3 * i] = 0.0;
cBuffer[nbitson + 3 * i] += b_a[3 * i] * b_a[nbitson];
cBuffer[nbitson + 3 * i] += b_a[3 * i + 1] * b_a[nbitson + 3];
cBuffer[nbitson + 3 * i] += b_a[3 * i + 2] * b_a[nbitson + 6];
}
}
for (nbitson = 0; nbitson < 3; nbitson++) {
b_a[3 * nbitson] = cBuffer[3 * nbitson];
b_a[1 + 3 * nbitson] = cBuffer[3 * nbitson + 1];
b_a[2 + 3 * nbitson] = cBuffer[3 * nbitson + 2];
}
}
} while (exitg1 == 0L);
if (b < 0.0) {
memcpy(&c_0[0], &c[0], 9U * sizeof(real_T));
EKF_inv(c_0, c);
}
} else {
for (nbitson = 0; nbitson < 9; nbitson++) {
c[nbitson] = (rtNaN);
}
}
}
} else {
for (nbitson = 0; nbitson < 9; nbitson++) {
c[nbitson] = (rtNaN);
}
}
}
//
// Function for MATLAB Function: '<Root>/MATLAB Function'
// function TerminalVoltage=PredictTerminalVoltage(Current,Power)
//
static real_T EKF_PredictTerminalVoltage(real_T Current, const real_T
Power_Ahat[9], const real_T Power_Bhat[3], real_T Power_Temp, const real_T
Power_Xhat_[3], real_T Power_Iterations, real_T Power_R0)
{
real_T TerminalVoltage;
real_T PolynomialA[9];
real_T PredictedStates[3];
real_T OCVatSOC;
int16_T i;
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,
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 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: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:322' for i=0:1:(Iterations-1)
for (i = 0; i < (int16_T)((Power_Iterations - 1.0) + 1.0); i++) {
// '<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_0[i_0];
}
}
// '<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_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: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;
}
real_T rt_powd_snf(real_T u0, real_T u1)
{
real_T y;
real_T tmp;
real_T tmp_0;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = (rtNaN);
} else {
tmp = std::abs(u0);
tmp_0 = std::abs(u1);
if (rtIsInf(u1)) {
if (tmp == 1.0) {
y = (rtNaN);
} else if (tmp > 1.0) {
if (u1 > 0.0) {
y = (rtInf);
} else {
y = 0.0;
}
} else if (u1 > 0.0) {
y = 0.0;
} else {
y = (rtInf);
}
} else if (tmp_0 == 0.0) {
y = 1.0;
} else if (tmp_0 == 1.0) {
if (u1 > 0.0) {
y = u0;
} else {
y = 1.0 / u0;
}
} else if (u1 == 2.0) {
y = u0 * u0;
} else if ((u1 == 0.5) && (u0 >= 0.0)) {
y = std::sqrt(u0);
} else if ((u0 < 0.0) && (u1 > std::floor(u1))) {
y = (rtNaN);
} else {
y = pow(u0, u1);
}
}
return y;
}
// Model step function
void EKF_step(void)
{
real_T C1;
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 dOCVatSOC_power;
real_T A;
real_T PolynomialA;
int16_T ix;
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,
1.0499999999999989, 1.0499999999999989, 0.92999999999999883,
0.97999999999999776, 1.0, 1.0, 1.0299999999999967, 1.1100000000000012,
1.1399999999999979, 1.1199999999999957, 1.0499999999999989,
1.0599999999999978, 1.0599999999999978, 1.0499999999999989,
0.79000000000000115, 0.73999999999999777, 0.71000000000000107,
0.72999999999999887, 0.57999999999999785, 0.56, 0.55000000000000115, 0.56,
0.47000000000000108, 0.4499999999999989, 0.44, 0.44, 0.38000000000000222,
0.37000000000000333, 0.36, 0.36, 0.32, 0.32, 0.32999999999999891,
0.34000000000000224, 0.28999999999999893, 0.38999999999999668,
0.48999999999999888, 0.50999999999999668, 0.38999999999999668,
0.52999999999999892, 0.59000000000000119, 0.609999999999999,
0.51000000000000112, 0.51000000000000112, 0.51000000000000112,
0.51000000000000112, 0.48, 0.52, 0.569999999999999, 0.56, 0.52,
0.6599999999999977, 0.6699999999999966, 0.65999999999999781,
0.63000000000000111, 0.670000000000001, 1.0999999999999979,
1.0499999999999989, 1.3099999999999967, 2.8099999999999987,
4.1499999999999968, 4.3099999999999969 };
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];
real_T Bhat_0[9];
boolean_T exitg1;
boolean_T exitg2;
// MATLAB Function: '<Root>/MATLAB Function' incorporates:
// Inport: '<Root>/Current'
// Inport: '<Root>/MacroTime'
// Inport: '<Root>/TemperatureIn'
// Inport: '<Root>/TimeStep'
// Inport: '<Root>/Voltage'
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// INITIALISE VARIABLES
// 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:21' if isempty(Count)
if (!EKF_DW.Count_not_empty) {
// '<S1>:1:23' Count=0;
EKF_DW.Count = 0.0;
EKF_DW.Count_not_empty = true;
// States
// '<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:32' R=0.01;
// SENSOR NOISE (VOLTAGE) THIS NEEDS TO BE FAIRLY LARGE TO ALLOW TO CONVERGE
// Capacity
// '<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:46' Rth=1e-6;
// SENSOR NOISE
// '<S1>:1:47' P0th=10*Qth;
EKF_DW.P0th = 10.0 * EKF_DW.Qth;
// Resistance
// '<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:51' Current_prev_R=0;
// '<S1>:1:52' alpha=0.999;
// Power
// '<S1>:1:56' Power.iMaxDisch=424;
EKF_DW.Power.iMaxDisch = 424.0;
// for max 10s otherwise mac is 265
// '<S1>:1:57' Power.iMaxCh=-100;
EKF_DW.Power.iMaxCh = -100.0;
// '<S1>:1:58' Power.MaxSOC=0.95;
EKF_DW.Power.MaxSOC = 0.95;
// '<S1>:1:59' Power.MinSOC=0.05;
EKF_DW.Power.MinSOC = 0.05;
// '<S1>:1:60' Power.Vmax=4.2;
EKF_DW.Power.Vmax = 4.2;
// '<S1>:1:61' Power.Vmin=2.7;
EKF_DW.Power.Vmin = 2.7;
// '<S1>:1:62' Power.Time=0;
EKF_DW.Power.Time = 0.0;
// '<S1>:1:63' Power.Ahat=zeros(3,3);
memset(&EKF_DW.Power.Ahat[0], 0, 9U * sizeof(real_T));
// '<S1>:1:64' Power.Bhat=[0 0 0]';
// '<S1>:1:65' Power.Temp=0;
EKF_DW.Power.Temp = 0.0;
// '<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;
EKF_DW.Power.Xhat_[1] = EKF_DW.Xhat[1];
EKF_DW.Power.Bhat[2] = 0.0;
EKF_DW.Power.Xhat_[2] = EKF_DW.Xhat[2];
// '<S1>:1:67' Power.TimeStep=0;
EKF_DW.Power.TimeStep = 0.0;
// '<S1>:1:68' Power.Iterations=0;
EKF_DW.Power.Iterations = 0.0;
// '<S1>:1:69' Power.R0=0;
EKF_DW.Power.R0 = 0.0;
// '<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:74' UpdatePower=1;
} else {
// '<S1>:1:75' else
// '<S1>:1:76' Count=Count+1;
EKF_DW.Count++;
}
//
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// INTERPOLATE PARAMETRES
// '<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: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: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;
Ahat[0] = 1.0;
Ahat[2] = 0.0;
Ahat[3] = 0.0;
Ahat[5] = 0.0;
Ahat[6] = 0.0;
Ahat[8] = 1.0;
// '<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;
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// PREDICTION STAGE
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// SOC ESTIMATION FOR CAPACITY ESTIMATION
// '<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:117' AbsSumCC=abs(Current_prev*TimeStep)+AbsSumCC;
EKF_DW.AbsSumCC += std::abs(EKF_DW.Current_prev * EKF_U.TimeStep);
// '<S1>:1:118' TimeToUpdateQ=TimeToUpdateQ+TimeStep;
EKF_DW.TimeToUpdateQ += EKF_U.TimeStep;
} else {
// '<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: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:130' if TimeToUpdateQ>60*30
if (EKF_DW.TimeToUpdateQ > 1800.0) {
// '<S1>:1:132' SumCC=0;
EKF_DW.SumCC = 0.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: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;
// '<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:144' UpdateNow=1;
UpdateNow = 1;
// '<S1>:1:145' XhatTh_Update=Xhat(1);
EKF_DW.XhatTh_Update = EKF_DW.Xhat[0];
}
}
// '<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:199' Ytrue=Voltage;
// ESTIMATION STAGE
// INTERPOLATE CORRECT VALUES
// '<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: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];
}
KGainTh = 1.0 / (Qhat_old + EKF_DW.R);
// '<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: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 = 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:223' if abs(Current-Current_prev_R)>5
if (std::abs(EKF_U.Current - EKF_DW.Current_prev_R) > 5.0) {
// '<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:226' R0_filt_prev=R0_filt;
EKF_DW.R0_filt_prev = Cth;
} else {
// '<S1>:1:227' else
// '<S1>:1:228' R0_filt=R0_filt_prev;
Cth = EKF_DW.R0_filt_prev;
}
// '<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:233' ~
// POWER ESTIMATION
// '<S1>:1:237' if MacroTime>UpdatePower*iP
if (EKF_U.MacroTime > EKF_DW.UpdatePower * EKF_DW.iP) {
// '<S1>:1:239' Current_prev_R=Current;
EKF_DW.Current_prev_R = EKF_U.Current;
// '<S1>:1:241' Power.Ahat=Ahat;
memcpy(&EKF_DW.Power.Ahat[0], &Ahat[0], 9U * sizeof(real_T));
// '<S1>:1:242' Power.Bhat=Bhat;
// '<S1>:1:243' Power.Temp=TemperatureIn;
EKF_DW.Power.Temp = EKF_U.TemperatureIn;
// '<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];
EKF_DW.Power.Xhat_[1] = EKF_DW.Xhat[1];
EKF_DW.Power.Bhat[2] = 1.0;
EKF_DW.Power.Xhat_[2] = EKF_DW.Xhat[2];
// '<S1>:1:245' Power.TimeStep=TimeStep;
EKF_DW.Power.TimeStep = EKF_U.TimeStep;
// '<S1>:1:246' Power.Iterations=50;
EKF_DW.Power.Iterations = 50.0;
// 50 iterations at 0.2s time step is 10seconds
// '<S1>:1:247' Power.R0=R0_filt;
EKF_DW.Power.R0 = Cth;
// '<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: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: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:261' PolynomialA=0;
PolynomialA = 0.0;
// '<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: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] = ((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;
UpdateNow = 1;
A = EKF_DW.Power.iMaxDisch;
if (rtIsNaN(EKF_DW.Power.iMaxDisch)) {
ix = 2;
exitg2 = false;
while ((!exitg2) && (ix < 4)) {
UpdateNow = ix;
if (!rtIsNaN(Bhat[ix - 1])) {
A = Bhat[ix - 1];
exitg2 = true;
} else {
ix++;
}
}
}
if (UpdateNow < 3) {
while (UpdateNow + 1 < 4) {
if (Bhat[UpdateNow] < A) {
A = Bhat[UpdateNow];
}
UpdateNow++;
}
}
// '<S1>:1:277' IMaxCh=max([Power.iMaxCh;iChOCV;iChSOC]);
Bhat[0] = EKF_DW.Power.iMaxCh;
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;
UpdateNow = 1;
C1 = EKF_DW.Power.iMaxCh;
if (rtIsNaN(EKF_DW.Power.iMaxCh)) {
ix = 2;
exitg1 = false;
while ((!exitg1) && (ix < 4)) {
UpdateNow = ix;
if (!rtIsNaN(Bhat[ix - 1])) {
C1 = Bhat[ix - 1];
exitg1 = true;
} else {
ix++;
}
}
}
if (UpdateNow < 3) {
while (UpdateNow + 1 < 4) {
if (Bhat[UpdateNow] > C1) {
C1 = Bhat[UpdateNow];
}
UpdateNow++;
}
}
// '<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: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:284' iP=iP+1;
EKF_DW.iP++;
}
// '<S1>:1:288' if isnan(Xhat(1))
// MAKE SURE SOC IS WITHIN LIMITS
// '<S1>:1:292' if Xhat(1)>1
if (EKF_DW.Xhat[0] > 1.0) {
// '<S1>:1:293' Xhat(1)=1;
EKF_DW.Xhat[0] = 1.0;
}
// '<S1>:1:296' if Xhat(1)<0
if (EKF_DW.Xhat[0] < 0.0) {
// '<S1>:1:297' Xhat(1)=0;
EKF_DW.Xhat[0] = 0.0;
}
// Outport: '<Root>/SOC' incorporates:
// MATLAB Function: '<Root>/MATLAB Function'
// OUTPUT THE VALUES
// '<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:
// MATLAB Function: '<Root>/MATLAB Function'
EKF_Y.Q0 = EKF_DW.Qhat / 3600.0;
// Outport: '<Root>/R0' incorporates:
// MATLAB Function: '<Root>/MATLAB Function'
EKF_Y.R0 = Cth;
// Outport: '<Root>/PowerDisch' incorporates:
// MATLAB Function: '<Root>/MATLAB Function'
EKF_Y.PowerDisch = EKF_DW.PowerDischHat;
// Outport: '<Root>/PowerCh' incorporates:
// MATLAB Function: '<Root>/MATLAB Function'
EKF_Y.PowerCh = EKF_DW.PowerChHat;
// Outport: '<Root>/DegradationIndex' incorporates:
// MATLAB Function: '<Root>/MATLAB Function'
EKF_Y.DegradationIndex = KGainTh;
}
// Model initialize function
void EKF_initialize(void)
{
// Registration code
// initialize non-finites
rt_InitInfAndNaN(sizeof(real_T));
// initialize error status
rtmSetErrorStatus(EKF_M, (NULL));
// states (dwork)
(void) memset((void *)&EKF_DW, 0,
sizeof(DW_EKF_T));
// external inputs
(void)memset((void *)&EKF_U, 0, sizeof(ExtU_EKF_T));
// external outputs
(void) memset((void *)&EKF_Y, 0,
sizeof(ExtY_EKF_T));
{
static const real_T tmp[9] = { 0.1, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.1
};
// SystemInitialize for MATLAB Function: '<Root>/MATLAB Function'
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;
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;
}
}
// Model terminate function
void EKF_terminate(void)
{
// (no terminate code required)
}
//
// File trailer for generated code.
//
// [EOF]
//