-
Notifications
You must be signed in to change notification settings - Fork 0
/
controller_ilc_mf.m
110 lines (98 loc) · 3.46 KB
/
controller_ilc_mf.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
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
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 = 200;
alpha = 1; % initialize step size
e_norm = zeros(num_iterations, 1);
% initialize G_star_e for the first time step
G_star_e_prev = 0;
tau = fliplr(eye(200));
% ILC Iteration
for i = 1:num_iterations
% get currrent output position
position = actual_dynamics(U, G_actual, d_actual);
% Calculate the current error
pos_error = Traj_ref - position;
e_norm(i) = norm(pos_error);
tau_e = tau*pos_error;
% Calculate the adjoint G*e
G_star_e = tau * actual_dynamics(tau_e, G_actual, d_actual);
% Calculate Delta h_i and Delta V_i
Delta_h_i = G_star_e - G_star_e_prev;
Delta_u_i = U - u_prev;
% Get step size for x
if Delta_h_i' * Delta_u_i >= 0
alpha = 0.01;
else
alpha = -Delta_h_i' * Delta_u_i / ((Delta_h_i)' * Delta_h_i);
end
u_prev = U;
% Update u and G*e
U = U + alpha * G_star_e;
G_star_e_prev = G_star_e;
end
%% 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-mf')
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-mf')
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-MF')
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-MF')
set(gca,"fontsize", 12, 'FontWeight', 'bold')
%%%%%%%%%%%%% Function definition %%%%%%%%%%%%%%%%%
function trajectory = actual_dynamics(U, G_actual, d_actual)
trajectory = G_actual * U + d_actual;
end