1
2
3
4
5
6
7
8
9
|
1
2
3
4
5
6
7
8
9
|
-
-
+
+
|
function [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,y_u,cond_uu] = ppp_lin(A,B,C,D,A_u,A_w,tau,Q,max_cond);
## usage: [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,y_u,cond_uu] = ppp_lin(A,B,C,D,A_u,A_w,tau,Q,max_cond)
function [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,y_u,cond_uu] = ppp_lin(A,B,C,D,A_u,A_w,tau,Q,R,P,max_cond);
## usage: [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,y_u,cond_uu] = ppp_lin(A,B,C,D,A_u,A_w,tau,Q,R,P,max_cond)
##
## Linear PPP (Predictive pole-placement) computation
## INPUTS:
## A,B,C,D: system matrices
## A_u: composite system matrix for U* generation
## one square matrix (A_ui) row for each system input
## each A_ui generates U*' for ith system input.
|
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
|
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
|
-
+
-
+
+
+
+
+
+
+
+
+
+
+
|
## Check some dimensions
[n_x,n_u,n_y] = abcddim(A,B,C,D);
if (n_x==-1)
return
endif
## Default Q
## Default Q (output weight)
if nargin<8
Q = ones(n_y,1);
endif
## Default order
## Default R (input weight)
if nargin<9
R = zeros(n_u,1);
endif
## Default P (terminal weight)
if nargin<10
P = zeros(n_x,n_x);
endif
## Default condittion number
if nargin<11
max_cond = 1e20;
endif
[n_U,m_U] = size(A_u);
if (n_U != n_u*m_U)&&(n_U != m_U)
error("A_u must be square or have N_u rows and N_u/n_u columns");
endif
|
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
|
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
111
112
113
114
115
116
117
118
119
|
+
+
-
+
-
+
+
+
|
endif
[n_t,m_t] = size(tau);
if n_t != 1
error("tau must be a row vector");
endif
dt = tau(2) - tau(1); # Sample interval
[n_Q,m_Q] = size(Q);
if ((m_Q != 1)&&(m_Q != m_t)) || (n_Q != n_y)
error("Q must be a column vector with one row per system output");
endif
if (m_Q == 1) # Convert to vector Q(i)
Q = Q*ones(1,m_t); # Extend to cover full range of tau
endif
##Set up initial states
u_0 = ones(m_U,1);
w_0 = ones(m_W,1);
## Find y_U - derivative of y* wrt U.
i_U = 0;
x_0 = zeros(n_x,1); # This is for x=0
y_u = []; # Initialise
x_u_t = []; # Initialise
Us = []; # Initialise
for i=1:n_U # Do for each input function U*_i
dU = zeros(n_U,1);
dU(++i_U) = 1; # Create dU/dU_i
[ys,us] = ppp_ystar (A,B,C,D,x_0,A_u,dU,tau); # Find ystar and ustar
[ys,us,xs] = ppp_ystar (A,B,C,D,x_0,A_u,dU,tau); # Find ystar and ustar
y_u = [y_u ys']; # Save y_u (y for input u) with one row for each t.
Us = [Us us']; # Save u (input) with one row for each t.
Us = [Us us']; # Save u (input) with one row for each
# t.
x_u_t = [x_u_t xs(:,m_t)]; # x_u at terminal time
endfor
Ws = []; # Initialise
if n_W>0
## Find w*
i_W = 0;
x_0 = zeros(n_x,1); # This is for x=0
|
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
|
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
|
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
|
## Add up cost derivatives for each output but weighted by Q.
## Scaled by time interval
## y_u,y_x and Ws should really be 3D matrices, but instead are stored
## with one row for each time and one vector (size n_y) column for
## each element of U
## Scale Q
Q = Q/m_t; # Scale by T/dt = number of points
Q = Q*dt; # Scale to give correct units
## Non-w bits
for i = 1:n_y # For each output
QQ = ones(n_U,1)*Q(i,:); # Resize Q
J_uu = J_uu + (QQ .* y_u(:,i:n_y:n_yu)') * y_u(:,i:n_y:n_yu);
J_ux = J_ux + (QQ .* y_u(:,i:n_y:n_yu)') * y_x(:,i:n_y:n_yx);
QQ = ones(n_x,1)*Q(i,:); # Resize Q
J_xx = J_xx + (QQ .* y_x(:,i:n_y:n_yx)') * y_x(:,i:n_y:n_yx);
endfor
## Input weighting (scalar for the moment)
if (n_u>1)
warning("Sorry, cant do n_u>1 just now");
endif
## Scale R
R = R*dt; # Scale to give correct units
for i = 1:m_t
Ust = Us(i,:);
J_uu = J_uu + Ust'*R*Ust;
endfor
## Exit if badly conditioned
cond_uu = cond(J_uu);
if cond_uu>max_cond
error(sprintf("J_uu is badly conditioned. Condition number = 10^%i",log10(cond_uu)));
endif
## Terminal constraint
tau_last = tau(m_t);
x_x_t = expm(A*tau_last); # deriv. of x*(tau_2) wrt x(0)
J_ux = J_ux + x_u_t'*P*x_x_t;
J_uu = J_uu + x_u_t'*P*x_u_t;
## w bits
if n_W>0
for i = 1:n_y # For each output
QQ = ones(n_U,1)*Q(i,:); # Resize Q
J_uw = J_uw + (QQ .* y_u(:,i:n_y:n_yu)') * Ws (:,i:n_y:n_yw);
QQ = ones(n_x,1)*Q(i,:); # Resize Q
J_xw = J_xw + (QQ .* y_x(:,i:n_y:n_yx)') * Ws (:,i:n_y:n_yw);
|