yay!
This commit is contained in:
parent
59e659ba42
commit
fca885cc28
@ -30,33 +30,128 @@ Ts_third_register = Ts_whole_register/3;
|
||||
sys_third_register = c2d(sys_cont, Ts_third_register, 'zoh');
|
||||
|
||||
% Create a Reference Signal
|
||||
max_time = 1e-2;
|
||||
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 = [0.7+1e-4i 0.7-1e-4i];
|
||||
opts.L = [0.1+0.01i 0.1-0.01i];
|
||||
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.plotting = true;
|
||||
opts.res = 3*4;
|
||||
opts.plotting = false;
|
||||
|
||||
[x_hist, y_hist, u_hist, x_hat_hist] = solve_full_step(opts);
|
||||
[x_hist_fs, y_hist_fs, u_hist_fs, x_hat_hist_fs] = solve_full_step(opts);
|
||||
|
||||
% ADC delay, with sub-steps
|
||||
[ref_third, t_third, N_third] = make_hdd_reference(12*Ts_third_register, Ts_third_register, 1e-2, 42);
|
||||
opts.K = [0.99+1e-4i 0.99-1e-4i];
|
||||
opts.L = [0.2+0.01i 0.2-0.01i];
|
||||
opts.debug= true;
|
||||
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 = 3;
|
||||
opts.sub_steps = num_sub_steps;
|
||||
opts.r = ref_third;
|
||||
opts.N = N_third/3;
|
||||
opts.res = 48;
|
||||
opts.plotting = true;
|
||||
opts.N = N_third/num_sub_steps;
|
||||
opts.res = 3*4;
|
||||
opts.plotting = false;
|
||||
|
||||
[x_hist, y_hist, u_hist, x_hat_hist] = solve_sub_step(opts);
|
||||
[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
|
||||
|
||||
@ -11,6 +11,7 @@ function [x_hist, y_hist, u_hist, x_hat_hist] = solve_full_step(opts)
|
||||
% ▸ L – observer gain matrix
|
||||
% ▸ K – state‑feedback gain matrix
|
||||
% ▸ r – reference trajectory [nu × N]
|
||||
% ▸ res – number of bits of resolution for ADC
|
||||
|
||||
% Optional fields in **opts**
|
||||
% ▸ plotting - plot things? or no?
|
||||
@ -39,10 +40,11 @@ N = opts.N;
|
||||
L = transpose(place(sys.A', sys.C', opts.L));
|
||||
K = place(sys.A, sys.B, opts.K);
|
||||
r = opts.r;
|
||||
res = opts.res;
|
||||
plotting = opts.plotting;
|
||||
|
||||
% ----------------‑‑ Diagnostics ----------------------------------------
|
||||
fprintf('\n*** solve_SAR_ADC called with: ***\n');
|
||||
fprintf('\n*** solve_SAR_ADC Full-Step called with: ***\n');
|
||||
vars = struct('x0',x0,'N',N,'L',L','K',K,'r',r);
|
||||
vars %#ok<NOPRT>
|
||||
|
||||
@ -53,7 +55,7 @@ Ts = sys.Ts;
|
||||
ny = size(C,1);
|
||||
|
||||
% ----------------‑‑ Pre‑allocate histories -----------------------------
|
||||
x_hist = zeros(nx,N+1);
|
||||
x_hist = zeros(nx,N);
|
||||
y_hist = zeros(ny,N);
|
||||
u_hist = zeros(nu,N);
|
||||
|
||||
@ -64,7 +66,7 @@ useObserver = ~isempty(L);
|
||||
if useObserver
|
||||
fprintf('Observer enabled.\n');
|
||||
x_hat = [0;0];
|
||||
x_hat_hist = zeros(nx,N+1);
|
||||
x_hat_hist = zeros(nx,N);
|
||||
x_hat_hist(:,1) = x_hat;
|
||||
else
|
||||
x_hat_hist = [];
|
||||
@ -91,9 +93,10 @@ for k = 1:N-1
|
||||
|
||||
% Plant output
|
||||
y_hist(:,k+1) = C*x_hist(:,k) + D*u_k; %SHIFT RIGHT TO INDUCE DELAY
|
||||
ADC_output = SAR_ADC_approx(y_hist(:,k), res, -0.1, 1.1, 1);
|
||||
|
||||
% Propagate observer states
|
||||
x_hat = A*x_hat + B*u_k + L*(y_hist(:,k) - C*x_hat - D*u_k);
|
||||
x_hat = A*x_hat + B*u_k + L*(ADC_output(1) - C*x_hat - D*u_k);
|
||||
x_hat_hist(:,k+1) = x_hat;
|
||||
|
||||
%Calculate Next State
|
||||
@ -104,7 +107,7 @@ end
|
||||
|
||||
if plotting
|
||||
figure;
|
||||
time = (0:N)*Ts;
|
||||
time = (0:N-1)*Ts;
|
||||
for i = 1:nx
|
||||
subplot(nx+1,2,i);
|
||||
plot(time,x_hat_hist(i,:),'-xr', time,x_hist(i,:),'-ob');
|
||||
@ -123,7 +126,7 @@ if plotting
|
||||
ylabel('Position Error');
|
||||
xlabel('Time (s)');
|
||||
grid on;
|
||||
sgtitle('SAR ADC State Trajectories, and Reference Error');
|
||||
sgtitle('SAR ADC State Trajectories, and Reference Error (FULL SAMPLE)');
|
||||
end
|
||||
fprintf('Complete!\n')
|
||||
end
|
||||
|
||||
@ -47,7 +47,7 @@ res = opts.res;
|
||||
plotting = opts.plotting;
|
||||
|
||||
% ----------------‑‑ Diagnostics ----------------------------------------
|
||||
fprintf('\n*** solve_SAR_ADC called with: ***\n');
|
||||
fprintf('\n*** solve_SAR_ADC Sub-Step called with: ***\n');
|
||||
vars = struct('x0',x0,'N',N,'L',L','K',K,'r',r,'J',J,'res',res);
|
||||
vars %#ok<NOPRT>
|
||||
|
||||
@ -58,7 +58,7 @@ Ts = sys.Ts;
|
||||
ny = size(C,1);
|
||||
|
||||
% ----------------‑‑ Pre‑allocate histories -----------------------------
|
||||
x_hist = zeros(nx,N*J+1);
|
||||
x_hist = zeros(nx,N*J);
|
||||
y_hist = zeros(ny,N*J);
|
||||
u_hist = zeros(nu,N*J);
|
||||
|
||||
@ -68,7 +68,7 @@ useObserver = ~isempty(L);
|
||||
if useObserver
|
||||
fprintf('Observer enabled.\n');
|
||||
x_hat = [0;0];
|
||||
x_hat_hist = zeros(nx,N*J+1);
|
||||
x_hat_hist = zeros(nx,N*J);
|
||||
x_hat_hist(:,1) = x_hat;
|
||||
else
|
||||
x_hat_hist = [];
|
||||
@ -90,39 +90,44 @@ end
|
||||
% ----------------‑‑ Simulation loop ------------------------------------
|
||||
for k = (1:J:(N-1)*J)%N-1
|
||||
x_hist(:,k+1) = A*x_hist(:,k+0) + B*u_hist(k+0);
|
||||
y_hist(:,k+0) = C*x_hist(:,k+0);
|
||||
y_hist(:,k+1) = C*x_hist(:,k+0);
|
||||
|
||||
%ASSUME SENSOR RANGE OF 0-1 RAD -> 0 -> 2^12
|
||||
sensor_value = y_hist(:,k+0)*2^res;
|
||||
ADC_output = SAR_ADC_approx(sensor_value, res, -0.2, 1.2, J);
|
||||
ADC_output = SAR_ADC_approx(y_hist(k+1), res, -0.1, 1.1, J);
|
||||
%fudge_factor = [2^4/2^12, 2^8/2^12, 1]; %manually adjust observer gains
|
||||
fudge_factor = [1, 1, 1]; %manually adjust observer gains
|
||||
|
||||
%FIRST SUBSTEP
|
||||
%you get the first n_split bits from the last substep
|
||||
|
||||
x_hat_hist(:,k+1) = A*x_hat_hist(:,k+0) + ...
|
||||
B*u_hist(k+0) + ...
|
||||
L*( ADC_output(1) - C*x_hat_hist(:,k+0));
|
||||
fudge_factor(1)*L*( ADC_output(1) - C*x_hat_hist(:,k+0));
|
||||
|
||||
u_hist(k+2) = K*(r(:,k+1)-x_hat_hist(:,k+1));
|
||||
u_hist(k+1) = K*(r(:,k+1)-x_hat_hist(:,k+1));
|
||||
|
||||
%SECOND SUBSTEP
|
||||
%now you get the first refinement of that original measurement
|
||||
x_hist(:,k+2) = A*x_hist(:,k+1) + B*u_hist(k+1);
|
||||
y_hist(:,k+2) = C*x_hist(:,k+1);
|
||||
|
||||
x_hat_hist(:,k+2) = A*(A*x_hat_hist(:,k+0) + ...
|
||||
B*u_hist(k+0) + ...
|
||||
L*(ADC_output(2) - C*x_hat_hist(:,k+0))...
|
||||
fudge_factor(2)*L*(ADC_output(2) - C*x_hat_hist(:,k+0))...
|
||||
) + ...
|
||||
B*u_hist(k+1);
|
||||
|
||||
u_hist(k+2) = K*(r(:,k+2)-x_hat_hist(:,k+2));
|
||||
|
||||
%THIRD SUBSTEP
|
||||
% You finally got the full measurement, and the adc starts
|
||||
%taking a new measurement
|
||||
x_hist(:,k+3) = A*x_hist(:,k+2) + B*u_hist(k+2);
|
||||
y_hist(:,k+3) = C*x_hist(:,k+2);
|
||||
|
||||
x_hat_hist(:,k+3) = A*(A*(A*x_hat_hist(:,k+0) + ...
|
||||
B*u_hist(k+0) + ...
|
||||
L*(ADC_output(3) - C*x_hat_hist(:,k+0))...
|
||||
fudge_factor(3)*L*(ADC_output(3) - C*x_hat_hist(:,k+0))...
|
||||
) + ...
|
||||
B*u_hist(k+1)...
|
||||
) + ...
|
||||
@ -131,13 +136,10 @@ for k = (1:J:(N-1)*J)%N-1
|
||||
u_hist(k+3) = K*(r(:,k+3)-x_hat_hist(:,k+3));
|
||||
%── DEBUG PRINTS (only if opts.debug) ────────────────────────────
|
||||
if isfield(opts,'debug') && opts.debug
|
||||
|
||||
fprintf("\n=== Macro‐step k = %d ===\n", k);
|
||||
|
||||
%— Sensor / ADC —
|
||||
fprintf("-- Sensor + ADC --\n");
|
||||
fprintf(" sensor_value = [%s] (y_hist(:,%d)*2^%d)\n", ...
|
||||
num2str(sensor_value.', ' %.4f'), k, res);
|
||||
fprintf("-- ADC --\n");
|
||||
[mADC,nADC] = size(ADC_output);
|
||||
fprintf(" ADC_output = %d×%d matrix:\n", mADC, nADC);
|
||||
for row = 1:mADC
|
||||
@ -154,7 +156,7 @@ for k = (1:J:(N-1)*J)%N-1
|
||||
|
||||
%— Microstep 2 (@ k+2) —
|
||||
fprintf("\n-- Microstep 2 (t = k+2) --\n");
|
||||
fprintf(" r(:,%d) = [%s]\n", k+1, num2str(r(:,k+2).', ' %.4f'));
|
||||
fprintf(" r(:,%d) = [%s]\n", k+2, num2str(r(:,k+2).', ' %.4f'));
|
||||
fprintf(" x_hist(:,%d) = [%s]\n", k+2, num2str(x_hist(:,k+2).', ' %.4f'));
|
||||
fprintf(" x_hat_hist(:,%d) = [%s]\n", k+2, num2str(x_hat_hist(:,k+2).', ' %.4f'));
|
||||
fprintf(" y_hist(:,%d) = [%s]\n", k+2, num2str(y_hist(:,k+2).', ' %.4f'));
|
||||
@ -162,7 +164,7 @@ for k = (1:J:(N-1)*J)%N-1
|
||||
|
||||
%— Microstep 3 (@ k+3) —
|
||||
fprintf("\n-- Microstep 3 (t = k+3) --\n");
|
||||
fprintf(" r(:,%d) = [%s]\n", k+1, num2str(r(:,k+3).', ' %.4f'));
|
||||
fprintf(" r(:,%d) = [%s]\n", k+3, num2str(r(:,k+3).', ' %.4f'));
|
||||
fprintf(" x_hist(:,%d) = [%s]\n", k+3, num2str(x_hist(:,k+3).', ' %.4f'));
|
||||
fprintf(" x_hat_hist(:,%d) = [%s]\n", k+3, num2str(x_hat_hist(:,k+3).', ' %.4f'));
|
||||
fprintf(" y_hist(:,%d) = [%s]\n", k+3, num2str(y_hist(:,k+3).', ' %.4f'));
|
||||
@ -176,7 +178,7 @@ end
|
||||
|
||||
if plotting
|
||||
figure;
|
||||
time = (0:N*J)*Ts;
|
||||
time = (0:N*J-1)*Ts;
|
||||
|
||||
for i = 1:nx
|
||||
subplot(nx+1,2,i);
|
||||
@ -189,20 +191,21 @@ if plotting
|
||||
|
||||
subplot(nx+1,2,3);
|
||||
hold on;
|
||||
stairs(time(1:N), r(1,1:N), "Color", "#00FF80")
|
||||
stairs(time(1:N), r(2,1:N), "Color", "#88aa00")
|
||||
stairs(time, r(1,:), "Color", "#00FF80")
|
||||
stairs(time, r(2,:), "Color", "#88aa00")
|
||||
hold off;
|
||||
|
||||
ylabel('Position Demanded');
|
||||
xlabel('Time (s)');
|
||||
|
||||
subplot(nx+1,2,4);
|
||||
stairs(time(1:N), x_hist(1,1:N) - r(1,1:N), "Color", "#964B00")
|
||||
stairs(time, x_hist(1,:) - r(1,:), "Color", "#964B00")
|
||||
ylabel('Position Error');
|
||||
xlabel('Time (s)');
|
||||
grid on;
|
||||
sgtitle('SAR ADC State Trajectories, and Reference Error');
|
||||
sgtitle('SAR ADC State Trajectories, and Reference Error (SUBSAMPLE)');
|
||||
end
|
||||
|
||||
fprintf('Complete!\n')
|
||||
|
||||
end
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user