
%----------------------------------------------------------
%Brute force parameter estimation of a DC motor
%assuming gain & time-constant model

%Finn Aakre Haugen (finn.haugen@hit.no)
%University College of Southeast Norway
%17 Sept 2016
%----------------------------------------------------------
%Model (gain & time-constant model):
%dydt(t)=(1/T)*[K*u(t)-y(t)]
%where
%y [V] is rotational speed measurement.
%u [V] is control signal.
%K is gain (to be estimated).
%T is time-constant (to be estimated).
%----------------------------------------------------------
format compact

clear all
close all
%----------------------------------------------------------
%Loading data:
load logfile1.lvm;
Ts=0.02; %Sampling interval
L=length(logfile1);%(Matrix name becomes same as logfile name.)
N=round(L/2);
%Generates proper time signal, and extracts data from logfile:
t_estim=Ts*[1:N]';
u_estim=logfile1(1:N,2);
y_estim=logfile1(1:N,3);
t_valid=Ts*[N+1:L]';
u_valid=logfile1(N+1:L,2);
y_valid=logfile1(N+1:L,3);

y_sim=y_estim*0+y_estim(1);

%Initialization:

%Creating arrays of parameter values to be tried:
K_min=0.2;K_max=2;N_K=100;
K_array=linspace(K_min,K_max,N_K);

T_min=0.1;T_max=1;N_T=100;
T_array=linspace(T_min,T_max,N_T);

%Initialization of optim criterion:
sspe_optim=inf;
K_optim=-inf;
T_optim=-inf;

tic;

for k_K=1:length(K_array)
    K=K_array(k_K);

for k_T=1:length(T_array)
    T=T_array(k_T);
     
%Objective function:
for k_sim=1:length(t_estim)-1,
    dy_sim_dt_k=(1/T)*(-y_sim(k_sim)+K*u_estim(k_sim));
    y_sim(k_sim+1)=y_sim(k_sim)+Ts*dy_sim_dt_k;
end

pred_error=y_estim-y_sim;
sspe=sum(pred_error.*pred_error);%sspe: sum of squared prediction errors

%Improving the previous solution:
 if sspe <= sspe_optim,
     sspe_optim=sspe;
     K_optim=K;
     T_optim=T;
 end
 
 %Storing objective function values for later plotting:
 sspe_matrix(k_K,k_T)=sspe;
 
end
end

%--------------------------------------------------
%Calculation of execution times:

disp('-------------------')
disp('Execution times of running the nested for-loops:')

dt_elapsed=toc
dt_elapsed_per_iteration=toc/(N_K*N_T)
N_iterations=N_K*N_T
% break
%--------------------------------------------------
%Displaying the optimal solution:

disp('-------------------')
disp('Optimal solution:')

sspe_optim
K_optim
T_optim

%--------------------------------------------------
%Plotting y_logg and y_sim:
%Objective function:
K=K_optim;
T=T_optim;

for k_sim=1:length(t_estim)-1,
    dy_sim_dt_k=(1/T)*(-y_sim(k_sim)+K*u_estim(k_sim));
    y_sim(k_sim+1)=y_sim(k_sim)+Ts*dy_sim_dt_k;
end

% n=1;
% figure(n)
h=figure; %Getting figure handle
fig_posleft=8;fig_posbottom=1.5;fig_width=20;fig_height=16;
fig_pos_size_1=[fig_posleft,fig_posbottom,fig_width,fig_height];
set(gcf,'Units','centimeters','Position',fig_pos_size_1);
figtext='Estimation of model params for DC motor';
set(gcf,'Name',figtext,'NumberTitle','on')

subplot(2,1,1)
plot(t_estim,y_sim,'r',t_estim,y_estim,'b')
axis([t_estim(1),t_estim(end),-3,3])
grid
title('Red: y_{sim} with optimal parameters. Blue: y_{real,estim}.')
xlabel('t [s]')
ylabel('[V]')

subplot(2,1,2)
plot(t_estim,u_estim)
axis([t_estim(1),t_estim(end),-3,3])
grid
title('u_{real,estim} (used for estimation)')
xlabel('t [s]')
ylabel('[V]')

%--------------------------------------------------

%Model validation:

%Simulation using u_valid (data series not used in param estimation)
%and estimated (optimal) params:
K=K_optim;
T=T_optim;

y_sim=y_valid*0+y_valid(1);

for k_sim=1:length(t_valid)-1,
    dy_sim_dt_k=(1/T)*(-y_sim(k_sim)+K*u_valid(k_sim));
    y_sim(k_sim+1)=y_sim(k_sim)+Ts*dy_sim_dt_k;
end

%Plotting both y_sim and y_valid:

% n=n+1;
% figure(n)
h=figure; %Getting figure handle
fig_posleft=8;fig_posbottom=1.5;fig_width=20;fig_height=18;
fig_pos_size_1=[fig_posleft,fig_posbottom,fig_width,fig_height];

set(gcf,'Units','centimeters','Position',fig_pos_size_1);
figtext='Estimation of model params for DC motor';
set(gcf,'Name',figtext,'NumberTitle','on')
subplot(2,1,1)
plot(t_valid,y_sim,'r',t_valid,y_valid,'b')
axis([t_valid(1),t_valid(end),-3,3])
grid
title('Red: y_{sim} with optimal parameters. Blue: y_{real,valid}.')
xlabel('t [s]')
ylabel('[V]')

subplot(2,1,2)
plot(t_valid,u_valid)
axis([t_valid(1),t_valid(end),-4,4])
grid
title('u_{real,valid} (used for validation)')
xlabel('t [s]')
ylabel('[V]')

% break
%--------------------------------------------------
%3D plot of the objective function, showing optimal solution:

% n=n+1;
% figure(n)
h=figure; %Getting figure handle
fig_posleft=8;fig_posbottom=1.5;fig_width=18;fig_height=16;
fig_pos_size_1=[fig_posleft,fig_posbottom,fig_width,fig_height];

set(gcf,'Units','centimeters','Position',fig_pos_size_1);
figtext='Optim plot';
set(gcf,'Name',figtext,'NumberTitle','on')

mesh(K_array,T_array,sspe_matrix');

azimut=-40;elevation=25;
view(azimut,elevation);
xlabel('K')
ylabel('T')
zlabel('sspe')

%Plotting a vertical line of the optimal solution:
N_line=10;
line_K=linspace(K_optim,K_optim,N_line); 
line_T=linspace(T_optim,T_optim,N_line);
point_sspe=linspace(sspe_optim,sspe_optim,N_line);
line_sspe=linspace(0,sspe_optim,N_line);

hold on
title('3D plot of sspe (sum of squared prediction errors). Red * shows optimal solution.')
plot3(line_K,line_T,line_sspe,'*r',line_K,line_T,line_sspe,'r');
hold off

