17
18
19
20
21
22
23
24
25
26
27
28
29
30
|
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
|
+
+
+
+
+
|
##Defaults
if nargin<1 # Default name to dir name
names = split(pwd,"/");
[n_name,m_name] = size(names);
Name = deblank(names(n_name,:));
endif
## System
sys = mtt2sys(Name); # Create system
[A,B,C,D] = sys2ss(sys); # SS form
[n_x, n_u, n_y] = abcddim(A,B,C,D); # Dimensions
if nargin<2
Simulate = 1;
endif
if nargin<3
ControlType = 2;
|
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
|
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
|
+
-
-
-
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
|
p_c.N = 10;
endif
if nargin<6
p_o.sigma = 0.001;
endif
if !struct_contains(p_c,"N")
p_c.N = 10; # Number of small samples per large sample
endif
# if !struct_contains(p_c,"N")
# p_c.N = 10; # Number of small samples per large sample
# endif
if !struct_contains(p_c,"delta_ol")
p_c.delta_ol = 1.0; # OL sample interval
endif
if !struct_contains(p_c,"T")
p_c.T = 5.0; # Last time point.
endif
if !struct_contains(p_c,"A_w")
p_c.A_w = 0;
endif
if !struct_contains(p_c,"A_u")
p_c.N_u = 3;
a_u = 2.0;
p_c.A_u = ppp_aug(p_c.A_w,laguerre_matrix(p_c.N_u-1,a_u));
endif
if !struct_contains(p_c,"Method")
p_c.Method = "lq";
p_c.Q = eye(n_y);
p_c.R = (0.1^2)*eye(n_u);
p_c.N_u = n_x;
endif
[p_c.N_u,M_u] = size(p_c.A_u);
if (p_c.N_u<>M_u)
error("A_u must be square");
endif
## System
sys = mtt2sys(Name); # Create system
[A,B,C,D] = sys2ss(sys); # SS form
[n_x, n_u, n_y] = abcddim(A,B,C,D)
ol_poles = eig(A)
|
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
|
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
|
+
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
+
+
+
+
|
K_w = zeros(p_c.N_u,n_y);
K_w(1,1) = 1;
K_w(2,1) = -1;
K_x = zeros(p_c.N_u,n_x);
U = K_w*w; # Initial control U
else # PPP control
I = ceil(p_c.T/p_c.delta_ol); # Number of large samples
if strcmp(p_c.Method, "original")
tau = [10:0.1:11]*(2/a_u); # Time horizons
[k_x,k_w,K_x,K_w] = ppp_lin(A,B,C,D,p_c.A_u,p_c.A_w,tau); # Design
U = K_w*w # Initial control U
tau = [10:0.1:11]*(2/a_u); # Time horizons
[k_x,k_w,K_x,K_w] = ppp_lin(A,B,C,D,p_c.A_u,p_c.A_w,tau); # Design
elseif strcmp(p_c.Method, "lq")
tau = [0:0.001:1.0]*5; # Time horizons
[k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,y_u,p_c.A_u] \
= ppp_lin_quad (A,B,C,D,tau,p_c.Q,p_c.R);
else
error(sprintf("Method %s not recognised", p_c.Method));
endif
U = K_w*w; # Initial control U
## Checks
[ol_zeros, ol_poles] = sys2zp(sys)
cl_poles = eig(A - B*k_x)
endif
## Sample times
dt = 0.1;
delta = p_c.N*dt;
Ad = expm(A*delta); # Discrete-time transition matrix
Ad = expm(A*p_c.delta_ol); # Discrete-time transition matrix
if (ControlType==2)
G = eye(n_x); # State noise gain
sigma_x = eye(n_x); # State noise variance
Sigma = p_o.sigma*eye(n_y); # Measurement noise variance
L = dlqe(Ad,G,C,sigma_x,Sigma)
else
L = zeros(n_x,n_y);
endif
obs_poles = eig(Ad-L*C)
## Short sample interval
dt = p_c.delta_ol/p_c.N;
## Control loop
y = [];
u = [];
t = [];
y_e = [];
t_e = [];
|
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
|
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
|
-
+
|
data = from_rt(p_c.N); # Receive data
[yi,ui] = convert_data(data);
y_now = yi(:,p_c.N); # Current output
endif
## Zero-gain (OL) observer with state resetting
[x_est y_est] = ppp_int_obs (x_est,y_now,U,A,B,C,D,p_c.A_u,delta,L);
[x_est y_est] = ppp_int_obs (x_est,y_now,U,A,B,C,D,p_c.A_u,p_c.delta_ol,L);
# ## Reset states
# x_est(2) = y_now(1); # Position
# x_est(4) = y_now(2)/g_s; # Angle
##Control
U = K_w*w- K_x*x_est;
|