117 lines
3.2 KiB
Matlab
117 lines
3.2 KiB
Matlab
%
|
|
% Problem 1b: Lyapunov-based Parameter Estimation
|
|
%
|
|
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 = 0:Ts:T_total;
|
|
N = length(t);
|
|
|
|
% Gamma setup
|
|
gamma = 0.66;
|
|
fprintf('Using gamma = %.4f (Lyapunov Based Estimation)\n', gamma);
|
|
|
|
% Define sine input only (as per problem statement)
|
|
u = 2.5 * sin(t);
|
|
|
|
% Simulate the true system
|
|
x = zeros(1, N);
|
|
dx = zeros(1, N);
|
|
ddx = zeros(1, N);
|
|
x(1) = 0; dx(1) = 0;
|
|
for k = 1:N-1
|
|
f = @(x_, dx_, u_) (1/m_true) * (u_ - b_true * dx_ - k_true * x_);
|
|
k1 = f(x(k), dx(k), u(k));
|
|
k2 = f(x(k) + Ts/2 * dx(k), dx(k) + Ts/2 * k1, u(k));
|
|
k3 = f(x(k) + Ts/2 * dx(k), dx(k) + Ts/2 * k2, u(k));
|
|
k4 = f(x(k) + Ts * dx(k), dx(k) + Ts * k3, u(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);
|
|
|
|
% Estimation using Lyapunov structure
|
|
phi_all = [ddx; dx; x]; % shape: [3 x N]
|
|
theta_hat = zeros(3, N);
|
|
theta_hat(:, 1) = [1; 1; 1];
|
|
|
|
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
|
|
|
|
% Final estimates
|
|
fprintf('\nFinal estimates:\n');
|
|
fprintf('Estimated m: %.4f, b: %.4f, k: %.4f\n', theta_hat(1,end), theta_hat(2,end), theta_hat(3,end));
|
|
|
|
% Plot results
|
|
figure('Name', 'Lyapunov Estimation (notes form)', 'Position', [100, 100, 1280, 860]);
|
|
sgtitle(sprintf('Input: sine | Gamma = %.3f | Lyapunov', gamma), 'FontWeight', 'bold');
|
|
|
|
subplot(3,1,1);
|
|
plot(t, theta_hat(1,:), 'b', 'LineWidth', 1.5);
|
|
ylabel('$$\hat{m}(t)$$ [kg]', 'Interpreter', 'latex');
|
|
grid on; title('Εκτίμηση μάζας');
|
|
|
|
subplot(3,1,2);
|
|
plot(t, theta_hat(2,:), 'r', 'LineWidth', 1.5);
|
|
ylabel('$$\hat{b}(t)$$ [Ns/m]', 'Interpreter', 'latex');
|
|
grid on; title('Εκτίμηση απόσβεσης');
|
|
|
|
subplot(3,1,3);
|
|
plot(t, theta_hat(3,:), 'k', 'LineWidth', 1.5);
|
|
ylabel('$$\hat{k}(t)$$ [N/m]', 'Interpreter', 'latex');
|
|
xlabel('t [sec]');
|
|
grid on; title('Εκτίμηση ελαστικότητας');
|
|
|
|
if ~exist('output', 'dir')
|
|
mkdir('output');
|
|
end
|
|
saveas(gcf, sprintf('output/Prob1b_lyapunov_gamma%.3f_%ds.png', gamma, T_total));
|
|
|
|
% Reconstruct estimated output x_hat(t)
|
|
x_hat = zeros(1, N);
|
|
dx_hat = zeros(1, N);
|
|
dx_hat(1) = 0;
|
|
for k = 1:N-1
|
|
m_hat = theta_hat(1,k);
|
|
b_hat = theta_hat(2,k);
|
|
k_hat = theta_hat(3,k);
|
|
ddx_hat = (u(k) - b_hat * dx_hat(k) - k_hat * x_hat(k)) / m_hat;
|
|
dx_hat(k+1) = dx_hat(k) + Ts * ddx_hat;
|
|
x_hat(k+1) = x_hat(k) + Ts * dx_hat(k);
|
|
end
|
|
e_x = x - x_hat;
|
|
|
|
% Plot extra figure with x, x_hat and e_x
|
|
figure('Name', 'System Response vs Estimation', 'Position', [100, 100, 1280, 860]);
|
|
sgtitle(sprintf('System Response and Error | Gamma = %.3f', gamma), 'FontWeight', 'bold');
|
|
|
|
subplot(2,1,1);
|
|
plot(t, x, 'b', t, x_hat, '--r', 'LineWidth', 1.5);
|
|
legend('x(t)', 'x_{hat}(t)', 'Location', 'Best');
|
|
ylabel('Θέση [m]');
|
|
grid on; title('Αντίδραση Συστήματος και Εκτίμηση');
|
|
|
|
subplot(2,1,2);
|
|
plot(t, e_x, 'k', 'LineWidth', 1.5);
|
|
ylabel('e_x(t)');
|
|
grid on; title('Σφάλμα θέσης: x(t) - x_{hat}(t)');
|
|
|
|
|
|
|
|
saveas(gcf, sprintf('output/Prob1b_extrastates_gamma%.3f_%ds.png', gamma, T_total));
|
|
|