%% "Finding Ultimate Limits of Performance for Hybrid Electric Vehicles"[1]
%  An exercise in recreating the results from Tate and Boyd's Paper
% 
%% 
% By Sindhura Buggaveeti and Allison Waters
%
%% Problem Description
%
%    In this project, the objective was to reproduce the results from Tate 
% and Boyd's paper, "Finding ultimatelimits of performance for hybrid  
% electric vehicles" [1]. The article proses problem of finding optimal  
% engine operation in a pure series hybrid vehicle over a fixed drive cycle 
% subject to a number of practical contraints such as nonlinear fuel/power  
% maps, minimum and maximum battery charge, battery efficiency, nonlinear 
% vehicle dynamics and losses, drive train efficiency, engine slew rate  
% limits.
%
% * Nonlinear fuel/power maps
% * Min and max battery charge 
% * Battery efficiency
% * Nonlinear vehicle dynamics and losses
% * Drive train efficiency
% * Engine slew rate limits
%
%  The problem of optimizing fuel efficiency is initially formulated as a
%  nonlinear convex optimization problem. This convex problem is then
%  accurately approximated as a large linear program. This optimal solution 
%  is the lower limit of fuel consumption that any control law can achieve
%  for the given drive cycle and vehicle.
%% Proposed Solution
%%
% 
% <<flowdiagram.PNG>>
% 
%% 
% Objective function for optimal fuel economy can be described as:
%
% $$  min \sum_{j=k_0}^{k_f} {f(k)} $$
%
% Following are the equality and inequality constraints for the proposed
% optimization problem.
% 
% $$f(k)=f_e(P_e(k))$$ 
%
% $$E(k+1)-E(k)=f_b(P_s(k))$$
%
% $$E(k_0)=E(k_f)$$
%
% $$P_e(k)=P_s(k)+P_m(k)+P_b(k)$$
% 
% $$P_e(k+1)-P_e(k)\leq Max Engine Slew Up$$
% 
% $$P_e(k+1)-P_e(k)\geq Max Engine Slew Down$$
% 
% $$P_e(k)\geq 0$$
%
% $$P_e(k)\leq Max Engine Power$$
%
% $$P_b(k)\geq 0$$
% 
% $$P_b(k)\leq Max Braking Power$$
%
% $$P_s(k)\geq Max Discharge Rate$$
%
% $$P_s(k)\leq Max Charge Rate$$
%
% $$E(k)\geq Min Battery Energy$$
%
% $$E(k)\leq Max Battery Energy$$
%
% The parameters and variable described in the above algorithm are:
% 
%  * Variables: $$f(k), E(k), P_e(k), P_b(k), P_s(k)$$
%  * Parameters: Max Engine Slew Up, Max Engine Slew Down, Max Engine 
%  Power, Max Braking Power, Max Discharge and charge rate, Min and Max 
%  BatteryEnergy
%
%     The optimization problem above was converted into a convex linear
%  program and solved using Grant and Boyd's convex optimization software
%  for Matlab, CVX [2][3].
%% Data Sources
%
%    To illustrate the results obtained using this method, a numerical 
% example was performed. The drive cycle is first 1371 seconds of FTP-75 
% Urban Dynamometer Driving Schedule'(UDDS) [4]. The vehicle mass is 1072kg 
% with a coefficient of drag(CD) of 0.3 and frontal area of 1.96m^2. The
% coefficient of rolling resistance is 0.015. Air density is 1.22kg/m^3.
% The vehicle is modeled with a 2kW battery load. The fuel consumption
% curve for engine is approximated by
%
% $$f(t)\geq P_e(t)*0.059+0.075$$
%
% $$f(t)\geq P_e(t)*0.096-1.041$$
%
% where f(t) is in unit of grams per second and P_e is in units of W.
%
%  The battery is lead acid(PbA) with 0.60 kW-hr maximum energy capcity and 
%  a reserve of 0.12 KW-hrs. The maximum charge and discharge rates are 
%  9.54kW. The battery has a charge efficiency of 80% and discharge 
%  efficiency of 100% as given in the paper.
%
%% Solution for Hybrid Vehicle
% *Input Parameters*
%% 
%  The mechanical power generated by the vehicle is calculated by the 
%  equation below using the following inputs given in the paper. 
%%
% $P_m=(1/2)*p*C_d*A*v^2+m*g*(C_r+a/g)*v/1000+2$
%%
Vel=xlsread('FTP.xlsx');    %FTP drive cycle data [4]
m=1072;     %Mass of vehicle
Cd=0.3;     %Coefficient of drag
A=1.96;     %Frontal Area
Cr=0.015;   %Rolling resistance coefficient
p=1.22;     %Density of air
g=9.81;     %Gravity
k=1371;     %Number of discrete time steps (each time step is 1 second)
V=0;        %Initial velocity
g1=0.8;     %Battery charge efficiency
g2=1;       %Battery discharge efficiency
ve=0.44704*Vel;  %Converting FTP velocity from mph to m/s
for i=1:k
    %Vehicle acceleration using derivative approximation
    a(i,1)=ve(i)-V;   
    %Updating V to be the previous velocity for the next acceleration
    %calculation 
    V=ve(i);
    %Calculating mechanical power required to drive the vehicle
    Pm(i,1)=(0.5*p*Cd*A*(ve(i,1)^2)+m*g*(Cr+a(i,1)/g))*ve(i,1)/1000+2; 
    time(i)=i;          %Time in seconds
