158 lines
4.6 KiB
Matlab
158 lines
4.6 KiB
Matlab
close all
|
||
% The quick brown fox jumps over the lazy dog. The dog stays blissfully asleep. :)
|
||
% Dane Sabo
|
||
% ME 2046 Final Project Code
|
||
|
||
%% System Setup
|
||
% Continuous System
|
||
J = 0.01; %kgm^2
|
||
C = 0.004; %Nm/(rad/s)
|
||
K = 10; %Nm/rad
|
||
K_i = 0.05; %Nm/rad
|
||
|
||
F = [0 1; -K/J -C/J];
|
||
G = [0; K_i/J];
|
||
|
||
G_disturb = [0; 1/J];
|
||
|
||
C = [1 0];
|
||
D = 0;
|
||
|
||
sys_cont = ss(F, G, C, D);
|
||
|
||
% Digital System Conversion
|
||
Ts_whole_register = 1/15e3; %s
|
||
sys_whole_register = c2d(sys_cont, Ts_whole_register, 'zoh');
|
||
|
||
|
||
% Assume a 12-bit SAR ADC with bits 0-3 in first, bits 4-7 in 2nd, 8-11 in 3rd
|
||
Ts_third_register = Ts_whole_register/3;
|
||
sys_third_register = c2d(sys_cont, Ts_third_register, 'zoh');
|
||
|
||
% Create a Reference Signal
|
||
max_time = 30.05;
|
||
[ref, t, N] = make_hdd_reference(max_time, Ts_whole_register, 1e-2, 42);
|
||
|
||
|
||
%% Running a Simulation
|
||
% System Poles - in continuse domain
|
||
L_poles = [-1/Ts_whole_register/5 + 0.01i, -1/Ts_whole_register/5-0.01i];
|
||
K_poles = [-10/Ts_whole_register/5 + 0.01i, -10/Ts_whole_register/5-0.01i];
|
||
|
||
% ADC Delay, no sub-steps - Setting Up Simulation
|
||
opts = struct(); % start empty
|
||
opts.sys = sys_whole_register;
|
||
opts.x0 = [0; 0];
|
||
opts.N = N;
|
||
opts.K = exp(K_poles*Ts_whole_register);
|
||
opts.L = exp(L_poles*Ts_whole_register);
|
||
% opts.K = [0.7+1e-4i 0.7-1e-4i];
|
||
% opts.L = [0.1+0.01i 0.1-0.01i];
|
||
opts.r = ref;
|
||
opts.res = 3*4;
|
||
opts.plotting = false;
|
||
|
||
[x_hist_fs, y_hist_fs, u_hist_fs, x_hat_hist_fs] = solve_full_step(opts);
|
||
|
||
% ADC delay, with sub-steps
|
||
num_sub_steps = 3;
|
||
[ref_third, t_third, N_third] = make_hdd_reference(max_time, Ts_third_register, 1e-2, 42);
|
||
opts.K = exp(K_poles*Ts_third_register);
|
||
opts.L = exp(L_poles*Ts_third_register);
|
||
% opts.K = [0.7+1e-4i 0.7-1e-4i];
|
||
% opts.L = [0.1+0.01i 0.1-0.01i];
|
||
opts.debug= false;
|
||
opts.sys = sys_third_register;
|
||
opts.sub_steps = num_sub_steps;
|
||
opts.r = ref_third;
|
||
opts.N = N_third/num_sub_steps;
|
||
opts.res = 3*4;
|
||
opts.plotting = false;
|
||
|
||
[x_hist_ss, y_hist_ss, u_hist_ss, x_hat_hist_ss] = solve_sub_step(opts);
|
||
|
||
%% Analysis
|
||
%RMSE - Overall Error Energy
|
||
solNames = {'Full Sample','Sub-Sample'};
|
||
xHists = {x_hist_fs, x_hist_ss}; % cell array of your x_hist outputs
|
||
rHists = {ref, ref_third}; % cell array of your x_hist outputs
|
||
|
||
nSolvers = numel(xHists);
|
||
rmseVals = zeros(1,nSolvers);
|
||
|
||
for i = 1:nSolvers
|
||
% extract the i-th solver’s first-state history
|
||
xi = xHists{i}(1,:);
|
||
ri = rHists{i}(1,:);
|
||
|
||
% compute RMSE
|
||
err = xi - ri;
|
||
mse = mean(err.^2);
|
||
rmseVals(i) = sqrt(mse);
|
||
|
||
% print with the solver’s name
|
||
fprintf(' %s RMSE = %.8f\n', solNames{i}, rmseVals(i));
|
||
end
|
||
|
||
%% ITAE – Averaged over Multiple Runs
|
||
step_time = 1e-2; % step time for HDD head
|
||
max_time_itae = step_time;
|
||
nRuns = 10; % number of repetitions
|
||
solNames = {'Full Sample','Sub-Sample'};
|
||
nSolvers = numel(solNames);
|
||
|
||
% Preallocate a matrix to hold ITAE values for each solver & run
|
||
itaeVals = zeros(nSolvers, nRuns);
|
||
|
||
for j = 1:nRuns
|
||
seedVal = j; % ensure reproducible, paired RNG state
|
||
rng(seedVal); % set the seed
|
||
|
||
%— Full-Step Solver —%
|
||
[ref_fs, t_fs, N_fs] = make_hdd_reference( ...
|
||
max_time_itae, Ts_whole_register, step_time, seedVal);
|
||
opts_fs = struct();
|
||
opts_fs.sys = sys_whole_register;
|
||
opts_fs.x0 = [0; 0];
|
||
opts_fs.N = N_fs;
|
||
opts_fs.K = exp(K_poles*Ts_whole_register);
|
||
opts_fs.L = exp(L_poles*Ts_whole_register);
|
||
opts_fs.r = ref_fs;
|
||
opts_fs.res = 3*4;
|
||
opts_fs.plotting = false; % turn off plots for batch runs
|
||
[x_hist_fs, ~, ~, ~] = solve_full_step(opts_fs);
|
||
|
||
err_fs = x_hist_fs(1,:) - ref_fs(1,:);
|
||
% Discrete‐time ITAE: sum over t * |error|
|
||
itaeVals(1,j) = sum(t_fs .* abs(err_fs));
|
||
|
||
%— Sub-Sample Solver —%
|
||
rng(seedVal); % reset to same seed
|
||
[ref_ss, t_ss, N_ss] = make_hdd_reference( ...
|
||
max_time_itae, Ts_third_register, step_time, seedVal);
|
||
opts_ss = struct();
|
||
opts_ss.sys = sys_third_register;
|
||
opts_ss.x0 = [0; 0];
|
||
opts_ss.N = N_ss/num_sub_steps;
|
||
opts_ss.K = exp(K_poles*Ts_third_register);
|
||
opts_ss.L = exp(L_poles*Ts_third_register);
|
||
opts_ss.sub_steps = num_sub_steps;
|
||
opts_ss.res = 3*4;
|
||
opts_ss.r = ref_ss;
|
||
opts_ss.plotting = false; % disable plotting
|
||
[x_hist_ss, ~, ~, ~] = solve_sub_step(opts_ss);
|
||
|
||
err_ss = x_hist_ss(1,:) - ref_ss(1,:);
|
||
itaeVals(2,j) = sum(t_ss .* abs(err_ss));
|
||
end
|
||
|
||
% Compute the average ITAE for each solver
|
||
avgItae = mean(itaeVals, 2);
|
||
|
||
% Display the results
|
||
fprintf('\nAverage ITAE over %d runs (simulation time = %.4g s):\n', ...
|
||
nRuns, max_time_itae);
|
||
for i = 1:nSolvers
|
||
fprintf(' %s: %.6e\n', solNames{i}, avgItae(i));
|
||
end
|