-
Notifications
You must be signed in to change notification settings - Fork 0
/
controller_ilc_qp.m
95 lines (89 loc) · 3.24 KB
/
controller_ilc_qp.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
close all; clear; clc
load initial_control_input.mat
load reference_trajectory.mat
load ilc_matrix_actual.mat
load ilc_matrix_nominal.mat
%% Construct necessary matrices for ILC iteration
ux = ux_initial; uy = uy_initial;
ux = ux(1:end-1);
uy = uy(1:end-1);
index_x = 1:2:199;
index_y = 2:2:200;
x_ref = x_ref(2:end);
y_ref = y_ref(2:end);
Traj_ref = zeros(200,1); % X = [x0, y0, x1, y1, ...]
U = zeros(200,1); % U = [ux0, uy0, ux1, uy1, ...]
for i = 1:100
Traj_ref(2*i-1) = x_ref(i);
Traj_ref(2*i) = y_ref(i);
U(2*i-1) = ux(i);
U(2*i) = uy(i);
end
time = 0.1:0.1:10;
N = 101; % Number of discretization points
%% Plot Result of Inital Control Input Computed Using Direct Collocation
u_prev = 0;
position = G_actual*U + d_actual;
subplot(2,1,1)
plot(time, x_ref, '-', 'LineWidth',2); hold on;
plot(time, position(index_x), '--', 'LineWidth',2)
xlabel('Time'); ylabel('Position X');legend('x\_ref', 'x\_dc')
set(gca,"fontsize", 12, 'FontWeight', 'bold')
subplot(2,1,2)
plot(time, y_ref, '-', 'LineWidth',2); hold on;
plot(time, position(index_y), '--', 'LineWidth',2)
xlabel('Time'); ylabel('Position Y');legend('y\_ref', 'y\_dc')
set(gca,"fontsize", 12, 'FontWeight', 'bold')
figure
plot(Traj_ref(index_x), Traj_ref(index_y), '-', 'LineWidth',2); hold on
plot(position(index_x), position(index_y), '-.', 'LineWidth',2)
xlabel('X'); ylabel('Y');legend('Reference', 'Direct Collocation'); axis equal
set(gca,"fontsize", 12, 'FontWeight', 'bold')
%% ILC Loop
num_iterations = 0;
e_norm = [];
% Initialize matrices for QP
delta_u = zeros(200,1);
H = eye(200); % J = 1/2*[delta_ux;delta_uy]'*H*[delta_ux;delta_uy]
Aeq = G_nominal;
beq = zeros(200,1);
tracking_error = 1;
% ILC Iteration
while tracking_error > 0.01
% Get currrent output position using actual model
position = G_actual*U + d_actual;
% Calculate the error of current iteration
pos_error = Traj_ref - position;
tracking_error = norm(pos_error);
e_norm(end+1) = tracking_error;
beq = pos_error;
% Calculate optimal change of U (G_nomial*[delta_ux;delta_uy] = [error_x;error_y] )
[x,fval,exitflag,output,lambda] = quadprog(H,[],[],[],Aeq,beq,[],[]);
delta_u = x;
% Update control input for next iteration
U = U+delta_u;
num_iterations = num_iterations + 1;
end
fprintf('number of iterations to converge: %d\n', num_iterations)
%% Plot the results
figure % Plot 1d trajectory vs time
subplot(2,1,1)
plot(time, x_ref, '-', 'LineWidth',2); hold on;
plot(time, position(index_x), '--', 'LineWidth',2)
legend('x\_ref', 'x\_ilc-qp')
set(gca,"fontsize", 12, 'FontWeight', 'bold')
subplot(2,1,2)
plot(time, y_ref, '-', 'LineWidth',2); hold on;
plot(time, position(index_y), '--', 'LineWidth',2)
legend('y\_ref', 'y\_ilc-qp')
set(gca,"fontsize", 12, 'FontWeight', 'bold')
figure % Plot 2d trajectory
plot(x_ref, y_ref, '-', 'LineWidth',2); hold on
plot(position(index_x), position(index_y), '-.', 'LineWidth',2)
legend('Reference', 'ILC-QP')
xlabel('X'); ylabel('Y'); axis equal
set(gca,"fontsize", 12, 'FontWeight', 'bold')
figure % Plot error vs iteration
plot(1:num_iterations, e_norm, '-x','LineWidth',2)
xlabel('Iteration'); ylabel('Error\_ILC-QP')
set(gca,"fontsize", 12, 'FontWeight', 'bold')