end

%%
% *Matrices and arrays*
one=ones(1,k);              %lxk matrix of ones
none=one(1,1:k-1);         %1xk-1 matrix of ones
%D is an "identity" matrix with -1 along diagonal and 1 along diagonal one
%above the center diagonal 
D=-diag(one)+diag(none,1);  
Z=zeros(k);                 %kxk matrix of zeros
T=eye(k);                   %kxk identity matrix
Onit=ones(k,1);             %kx1 matrix of ones
ED=D;               %ED is a kxk identity matrix with the element (k,k)=0
ED(k,k)=0;
c=zeros(1,8*k);
c(1,1:k)=one;
%%
% *Generating equality constraint matrices*
%% 
%  The matrix equality constraint Ax==b is made from the equality equations
%  where A is a (4*k+1)x11*k matrix and b is a (4*k+1)x1 matrix. Slack and surplus  
%  variables were added to 2 equations to convert the inequality  
%  constraints that could cause the set of feasible solutions to degenerate
%  the rank of the constraints into equality constraints. The storage power
%  of the battery (Ps) was split into positive (Psp) and negative values
%  (Psn) so that the variables, Psp and Psn, could be positive. It is
%  required that all variables in the linear program be greater than zero.
%  The battery power was determined from the equation below.
%%
% $P_s=P_s^+-P_s^-$
%
%  By substituting the equation for battery power into the constraints, 
%  Ax==b consisted of the following 5 equations: 
%%
% $f-0.059P_e-s_1=0.075$
%%
% $$-f+0.096P_e+s_2=1.041667$$
%%
% $P_e+P_s^--P_s^+-P_b=P_m$
%%
% $E(k+1)-E(k)-0.8P_s^+(k)+P_s^-(k)=0$
%%
% $E(1)-E(1371)=0$

A1=[T,-0.059.*T,-T,Z,Z,Z,Z,Z;-T,0.096.*T,Z,T,Z,Z,Z,Z;Z,T,Z,Z,T,-T,-T,Z;Z,Z,Z,Z,T,-0.8.*T,Z,ED];
A=zeros(4*k+1,8*k);
A(4*k+1,7*k+1)=1;
A(4*k+1,8*k)=-1;
A(1:4*k,1:8*k)=A1+A(1:4*k,1:8*k);

b=zeros(4*k+1,1);               %Initializing b
b(1:k,1)=0.075.*Onit;           %Equation 1 right side
b(k+1:2*k,1)=1.041667.*Onit;    %Equation 2 right side
b(2*k+1:3*k,1)=Pm;              %Equation 3 right side
%%
% *Generating inequality constraint matrices*
%% 
%  The matrix inequality constraint Gx<=H was made from the inequality
%  equations where G is a 7*kx11*k matrix and H is a 7*kx1 matrix. The 
%  inequality constraints consist of the upper bound limits on the four
%  parameters Pe, Psp, Psn, Pb, E, and the upper bound limits on the rate
%  of change in engine supply power (Pe). In total, 7 equations were
%  combined to make the matrix inequality constraint.
%%
% $P_e\leq50$
%%
% $P_s^+\leq9.54$
%%
% $P_s^-\leq9.54$
%%
% $P_b\leq13.3625$
%%
% $E\leq2160$
%%
% $P_e(k+1)-P_e(k)\leq10$
%%
% $P_e(k)-P_e(k+1)\leq20$

