89 lines
1.9 KiB
Matlab
89 lines
1.9 KiB
Matlab
% === Problem 2b: Estimation using only q(t) and u(t) ===
|
|
|
|
% True parameter values
|
|
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
|
|
data = readtable('output/problem1_data.csv');
|
|
t = data.t;
|
|
q = data.q;
|
|
u = data.u;
|
|
|
|
Ts = t(2) - t(1);
|
|
N = length(t);
|
|
|
|
% Compute dq and ddq using central differences
|
|
dq = zeros(N, 1);
|
|
ddq = zeros(N, 1);
|
|
|
|
for k = 2:N-1
|
|
dq(k) = (q(k+1) - q(k-1)) / (2 * Ts);
|
|
ddq(k) = (q(k+1) - 2*q(k) + q(k-1)) / Ts^2;
|
|
end
|
|
|
|
% Build regression matrix and output vector
|
|
X = [ddq, dq, q];
|
|
y = u;
|
|
|
|
% Least Squares estimation
|
|
theta_hat = (X' * X) \ (X' * y);
|
|
|
|
% Parameter extraction
|
|
mL2_est = theta_hat(1);
|
|
c_est = theta_hat(2);
|
|
mgL_est = theta_hat(3);
|
|
|
|
% Reconstruct ddq_hat from estimated parameters
|
|
ddq_hat = (1 / mL2_est) * (u - c_est * dq - mgL_est * q);
|
|
|
|
% Integrate to get dq_hat and q_hat
|
|
dq_hat = zeros(N,1);
|
|
q_hat = zeros(N,1);
|
|
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 2b - LS Estimation from q only', '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 from q(t) only');
|
|
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;
|
|
|
|
% Save figure
|
|
saveas(gcf, 'output/Prob2b_20s_Ts0.1.png');
|
|
|
|
% Print results
|
|
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));
|