104 lines
2.7 KiB
Matlab
104 lines
2.7 KiB
Matlab
%
|
|
% 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');
|