%-------------------------------------------------------------------------%
%%%%%%%%%%%%%% Model Predictive Control Implementation %%%%%%%%%%%%%
%-------------------------------------------------------------------------%
% State variable here is SOC... based on voltage: SOC = CV/CVmax = V/Vmax
% MPC method proceeds forward through the cycle, and calculates a "DP loop"
% at each time step - trying to minimize the cost over this short time,
% then executes the first of these controls and proceeds to the next step.
% Known power demand, velocities, acceleration, and vehicle parameters
clear k h j l
q = waitbar(0,'MPC Simulation Progress');
% Initialize variables:
n = 101; % Discretization steps for engine torque and SOC (101)
nc = 1; % Control horizon (unused)
np = 3; % Prediction horizon (used) = 1780 for DP comparison
% can be up to 55 w/o affecting zero-dmd tail of cycle
MPC.SOC = linspace(ess.soc_min,ess.soc_max,n); % possible pack SOCs
% Also constrains SOC to given limits from init file
MPC.largecost = 100; % large cost value
MPC.eng_on_off_cost = 15e-4; % Engine on/off cost value
load data_DP_SOC_costs SOC_cost_one
% Decay function for SOC cost that decreases as horizon length increases
SOC_curve = [0 0 8e-3 5.83e-3 4.5e-3 3.5e-3 3.2e-3 2.8e-3 2.4e-3...
2.1e-3 1.9e-3 1.8e-3 1.7e-3 1.6e-3 1.5e-3 1.4e-3...
1.013e-3 1.005e-3 9.97e-4 9.91e-4 9.857e-4 9.812e-4 9.774e-4 ...
9.74e-4 9.71e-4 9.68e-4 9.663e-4 9.64e-4 9.62e-4 0];
MPC.SOC_cost = SOC_cost_one*SOC_curve(np); % Low SOC cost
% Import decay prediction parameters:
load data_decay_rates_cshvr lambda_vals torque_vals
lambda = zeros(1,ttot);
kmax = ttot; % steps for timer loop - whole cycle
hmax = n; jmax = hmax; % other loop steps (k (h (j ) ) )
% Initialize loop variables for speed:
MPC.costmin=MPC.largecost*ones(hmax,np+1); MPC.costmin(:,np+1)=0;
MPC.Tengmin=zeros(hmax,np+1); MPC.SOCdotmin=MPC.Tengmin;
MPC.SOCnextmin=ones(hmax,np+1); MPC.Tmotmin=MPC.Tengmin;
MPC.Tbrakemin=MPC.Tengmin; MPC.mdotmin=MPC.Tengmin; MPC.EngineOn=MPC.Tengmin;
% Initialize optimal costs, control vectors
Tengopt=zeros(1,kmax);SOCdotopt=Tengopt;SOCnextopt=Tengopt;SOCopt=Tengopt;
Tmotopt=Tengopt;Tbrakeopt=Tengopt;mdotopt=Tengopt;EngOnopt=Tengopt;Costopt=Tengopt;
% Initialize the engine "off" control signal
EngOff_Sig=Tengopt; EngOff_Sig(1:3)=1; % Engine starts "off"
% Assumptions:
ess.pack_cap = 3000/ess.num_cell_series; % Assumed constant pack capacitance
% Constraints:
% Engine torque, constrained at each time step, where
% Possible values for Teng calculated within loop below at each time
cstr.Teng_max = min(interp1q(eng.spd_max_hot_index',eng.trq_max_hot_map',weng),...
max(eng.trq_fuel_hot_index));
% min fcn prevents NAN's for being outside of mdot table range
% Engine torque losses
cstr.Teng_loss = eng.pwr_loss./weng + interp1q(eng.spd_zero_fuel_hot_index',...
-eng.trq_zero_fuel_hot_index',weng);
% Max/min motor torque - symmetric (also motor power constraint)
cstr.Tmot_max_min = interp1q(mc.spd_max_index',mc.trq_max_map',wmot);
% Max regen torque calculated in loop...
% Motor / ESS current:
cstr.mot_curr = min(mc.curr_max,ess.curr_chg_max);
% Constraint on UC power capability, indexed for each SOC
cstr.Puc_max = (MPC.SOC*ess.pack_vmax).^2/(4*ess.pack_res);
cstr.Puc_max(1) = 0; % No positive current at low SOC
cstr.Puc_min = ess.pack_vmax_surge*(MPC.SOC*ess.pack_vmax - ess.pack_vmax_surge)/ess.pack_res;
cstr.Puc_min(n) = 0; % No negative current at high SOC
% UC SOC constraints
cstr.SOC_initial = 0.8;
cstr.SOC_min = ess.soc_min;
cstr.SOC_max = ess.soc_max;
% Calculations inside MPC loop start with Input
% Engine Torque/Speed (gives fuel rate) -> Road Torque demand ->
% Motor torque -> ESS power -> UC SOC change -> Next SOC
for k=1:kmax-np % advance forward through the cycle
waitbar(k/kmax)
% Choose a value for time constant(=1/lambda) based on the value of Torque
if T_dmd(k) > torque_vals(1)
lambda(k)= lambda_vals(1);
else
for kp = 1:length(lambda_vals)-1
if T_dmd(k)< torque_vals(kp) && T_dmd(k) > torque_vals(kp+1)
lambda(k) = lambda_vals(kp+1);
break
end
end
end
% Generate predicted future torques (first term is actual torque)
MPC.Tpred = T_dmd_eng(k)*exp(((1:np)-1)*lambda(k));
for m = np:-1:1 % for each time do receding horizon DP
% Possible Torques for engine at each given speed (constrains Teng)
MPC.Teng = [0 linspace(0,cstr.Teng_max(k+m-1)-cstr.Teng_loss(k+m-1),n-1)+...
cstr.Teng_loss(k+m-1)];
% Fuel Rates for each engine torque - same for all SOC's
MPC.mdot = interp2(eng.trq_fuel_hot_index,eng.spd_fuel_hot_index,...
eng.fuel_hot_map,MPC.Teng,weng(k+m-1));
MPC.mdot(1) = 0; % engine off - mdot = 0
% Max Regen torque (indexed over SOC)
cstr.Tmot_max_regen = max(-(mc.curr_max^2*ess.pack_res+...
mc.curr_max*MPC.SOC*ess.pack_vmax),-cstr.Tmot_max_min(k+m-1));
for h=hmax:-1:1 % Indexes possible states(SOC) at (k-1)th timestep
% Initialize Cost = 0 - modified if constraint violated, then mdot and
% other costs added at the end
MPC.cost = zeros(1,n);
% Calculate Motor Torque - max/min prevents extra torque from being demanded
MPC.Tmot = (MPC.Tpred(m) - (MPC.Teng -(MPC.Teng>0).*cstr.Teng_loss(k+m-1)).*cpl.eff)./tc.ratio;
% Max regen condition
if MPC.Tpred(m)/tc.ratio < -cstr.Tmot_max_min(k+m-1)
MPC.Tmot(1) = max(cstr.Tmot_max_regen(h),cstr.Puc_min(h)/wmot(k+m-1));
% max regen torque value possible for given SOC... approximate
MPC.cost(1) = 0;
end
% Constraints on Motor Torque/Power:
if h==hmax
MPC.cost(MPC.Tmot < 0) = MPC.largecost;
MPC.Tmot(MPC.Tmot < 0) = 0;
elseif h==1
MPC.cost(MPC.Tmot > 0) = MPC.largecost;
MPC.Tmot(MPC.Tmot > 0) = 0;
end
MPC.cost(MPC.Tmot > cstr.Tmot_max_min(k+m-1)) = MPC.largecost;
MPC.Tmot(MPC.Tmot > cstr.Tmot_max_min(k+m-1)) = cstr.Tmot_max_min(k+m-1);
MPC.cost(MPC.Tmot < -cstr.Tmot_max_min(k+m-1)) = MPC.largecost;
MPC.Tmot(MPC.Tmot < -cstr.Tmot_max_min(k+m-1)) = -cstr.Tmot_max_min(k+m-1);
% ESS Electrical Power lookup - from motor data
% (can add a static or time-varying load to this for electrical power loss)
MPC.Puc = interp2(mc.trq_eff_index,mc.spd_eff_index,mc.pwr_elec_map,MPC.Tmot,wmot(k+m-1));
% Ultracapacitor power constraints
% Max power UC discharge:
MPC.cost(MPC.Puc > cstr.Puc_max(h)) = MPC.largecost;
MPC.Puc(MPC.Puc > cstr.Puc_max(h)) = cstr.Puc_max(h);
MPC.Tmot(MPC.Puc > cstr.Puc_max(h)) = 0;
% Max power UC charging:
MPC.cost(MPC.Puc < cstr.Puc_min(h)) = MPC.largecost;
MPC.Puc(MPC.Puc < cstr.Puc_min(h)) = cstr.Puc_min(h);
MPC.Tmot(MPC.Puc < cstr.Puc_min(h)) = 0;
% ESS current out is negative, so into UC is positive
MPC.Iuc = (-MPC.SOC(h)*ess.pack_vmax + sqrt((MPC.SOC(h)*ess.pack_vmax)^2 ...
- 4*ess.pack_res*MPC.Puc)) / (2*ess.pack_res);
% Motor current draw constraint
MPC.cost(abs(MPC.Iuc) >= cstr.mot_curr) = MPC.largecost;
MPC.Iuc(MPC.Iuc > cstr.mot_curr) = cstr.mot_curr;
MPC.Iuc(MPC.Iuc < -cstr.mot_curr) = -cstr.mot_curr;
% Charging efficiency consideration - from Maxwell model
MPC.Iuc(MPC.Iuc > 0) = MPC.Iuc(MPC.Iuc > 0)*ess.chg_eff;
% SOC change
MPC.SOCdot = MPC.Iuc./(ess.pack_cap*ess.pack_vmax);
% Next SOC - to help check constraints
MPC.SOCnext = MPC.SOC(h) + MPC.SOCdot*dt;
% Ultracapacitor SOC constraints
MPC.cost(MPC.SOCnext > cstr.SOC_max) = MPC.largecost;
MPC.SOCdot(MPC.SOCnext > cstr.SOC_max) = (ess.soc_max - MPC.SOC(h))/dt;
MPC.Iuc(MPC.SOCnext > cstr.SOC_max) = MPC.SOCdot(MPC.SOCnext > cstr.SOC_max).*...
(ess.pack_cap.*ess.pack_vmax);
MPC.Puc(MPC.SOCnext > cstr.SOC_max) = (MPC.Iuc(MPC.SOCnext > cstr.SOC_max).^2).*...
ess.pack_res+ MPC.Iuc(MPC.SOCnext > cstr.SOC_max).*MPC.SOC(h).*ess.pack_vmax ;
MPC.Tmot(MPC.SOCnext > cstr.SOC_max) = MPC.Puc(MPC.SOCnext > cstr.SOC_max)./wmot(k+m-1);
MPC.SOCnext(MPC.SOCnext > cstr.SOC_max) = ess.soc_max;
MPC.cost(MPC.SOCnext < cstr.SOC_min) = MPC.largecost;
MPC.SOCdot(MPC.SOCnext < cstr.SOC_min) = (ess.soc_min - MPC.SOC(h))/dt;
MPC.Iuc(MPC.SOCnext < cstr.SOC_min) = MPC.SOCdot(MPC.SOCnext
