94 lines
2.3 KiB
Matlab
94 lines
2.3 KiB
Matlab
%
|
|
% 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');
|