Lab02: WIP

Lab02/scripts/Problem1a.m

@@ -0,0 +1,121 @@
+% Gradient estimation for mass-spring-damper system (Exercise 1a)
+
+% True system parameters
+m_true = 1.315;
+b_true = 0.225;
+k_true = 0.725;
+
+% Simulation parameters
+Ts = 0.001;
+T_total = 30;
+t = 0:Ts:T_total;
+N = length(t);
+
+% Gamma setup
+gamma = 0.33;
+use_normalization = true;  % Set to false to disable normalization
+
+fprintf('Using gamma = %.4f\n', gamma);
+if use_normalization
+    fprintf('Using normalized gradient update.\n');
+else
+    fprintf('Using unnormalized gradient update.\n');
+end
+
+% Define both input cases
+input_cases = {...
+    struct('name', 'constant', 'u', 2.5 * ones(1, N)), ...
+    struct('name', 'sine', 'u', 2.5 * sin(t)) ...
+};
+
+fprintf('True m: %.4f, b: %.4f, k: %.4f\n', m_true, b_true, k_true);
+
+for case_idx = 1:length(input_cases)
+    input_case = input_cases{case_idx};
+    u = input_case.u;
+
+    % Preallocate state variables
+    x = zeros(1, N);
+    dx = zeros(1, N);
+    ddx = zeros(1, N);
+
+    % Initial conditions
+    x(1) = 0;
+    dx(1) = 0;
+
+    % Simulate true system using RK4
+    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;  % store first derivative (for later reuse)
+        dx(k+1) = dx(k) + Ts/6 * (k1 + 2*k2 + 2*k3 + k4);
+        x(k+1) = x(k) + Ts * dx(k);
+    end
+
+    % Approximate acceleration using finite differences
+    ddx(1:end-1) = diff(dx) / Ts;
+    ddx(end) = ddx(end-1);  % replicate last value
+
+    % Gradient estimation setup
+    theta_hat = zeros(3, N);            % rows: [m; b; k]
+    theta_hat(:, 1) = [1; 1; 1];        % initial guesses
+
+    % Run gradient estimator
+    for k = 1:N-1
+        u_vec = [ddx(k); dx(k); x(k)];
+        y = u(k);
+        y_hat = theta_hat(:, k)' * u_vec;
+        e = y - y_hat;
+        if use_normalization
+            norm_factor = 1 + norm(u_vec)^2;
+            theta_hat(:, k+1) = theta_hat(:, k) + Ts * gamma * (e / norm_factor) * u_vec;
+        else
+            theta_hat(:, k+1) = theta_hat(:, k) + Ts * gamma * e * u_vec;
+        end
+    end
+
+    % Print final estimated values to console
+    fprintf('\nFinal estimates for input: %s\n', input_case.name);
+    fprintf('Estimated m: %.4f, b: %.4f, k: %.4f\n', theta_hat(1,end), theta_hat(2,end), theta_hat(3,end));
+
+    % Plot estimated parameters
+    figure('Name', ['Estimated Parameters - ' input_case.name], 'Position', [100, 100, 1280, 860]);
+    normalization_text = ternary(use_normalization, 'Normalized', 'Unnormalized');
+    sgtitle(sprintf('Input: %s   |   Gamma = %.3f   |   %s', input_case.name, gamma, normalization_text), '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('Εκτίμηση ελαστικότητας');
+
+    % Save figure
+    if ~exist('output', 'dir')
+        mkdir('output');
+    end
+    saveas(gcf, sprintf('output/Prob1a_estimation_%s_gamma%.3f_%s_%ds.png', input_case.name, gamma, normalization_text, T_total));
+end
+
+function out = ternary(cond, val_true, val_false)
+    if cond
+        out = val_true;
+    else
+        out = val_false;
+    end
+end

Lab02/scripts/Problem1b.m

@@ -0,0 +1,116 @@
+%
+% Problem 1b: Lyapunov-based Parameter Estimation
+%
+
+
+% 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));
+

Lab02/scripts/Problem1c.m

@@ -0,0 +1,102 @@
+%
+% Problem 1c: Effect of bounded sinusoidal disturbance on measurement x(t)
+%
+
+% 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));