G=[Z,T,Z,Z,Z,Z,Z,Z;Z,Z,Z,Z,T,Z,Z,Z;Z,Z,Z,Z,Z,T,Z,Z;Z,Z,Z,Z,Z,Z,T,Z;Z,Z,Z,Z,Z,Z,Z,T;Z,D,Z,Z,Z,Z,Z,Z;Z,-D,Z,Z,Z,Z,Z,Z];
H=[50.*Onit;9.54.*Onit;6.875.*Onit;13.585.*Onit;2160.*Onit;10.*Onit;20.*Onit];
%%
% *CVX Code*
%% 
%  With the additions of slack and surplus variables, the total number of
%  variables in the convex optimization problem is 10968 with 8 vectors 
%  each with 1371 elements (the vectors are fuel consumption (f), engine
%  power (Pe), surplus variable (s1), slack variable (s2), negative storage
%  power (Psn), positive storage power (Psp), brake power (Pb), battery
%  energy (E)). The problem is a linear program optimization problem,  
%  therefore, all of the variables must be positive. The battery energy has
%  a minimum value constraint that it must be greater than 432 
%  kiloWatt-seconds.
cvx_begin
    variables x(8*k,1)
    minimize c*x
    subject to
        A*x==b
        G*x<=H
        x>=0
        x(7*k+1:8*k,1)>=432
cvx_end
%%
% *Renaming variables*
f=x(1:k,1)*3.6;         %Fuel consumption [kg/hr]
Pen=x(k+1:2*k,1);       %Engine power [kW]
s1=x(2*k+1:3*k,1);      %Surplus variable
s2=x(3*k+1:4*k,1);      %Slack variable
Psn=x(4*k+1:5*k,1);     %Negative component of battery power [kW]
Psp=x(5*k+1:6*k,1);     %Positive component of battery power [kW]
Ps=Psp-Psn;             %Battery power [kW]
Pb=x(6*k+1:7*k,1);      %Brake power [kW]
E=x(7*k+1:8*k,1)/3600;  %Battery energy [kW-hr]
%% Visualization for Hybrid Vehicle
% *Generating plots*
for i=1:k
    if Pm(i,1)<-6.875       %Inverter has a lower limit of -6.875 kW
        Pm(i,1)=-6.875;
    end
end
figure;
plot(Pm)
title('Drive Inverter Power for Hybrid Vehicle')
ylabel('Power [kW]')
xlabel('Time [s]')
axis([0 1400 -50 50])
figure;
plot(Pen)
title('Spark Ignition Power for Hybrid Vehicle')
ylabel('Power [kW]')
xlabel('Time [s]')
axis([0 1400 0 25])
figure;
plot(f)
title('Spark Ignition Fuel Use for Hybrid Vehicle')
ylabel('Fuel Usage [kg/hr]')
xlabel('Time [s]')
axis([0 1400 0 6])
figure;
plot(Pb)
title('Brake Power for Hybrid Vehicle')
ylabel('Power [kW]')
xlabel('Time [s]')
axis([0 1400 -50 50])
figure;
plot(Ps)
title('PbA Battery Power for Hybrid Vehicle')
ylabel('Power [kW]')
xlabel('Time [s]')
axis([0 1400 -10 10])
figure;
plot(E)
title('PbA Battery Energy for Hybrid Vehicle')
ylabel('Power [kW-hr]')
xlabel('Time [s]')
axis([0 1400 0.1 0.4])
%%
% *Calculating optimal global fuel economy*
%Integrating fuel consumption [g/s] to total fuel [g]
fuel=trapz(time,x(1:k,1));   
%Integrating velocity [m/s] to total distance travelled [m]
distance=trapz(time,ve);    
%Calculating distance travelled per total fuel consumed and converting to
%miles per gallon.
MPG=(distance*1000/fuel)*0.734/(0.2642*1609.34);
display(MPG)
%% Solution for Internal Combustion Vehicle(without battery)
% *Generating equality constraint matrices*
%%
%  The solution for the internal combustion vehicle uses the same input 
%  parameters and matrices and arrays as initialized above. The matrix 
%  equality constraint Ax==b is made from the equality equations where A is
%  a 3*kx11*k matrix and b is a 3*kx1 matrix. Slack and surplus variables 
%  were added to 2 equations to convert the inequality constraints into
%  equality constraints. Ax==b consists of 3 equations:  
%%
% $f-0.059*P_e-s_1=0.075$
%%
% $$-f+0.096*P_e+s_2=1.041667$$
%%
% $P_e-P_b=P_m$
%%
A=[T,-0.059.*T,-T,Z,Z;-T,0.096.*T,Z,T,Z;Z,T,Z,Z,-T];

