104 lines
2.8 KiB
Matlab
104 lines
2.8 KiB
Matlab
%
|
|
% Problem 1c: Effect of bounded sinusoidal disturbance on measurement x(t)
|
|
%
|
|
clear
|
|
|
|
% True system parameters
|
|
m_true = 1.315;
|
|
b_true = 0.225;
|
|
k_true = 0.725;
|
|
|
|
% Simulation parameters
|
|
Ts = 0.001;
|
|
T_total = 40;
|
|
t_full = 0:Ts:T_total;
|
|
|
|
% Generate full input signal
|
|
u_full = 2.5 * sin(t_full);
|
|
|
|
% Simulate the true system
|
|
x = zeros(1, length(t_full));
|
|
dx = zeros(1, length(t_full));
|
|
ddx = zeros(1, length(t_full));
|
|
x(1) = 0; dx(1) = 0;
|
|
for k = 1:length(t_full)-1
|
|
f = @(x_, dx_, u_) (1/m_true) * (u_ - b_true * dx_ - k_true * x_);
|
|
k1 = f(x(k), dx(k), u_full(k));
|
|
k2 = f(x(k) + Ts/2 * dx(k), dx(k) + Ts/2 * k1, u_full(k));
|
|
k3 = f(x(k) + Ts/2 * dx(k), dx(k) + Ts/2 * k2, u_full(k));
|
|
k4 = f(x(k) + Ts * dx(k), dx(k) + Ts * k3, u_full(k));
|
|
ddx(k) = k1;
|
|
dx(k+1) = dx(k) + Ts/6 * (k1 + 2*k2 + 2*k3 + k4);
|
|
x(k+1) = x(k) + Ts * dx(k);
|
|
end
|
|
ddx(1:end-1) = diff(dx) / Ts;
|
|
ddx(end) = ddx(end-1);
|
|
|
|
% Initial estimation (clean) using Lyapunov
|
|
T_total = 40;
|
|
index_limit = round(T_total / Ts);
|
|
t = t_full(1:index_limit);
|
|
N = length(t);
|
|
u = u_full(1:index_limit);
|
|
x = x(1:index_limit);
|
|
dx = dx(1:index_limit);
|
|
ddx = ddx(1:index_limit);
|
|
|
|
phi_all = [ddx; dx; x];
|
|
theta_hat = zeros(3, N);
|
|
theta_hat(:, 1) = [1; 1; 1];
|
|
gamma = 0.66;
|
|
for k = 1:N-1
|
|
phi = phi_all(:,k);
|
|
y = u(k);
|
|
y_hat = theta_hat(:,k)' * phi;
|
|
e = y - y_hat;
|
|
theta_hat(:,k+1) = theta_hat(:,k) + Ts * gamma * e * phi;
|
|
end
|
|
|
|
% Disturbance settings
|
|
eta0 = 0.1;
|
|
f0 = 0.5;
|
|
eta = eta0 * sin(2 * pi * f0 * t);
|
|
x_noisy = x + eta;
|
|
|
|
% Use clean derivatives, noisy position
|
|
phi_all_noise = [ddx; dx; x_noisy];
|
|
theta_hat_noise = zeros(3, N);
|
|
theta_hat_noise(:, 1) = [1; 1; 1];
|
|
|
|
for k = 1:N-1
|
|
phi = phi_all_noise(:,k);
|
|
y = u(k);
|
|
y_hat = theta_hat_noise(:,k)' * phi;
|
|
e = y - y_hat;
|
|
theta_hat_noise(:,k+1) = theta_hat_noise(:,k) + Ts * gamma * e * phi;
|
|
end
|
|
|
|
fprintf('\n1c: Final estimates with disturbance:\n');
|
|
fprintf('Estimated m: %.4f, b: %.4f, k: %.4f\n', ...
|
|
theta_hat_noise(1,end), theta_hat_noise(2,end), theta_hat_noise(3,end));
|
|
|
|
figure('Name', '1c - Parameter Estimation with Disturbance', 'Position', [100, 100, 1280, 860]);
|
|
sgtitle(sprintf('Lyapunov Estimation with Disturbance | η_0 = %.2f', eta0));
|
|
|
|
subplot(3,1,1);
|
|
plot(t, theta_hat(1,:), 'b', t, theta_hat_noise(1,:), '--b', 'LineWidth', 1.2);
|
|
ylabel('m(t)'); grid on; title('Μάζα');
|
|
legend('Clear', 'With noise');
|
|
|
|
subplot(3,1,2);
|
|
plot(t, theta_hat(2,:), 'r', t, theta_hat_noise(2,:), '--r', 'LineWidth', 1.2);
|
|
ylabel('b(t)'); grid on; title('Απόσβεση');
|
|
legend('Clear', 'With noise');
|
|
|
|
subplot(3,1,3);
|
|
plot(t, theta_hat(3,:), 'k', t, theta_hat_noise(3,:), '--k', 'LineWidth', 1.2);
|
|
ylabel('k(t)'); xlabel('t [s]'); grid on; title('Ελαστικότητα');
|
|
legend('Clear', 'With noise');
|
|
|
|
if ~exist('output', 'dir')
|
|
mkdir('output');
|
|
end
|
|
saveas(gcf, sprintf('output/Prob1c_disturbance_eta%.2f.png', eta0));
|