-
Notifications
You must be signed in to change notification settings - Fork 0
/
SOCbySPKF-CapstoneProjectPart2.m
267 lines (223 loc) · 9.39 KB
/
SOCbySPKF-CapstoneProjectPart2.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
% First, make sure that the ESC toolbox functions are in the path
addpath readonly
function spkfData = initSPKF(v0,T0,SigmaX0,SigmaV,SigmaW,model)
% Initial state description
ir0 = 0; spkfData.irInd = 1;
hk0 = 0; spkfData.hkInd = 2;
SOC0 = SOCfromOCVtemp(v0,T0,model); spkfData.zkInd = 3;
spkfData.xhat = [ir0 hk0 SOC0]'; % initial state
% Covariance values
spkfData.SigmaX = SigmaX0;
spkfData.SigmaV = SigmaV;
spkfData.SigmaW = SigmaW;
spkfData.Snoise = real(chol(diag([SigmaW; SigmaV]),'lower'));
spkfData.Qbump = 5;
% SPKF specific parameters
Nx = length(spkfData.xhat); spkfData.Nx = Nx; % state-vector length
Ny = 1; spkfData.Ny = Ny; % measurement-vector length
Nu = 1; spkfData.Nu = Nu; % input-vector length
Nw = size(SigmaW,1); spkfData.Nw = Nw; % process-noise-vector length
Nv = size(SigmaV,1); spkfData.Nv = Nv; % sensor-noise-vector length
Na = Nx+Nw+Nv; spkfData.Na = Na; % augmented-state-vector length
h = sqrt(3); h = 3;
spkfData.h = h; % SPKF/CDKF tuning factor
Weight1 = (h*h-Na)/(h*h); % weighting factors when computing mean
Weight2 = 1/(2*h*h); % and covariance
spkfData.Wm = [Weight1; Weight2*ones(2*Na,1)]; % mean
spkfData.Wc = spkfData.Wm; % covar
% previous value of current
spkfData.priorI = 0;
spkfData.signIk = 0;
% store model data structure too
spkfData.model = model;
end
function [zk,zkbnd,spkfData] = iterSPKF(vk,ik,Tk,deltat,spkfData)
model = spkfData.model;
% Load the cell model parameters
Q = getParamESC('QParam',Tk,model);
G = getParamESC('GParam',Tk,model);
M = getParamESC('MParam',Tk,model);
M0 = getParamESC('M0Param',Tk,model);
RC = exp(-deltat./abs(getParamESC('RCParam',Tk,model)))';
R = getParamESC('RParam',Tk,model)';
R0 = getParamESC('R0Param',Tk,model);
eta = getParamESC('etaParam',Tk,model);
if ik<0, ik=ik*eta; end;
% Get data stored in spkfData structure
I = spkfData.priorI;
SigmaX = spkfData.SigmaX;
xhat = spkfData.xhat;
Nx = spkfData.Nx;
Nw = spkfData.Nw;
Nv = spkfData.Nv;
Na = spkfData.Na;
Snoise = spkfData.Snoise;
Wc = spkfData.Wc;
irInd = spkfData.irInd;
hkInd = spkfData.hkInd;
zkInd = spkfData.zkInd;
if abs(ik)>Q/100, spkfData.signIk = sign(ik); end;
signIk = spkfData.signIk;
% Step 1a: State estimate time update
% - Create xhatminus augmented SigmaX points
% - Extract xhatminus state SigmaX points
% - Compute weighted average xhatminus(k)
% Step 1a-1: Create augmented SigmaX and xhat
[sigmaXa,p] = chol(SigmaX,'lower');
if p>0,
fprintf('Cholesky error. Recovering...\n');
theAbsDiag = abs(diag(SigmaX));
sigmaXa = diag(max(SQRT(theAbsDiag),SQRT(spkfData.SigmaW)));
end
sigmaXa=[real(sigmaXa) zeros([Nx Nw+Nv]); zeros([Nw+Nv Nx]) Snoise];
xhata = [xhat; zeros([Nw+Nv 1])];
% NOTE: sigmaXa is lower-triangular
% Step 1a-2: Calculate SigmaX points (strange indexing of xhata to
% avoid "repmat" call, which is very inefficient in MATLAB)
Xa = xhata(:,ones([1 2*Na+1])) + ...
spkfData.h*[zeros([Na 1]), sigmaXa, -sigmaXa];
% Step 1a-3: Time update from last iteration until now
% stateEqn(xold,current,xnoise)
Xx = stateEqn(Xa(1:Nx,:),I,Xa(Nx+1:Nx+Nw,:));
xhat = Xx*spkfData.Wm;
% Step 1b: Error covariance time update
% - Compute weighted covariance sigmaminus(k)
% (strange indexing of xhat to avoid "repmat" call)
Xs = Xx - xhat(:,ones([1 2*Na+1]));
SigmaX = Xs*diag(Wc)*Xs';
% Step 1c: Output estimate
% - Compute weighted output estimate yhat(k)
I = ik; yk = vk;
Y = outputEqn(Xx,I,Xa(Nx+Nw+1:end,:),Tk,model);
yhat = Y*spkfData.Wm;
% Step 2a: Estimator gain matrix
Ys = Y - yhat(:,ones([1 2*Na+1]));
SigmaXY = Xs*diag(Wc)*Ys';
SigmaY = Ys*diag(Wc)*Ys';
L = SigmaXY/SigmaY;
% Step 2b: State estimate measurement update
r = yk - yhat; % residual. Use to check for sensor errors...
if r^2 > 100*SigmaY, L(:,1)=0.0; end
xhat = xhat + L*r;
xhat(zkInd)=min(1.05,max(-0.05,xhat(zkInd)));
xhat(hkInd) = min(1,max(-1,xhat(hkInd)));
% Step 2c: Error covariance measurement update
SigmaX = SigmaX - L*SigmaY*L';
[~,S,V] = svd(SigmaX);
HH = V*S*V';
SigmaX = (SigmaX + SigmaX' + HH + HH')/4; % Help maintain robustness
% Q-bump code
if r^2>4*SigmaY, % bad voltage estimate by 2-SigmaX, bump Q
fprintf('Bumping sigmax\n');
SigmaX(zkInd,zkInd) = SigmaX(zkInd,zkInd)*spkfData.Qbump;
end
% Save data in spkfData structure for next time...
spkfData.priorI = ik;
spkfData.SigmaX = SigmaX;
spkfData.xhat = xhat;
zk = xhat(zkInd);
zkbnd = 3*sqrt(SigmaX(zkInd,zkInd));
% Calculate new states for all of the old state vectors in xold.
function xnew = stateEqn(xold,current,xnoise)
current = current + xnoise; % noise adds to current
xnew = 0*xold;
xnew(irInd,:) = RC*xold(irInd,:) + (1-diag(RC))*current;
Ah = exp(-abs(current*G*deltat/(3600*Q))); % hysteresis factor
xnew(hkInd,:) = Ah.*xold(hkInd,:) + (Ah-1).*sign(current);
xnew(zkInd,:) = xold(zkInd,:) - current/3600/Q;
xnew(hkInd,:) = min(1,max(-1,xnew(hkInd,:)));
xnew(zkInd,:) = min(1.05,max(-0.05,xnew(zkInd,:)));
endfunction
% Calculate cell output voltage for all of state vectors in xhat
function yhat = outputEqn(xhat,current,ynoise,T,model)
yhat = OCVfromSOCtemp(xhat(zkInd,:),T,model);
yhat = yhat + M*xhat(hkInd,:) + M0*signIk;
yhat = yhat - R*xhat(irInd,:) - R0*current + ynoise(1,:);
end
% "Safe" square root
function X = SQRT(x)
X = sqrt(max(0,x));
end
end
% GRADED FUNCTION (do not modify this line)
% function [SigmaW, SigmaV, SigmaZ0] = tuneEKF
%
% SigmaW - covariance value for current-sensor process noise
% SigmaV - covariance value for voltage-sensor measurement noise
% SigmaZ0 - covariance value for error in initial SOC estimate
function [SigmaW, SigmaV, SigmaZ0] = tuneSPKF
% BEGIN MODIFYING CODE AFTER THIS
SigmaW = 0.001; % This is a sample value. You will need to change it.
SigmaV = 0.1; % This is a sample value. You will need to change it.
SigmaZ0 = 0.1; % This is a sample value. You will need to change it.
end
% Load model file corresponding to a cell of this type
% Has the variables: current, SOC, time, voltage
load readonly/PAN_CAPSTONE_DATA.mat; % load data from Panasonic NMC cell, +25 degC
T = 25; % Test temperature
time = DYNData.script1.time(:); deltat = time(2)-time(1);
time = time-time(1); % start time at 0
current = DYNData.script1.current(:); % discharge > 0; charge < 0.
voltage = DYNData.script1.voltage(:);
soc = DYNData.script1.soc(:);
% Load cell-test data to be used for this batch experiment
% Contains variable "DYNData" of which the field "script1" is of
% interest. This has sub-fields time, current, voltage, soc.
load readonly/PANmodel.mat; % load ESC model of Panasonic NMC cell
% Reserve storage for computed results, for plotting
sochat = zeros(size(soc));
socbound = zeros(size(soc));
% Get tuning values from user-modified function
[SigmaW, SigmaV, SigmaZ0] = tuneSPKF;
SigmaX0 = diag([1e-6 1e-6 SigmaZ0]);
spkfData = initSPKF(voltage(1),T,SigmaX0,SigmaV,SigmaW,model);
% This simulation tests the SPKF when there is an inital SOC-estimation error
% The true initial SOC is 95%, but we will initialize the SOC estimate in the
% filter to 90% and see how quickly and well the filter converges toward the
% correct SOC.
spkfData.xhat(spkfData.zkInd)=0.90; %
% Now, enter loop for remainder of time, where we update the SPKF
% once per sample interval
fprintf('Please be patient. This code will take a minute or so to execute.\n')
for k = 1:length(voltage),
vk = voltage(k); % "measure" voltage
ik = current(k); % "measure" current
Tk = T; % "measure" temperature
% Update SOC (and other model states)
[sochat(k),socbound(k),spkfData] = iterSPKF(vk,ik,Tk,deltat,spkfData);
% update waitbar periodically, but not too often (slow procedure)
if mod(k,300)==0,
fprintf(' Completed %d out of %d iterations...\n',k,length(voltage));
end
end
%%
subplot(1,2,1); plot(time/60,100*sochat,time/60,100*soc); hold on
plot([time/60; NaN; time/60],[100*(sochat+socbound); NaN; 100*(sochat-socbound)]);
title('SOC estimation using SPKF'); grid on
xlabel('Time (min)'); ylabel('SOC (%)'); legend('Estimate','Truth','Bounds');
%%
J1 = sqrt(mean((100*(soc-sochat)).^2));
fprintf('RMS SOC estimation error = %g%%\n',J1);
%%
J2 = 100*socbound(end);
fprintf('Final value of SOC estimation error bounds = %g%%\n',J2);
%%
subplot(1,2,2); plot(time/60,100*(soc-sochat)); hold on
plot([time/60; NaN; time/60],[100*socbound; NaN; -100*socbound],'--');
title('SOC estimation errors using EKF');
xlabel('Time (min)'); ylabel('SOC error (%)'); ylim([-4 4]);
legend('Estimation error','Bounds');
grid on
ind = find(abs(soc-sochat)>socbound);
fprintf('Percent of time error outside bounds = %g%%\n',length(ind)/length(soc)*100);
% Compute the prospective grade
tableRow = min(11,ceil(max(0,J1-0.1)/0.01 + 1));
tableCol = min(11,ceil(max(0,J2-0.2)/0.02 + 1));
table = hankel([10:-1:0]);
grade = table(tableRow,tableCol);
if ~isempty(ind),
fprintf('Your SOC estimation error was sometimes outside of bounds, so your overall grade is 0/10.');
else
fprintf('Your grade is calculated from row %d and column %d of the grading table that is\n',tableRow,tableCol);
fprintf('listed in the project description page. This will result in a grade of %d/10.\n',grade);
end