b=zeros(3*k,1);                 %Initializing b
b(1:k,1)=0.075.*Onit;           %Equation 1 right side
b(k+1:2*k,1)=1.041667.*Onit;    %Equation 2 right side
b(2*k+1:3*k,1)=Pm;              %Equation 3 right side

c=zeros(1,5*k);
c(1,1:k)=one;
%%
% *Generating inequality constraint matrices*
%%
%  The matrix inequality constraint Gx<=H is made from the inequality
%  equations where G is a 4*kx11*k matrix and H is a 4*kx1 matrix. The 
%  inequality constraints consist of the upper bound limits on the two
%  parameters Pe, Pb, and upper bound limits on the engine slew rate. The
%  four inequality equations are as follows:
%%
% $P_e\leq50$
%%
% $P_b\leq23.125$
%%
% $P_e(k+1)-P_e(k)\leq10$
%%
% $P_e(k)-P_e(k+1)\leq20$

G=[Z,T,Z,Z,Z;Z,Z,Z,Z,T;Z,D,Z,Z,Z;Z,-D,Z,Z,Z];
H=[50.*Onit;23.125.*Onit;10.*Onit;20.*Onit];
%%
% *CVX Code*
%%
%  With the additions of slack and surplus variables, the total number of
%  variables in the convex optimization problem is 6855 with 5 vectors 
%  each with 1371 elements (the vectors are fuel consumption (f), engine
%  power (Pe), surplus variable (s1), slack variable (s2), brake power 
%  (Pb)). The problem is a linear program optimization problem, therefore, 
%  all of the variables must be positive. 
% 
cvx_begin
    variables x(5*k,1)
    minimize c*x
    subject to
        A*x==b
        G*x<=H
        x>=0
