80 lines
1.8 KiB
Matlab
80 lines
1.8 KiB
Matlab
% === Problem 2: Parameter Estimation using Least Squares ===
|
|
|
|
% True data for comparison
|
|
m = 0.75;
|
|
L = 1.25;
|
|
c_true = 0.15;
|
|
g = 9.81;
|
|
|
|
mL2_true = m * L^2;
|
|
mgL_true = m * g * L;
|
|
|
|
theta_true = [mL2_true; c_true; mgL_true];
|
|
|
|
% Load sampled data from Problem 1
|
|
data = readtable('output/problem1_data.csv');
|
|
t = data.t;
|
|
q = data.q;
|
|
dq = data.dq;
|
|
ddq = data.ddq;
|
|
u = data.u;
|
|
|
|
% Build regression matrix X and target vector y = u
|
|
X = [ddq, dq, q]; % columns correspond to coefficients of [mL^2, c, mgL]
|
|
y = u;
|
|
|
|
% Least Squares estimation: theta_hat = [mL^2; c; mgL]
|
|
theta_hat = (X' * X) \ (X' * y);
|
|
|
|
% Extract individual parameters (optional interpretation)
|
|
mL2_est = theta_hat(1);
|
|
c_est = theta_hat(2);
|
|
mgL_est = theta_hat(3);
|
|
|
|
% Reconstruct ddq_hat using the estimated parameters
|
|
ddq_hat = (1 / mL2_est) * (u - c_est * dq - mgL_est * q);
|
|
|
|
% Numerical integration to recover dq̂(t) and q̂(t)
|
|
Ts = t(2) - t(1);
|
|
N = length(t);
|
|
dq_hat = zeros(N,1);
|
|
q_hat = zeros(N,1);
|
|
|
|
% Initial conditions
|
|
dq_hat(1) = dq(1);
|
|
q_hat(1) = q(1);
|
|
|
|
for k = 2:N
|
|
dq_hat(k) = dq_hat(k-1) + Ts * ddq_hat(k-1);
|
|
q_hat(k) = q_hat(k-1) + Ts * dq_hat(k-1);
|
|
end
|
|
|
|
% Estimation error
|
|
e_q = q - q_hat;
|
|
|
|
% === Plots ===
|
|
figure('Name', 'Problem 2 - LS Estimation', 'Position', [100, 100, 1280, 800]);
|
|
|
|
subplot(3,1,1);
|
|
plot(t, q, 'b', t, q_hat, 'r--');
|
|
legend('q(t)', 'q̂(t)');
|
|
title('Actual vs Estimated Angle');
|
|
ylabel('Angle [rad]');
|
|
grid on;
|
|
|
|
subplot(3,1,2);
|
|
plot(t, e_q, 'k');
|
|
title('Estimation Error e_q(t) = q(t) - q̂(t)');
|
|
ylabel('Error [rad]');
|
|
grid on;
|
|
|
|
subplot(3,1,3);
|
|
bar(["mL^2", "c", "mgL"], theta_hat);
|
|
title('Estimated Parameters');
|
|
ylabel('Value');
|
|
grid on;
|
|
saveas(gcf, 'output/Prob2_20s_Ts0.1.png');
|
|
|
|
fprintf(' Actual Parameters: mL^2=%f, c=%f, mgL=%f\n', theta_true(1), theta_true(2), theta_true(3));
|
|
fprintf('Estimated Parameters: mL^2=%f, c=%f, mgL=%f\n', theta_hat(1), theta_hat(2), theta_hat(3));
|