Lab02/scripts/Problem2a.m

@@ -0,0 +1,93 @@
+%
+% Problem 2a: Nonlinear system roll model parameter estimation without disturbance
+%
+
+% True system parameters
+a1 = 2.0;
+a2 = 1.0;
+a3 = 0.5;
+b = 2.0;
+
+% Simulation setup
+Ts = 0.001;
+T_total = 30;
+t = 0:Ts:T_total;
+N = length(t);
+
+% Reference trajectory: step profile
+r_d = zeros(1, N);
+r_d(t >= 10 & t < 20) = pi/10;
+
+% Smooth bound phi(t)
+phi0 = 1.5;
+phi_inf = 0.05;
+lambda = 0.5;
+phi = (phi0 - phi_inf) * exp(-lambda * t) + phi_inf;
+
+% Control parameters
+k1 = 1.0;
+k2 = 1.0;
+rho = 1.0;
+
+% Initial conditions
+r = zeros(1, N);
+dr = zeros(1, N);
+ddr = zeros(1, N);
+
+% Parameter estimation setup
+theta_hat = zeros(4, N);
+theta_hat(:,1) = [1; 1; 1; 1];
+gamma = 0.66;
+
+% Output storage for control input and errors
+alpha = zeros(1, N);
+u = zeros(1, N);
+
+for k = 1:N-1
+    % Compute normalized errors
+    z1 = (r(k) - r_d(k)) / phi(k);
+    z1 = max(min(z1, 0.999), -0.999);
+    alpha(k) = -k1 * log((1 + z1) / (1 - z1));
+
+    z2 = (dr(k) - alpha(k)) / rho;
+    z2 = max(min(z2, 0.999), -0.999);
+    u(k) = -k2 * log((1 + z2) / (1 - z2));
+
+    % True system dynamics
+    phi_true = [-dr(k); -sin(r(k)); dr(k)^2 * sin(2*r(k)); u(k)];
+    ddr(k) = a1 * phi_true(1) + a2 * phi_true(2) + a3 * phi_true(3) + b * phi_true(4);
+
+    % Integrate dynamics
+    dr(k+1) = dr(k) + Ts * ddr(k);
+    r(k+1) = r(k) + Ts * dr(k);
+
+    % Estimation
+    phi_est = phi_true; % same form
+    y = ddr(k);
+    y_hat = theta_hat(:,k)' * phi_est;
+    e = y - y_hat;
+    theta_hat(:,k+1) = theta_hat(:,k) + Ts * gamma * e * phi_est;
+end
+
+% Final estimates
+fprintf('\n2a: Final estimated parameters:\n');
+fprintf('a1: %.4f, a2: %.4f, a3: %.4f, b: %.4f\n', theta_hat(1,end), theta_hat(2,end), theta_hat(3,end), theta_hat(4,end));
+
+% Plot parameter estimates
+figure('Name', 'Problem 2a - Parameter Estimation', 'Position', [100, 100, 1280, 860]);
+sgtitle('Nonlinear Roll System - Parameter Estimation');
+
+subplot(2,1,1);
+plot(t, theta_hat', 'LineWidth', 1.4);
+legend('a_1', 'a_2', 'a_3', 'b');
+ylabel('\theta estimates'); grid on; title('Εκτιμήσεις παραμέτρων');
+
+subplot(2,1,2);
+plot(t, r, 'b', t, r_d, '--r', 'LineWidth', 1.4);
+legend('r(t)', 'r_d(t)');
+ylabel('Roll angle [rad]'); xlabel('Time [s]'); grid on; title('Παρακολούθηση τροχιάς');
+
+if ~exist('output', 'dir')
+    mkdir('output');
+end
+saveas(gcf, 'output/Problem2a_estimation.png');

Lab02/scripts/Problem2b.m

@@ -0,0 +1,103 @@
+%
+% Problem 2b: Estimation of unknown parameters using Lyapunov (r, dr, u measurable)
+%
+
+% True system parameters
+a1 = 2.0;
+a2 = 1.0;
+a3 = 0.5;
+b = 2.0;
+
+% Simulation setup
+Ts = 0.001;
+T_total = 30;
+t = 0:Ts:T_total;
+N = length(t);
+
+% Reference trajectory
+r_d = zeros(1, N);
+r_d(t >= 10 & t < 20) = pi/10;
+
+% Smooth bound phi(t)
+phi0 = 1.5;
+phi_inf = 0.05;
+lambda = 0.5;
+phi = (phi0 - phi_inf) * exp(-lambda * t) + phi_inf;
+
+% Control parameters
+k1 = 1.0;
+k2 = 1.0;
+rho = 1.0;
+
+% Initial conditions
+r = zeros(1, N);
+dr = zeros(1, N);
+ddr = zeros(1, N);
+
+% Estimated trajectory reconstruction
+r_hat = zeros(1, N);
+dr_hat = zeros(1, N);
+dr_hat(1) = 0; r_hat(1) = 0;
+
+% Parameter estimation setup
+theta_hat = zeros(4, N);
+theta_hat(:,1) = [1; 1; 1; 1];
+gamma = 0.66;
+
+alpha = zeros(1, N);
+u = zeros(1, N);
+
+for k = 1:N-1
+    % Control law
+    z1 = (r(k) - r_d(k)) / phi(k);
+    z1 = max(min(z1, 0.999), -0.999);
+    alpha(k) = -k1 * log((1 + z1) / (1 - z1));
+
+    z2 = (dr(k) - alpha(k)) / rho;
+    z2 = max(min(z2, 0.999), -0.999);
+    u(k) = -k2 * log((1 + z2) / (1 - z2));
+
+    % System dynamics
+    phi_vec = [-dr(k); -sin(r(k)); dr(k)^2 * sin(2*r(k)); u(k)];
+    ddr(k) = a1 * phi_vec(1) + a2 * phi_vec(2) + a3 * phi_vec(3) + b * phi_vec(4);
+    dr(k+1) = dr(k) + Ts * ddr(k);
+    r(k+1) = r(k) + Ts * dr(k);
+
+    % Parameter estimation
+    y = ddr(k);
+    y_hat = theta_hat(:,k)' * phi_vec;
+    e = y - y_hat;
+    theta_hat(:,k+1) = theta_hat(:,k) + Ts * gamma * e * phi_vec;
+
+    % Reconstruct r_hat from estimated theta
+    phi_hat = [-dr_hat(k); -sin(r_hat(k)); dr_hat(k)^2 * sin(2 * r_hat(k)); u(k)];
+    dd_r_hat = theta_hat(:,k)' * phi_hat;
+    dr_hat(k+1) = dr_hat(k) + Ts * dd_r_hat;
+    r_hat(k+1) = r_hat(k) + Ts * dr_hat(k);
+end
+
+fprintf('\n2b: Final estimated parameters:\n');
+fprintf('a1: %.4f, a2: %.4f, a3: %.4f, b: %.4f\n', theta_hat(1,end), theta_hat(2,end), theta_hat(3,end), theta_hat(4,end));
+
+% Plot
+figure('Name', 'Problem 2b - Estimation with State Comparison', 'Position', [100, 100, 1280, 860]);
+sgtitle('Problem 2b - Parameter Estimation and State Reconstruction');
+
+subplot(3,1,1);
+plot(t, theta_hat', 'LineWidth', 1.4);
+legend('a_1', 'a_2', 'a_3', 'b');
+ylabel('\theta estimates'); grid on; title('Εκτιμήσεις παραμέτρων');
+
+subplot(3,1,2);
+plot(t, r, 'b', t, r_hat, '--r', 'LineWidth', 1.4);
+legend('r(t)', 'r_{hat}(t)');
+ylabel('Roll angle [rad]'); title('Πραγματικό vs εκτιμώμενο r(t)'); grid on;
+
+subplot(3,1,3);
+plot(t, r - r_hat, 'k', 'LineWidth', 1.4);
+ylabel('e_r(t)'); xlabel('Time [s]'); title('Σφάλμα θέσης: r(t) - r̂(t)'); grid on;
+
+if ~exist('output', 'dir')
+    mkdir('output');
+end
+saveas(gcf, 'output/Problem2b_estimation.png');