cvx_end
%%
% *Renaming variables*
f=x(1:k,1)*3.6;         %Fuel consumption [kg/hr]
Pen=x(k+1:2*k,1);       %Engine power [kW]
s1=x(2*k+1:3*k,1);      %Surplus variable
s2=x(3*k+1:4*k,1);      %Slack variable
Pb=x(4*k+1:5*k,1);      %Brake power [kW]
%% Visualization for Internal Combustion Vehicle(without battery)
% *Generating plots*
figure;
plot(Pen)
title('Spark Ignition Power for IC Vehicle')
ylabel('Power [kW]')
xlabel('Time [s]')
axis([0 1400 0 30])
figure;
plot(f)
title('Spark Ignition Fuel Use for IC Vehicle')
ylabel('Fuel Usage [kg/hr]')
xlabel('Time [s]')
axis([0 1400 0 7])
figure;
plot(Pb)
title('Brake Power for IC Vehicle')
ylabel('Power [kW]')
xlabel('Time [s]')
axis([0 1400 -50 50])
%%
% *Calculating optimal global fuel economy*
%Integrating fuel consumption [g/s] to total fuel [g]
fuel=trapz(time,x(1:k,1));   
%Integrating velocity [m/s] to total distance travelled [m]
distance=trapz(time,ve);    
%Calculating distance travelled per total fuel consumed and converting to
%miles per gallon.
IC_MPG=(distance*1000/fuel)*0.734/(0.2642*1609.34);
display(IC_MPG)
%% Analysis and Conclusions
% 
%     The code successfully reproduced the global fuel economy for both the
%  hybrid vehicle and the internal combustion vehicle. The authors had a
%  global fuel economy of 44.44 mpg and 41.55 mpg for the hybrid vehicle
%  and internal combustion vehicle, respectively. Whereas the reproduced
%  results were 44.64 mpg and 41.54 mpg for the hybrid and internal
%  combustion vehicles respectively. The percentage errors between the two
%  sets of results were 0.45% and 0.02%. 
%    The authors did not provide any graphs for the internal combustion
%  vehicle, however, they did provide graphs for the hybrid vehicle of the 
%  drive inverter power, engine power, fuel usage, brake power, battery 
%  power and battery energy. The reproduced graphs for the drive inverter 
%  power and brake power closely matched the graphs given in the paper. 
%     The reproduced spark ignition power followed the same trend as the 
%  given spark ignition power however the reproduced spark ignition power 
%  values appeared to be greater than the given values by a factor of 0.25 
%  for values greater than  approximately 10 kW. The reproduced spark 
%  ignition fuel use also over predicted the given results by some vertical
%  offset and factor. Fromthese results, it is suspected that the  
%  inequality equations relating fuel usage to spark ignition power given 
%  in the paper were eithersubject to error or required further 
%  modifications before implementing them in the linear program. 
%     The positive values of the reproduced battery power perfectly matched
%  the positive values of the given batterypower when an upper limit of 
%  6.875 kW was applied.  Although an upper limit of 6.875 kW was not  
%  explicitly given in the paper, it was implicity given from the given 
%  inverter power graph. The graph shows that a maximum of 6.875 kW can be 
%  regenerated from the vehicle's kinectic power. Therefore, due to the 
%  inverter power upper limit, the battery can only accept 6.875 kW. The 
%  negative battery power did not match the given negative battery power. 
%  This occurred because the power supplied by the engine was nearly enough
%  to power the motor, brakes and auxiliary systems. The maximum power that  
%  the battery had to supply was 1.3322 kW. The engine was capable of 
%  supplying a maximum power of 50 kW, however, only a maximum of 26.875 kW
%  was used because the cost function was proportional to spark ignition 
%  power (engine supply power).
%     The battery energy had the same trend as the given battery energy, 
%  however, the reproduced graph was different than the given graph because 
%  the battery energy was dependent on the battery power. 
%     Although the plots of engine power and engine fuel use match with 
%  those presented in the paper,its not the same in case of the plots of 
%  battery power and energy and engine fuel use. So, we modified the limits 
%  on battery energy,battery charge and discharge rates. For battery  
%  maximum capacity of 0.4kW-hrs, maximum charge and discharge rates of 
%  6.54kW and approximating the fuel consumption curve of the engine by:
%
% $$f(t)\geq P_e(t)*0.0590+0.0260$$
%
% $$f(t)\geq P_e(t)*0.115+0.84166$$
%
%  We obtain all the plots similar to those presented in paper. However, 
%  the global fuel optimal economy obtained after changing the values is 
%  50.1mpg, with an error of 12% from the value mentioned by the 
%  author (44.44mpg).
%     Lastly, the code did not behave as the code described by the authors.
%  The code for the hybrid vehicle as reproduced in this project had 23306
%  variables and 6853 equality constraints. As recorded by Tate and Boyd,
%  their program had 26068 variables and 19210 constraints. The reproduced
%  code took 0.65 seconds to complete whereas Tate and Boyd reported their
%  code taking less than 2 minutes. These differences could be related to
%  the differing softwares used to execute the linear program. As stated
%  above, the reproduced code used CVX in Matlab while the original authors
%  used PCx in Pentium Pro.
%% References
%  [1] Tate, E. D. and Boyd, S. P.,2000, "Finding ultimiate limits of 
%  performance for hybrid electric vehicles", _Future Transporation_ 
%  _Technology Conference_, 2000
% 
%  [2] Michael Grant and Stephen Boyd. CVX: Matlab software for disciplined
%  convex programming, version 2.0 beta. http://cvxr.com/cvx, September 
%  2013.
%
%  [3] Michael Grant and Stephen Boyd. Graph implementations for nonsmooth 
%  convex programs, Recent Advances in Learning and Control (a tribute to 
%  M. Vidyasagar), V. Blondel, S. Boyd, and H. Kimura, editors, pages 
%  95-110, Lecture Notes in Control and Information Sciences, Springer, 
%  2008. http://stanford.edu/~boyd/graph_dcp.html.
%
%  [4] EPA, United States Environmental Protection Agency, 2016, "Federal
%  Test Procedure Revisions," from https://www3.epa.gov/otaq/sftp.htm