Commit 0a78d905 authored by Ferdinand Fischer's avatar Ferdinand Fischer
Browse files

Multiple implementations for the transition planning and changes due to new quantity version

parent 75ffd182
...@@ -30,17 +30,16 @@ function [u, y, x] = planPolynomialTrajectory(sys, t, optArgs) ...@@ -30,17 +30,16 @@ function [u, y, x] = planPolynomialTrajectory(sys, t, optArgs)
% T: the time for which the transition should occur % T: the time for which the transition should occur
arguments arguments
sys ss; sys ss;
t double; t quantity.Domain;
optArgs.x0 double = zeros(size(sys.A, 1), 1); optArgs.x0 double = zeros(size(sys.A, 1), 1);
optArgs.x1 double = zeros(size(sys.A, 1), 1); optArgs.x1 double = zeros(size(sys.A, 1), 1);
optArgs.domainName string = "t";
end end
x0 = optArgs.x0; x0 = optArgs.x0;
x1 = optArgs.x1; x1 = optArgs.x1;
t0 = t(1); t0 = t.lower;
t1 = t(end); t1 = t.upper;
%% Definitions %% Definitions
A = sys.A; A = sys.A;
...@@ -55,7 +54,7 @@ n_u = size(B,2); ...@@ -55,7 +54,7 @@ n_u = size(B,2);
d = sym('d',[n_u,(2*n)]); d = sym('d',[n_u,(2*n)]);
% variables % variables
s = sym('s'); s = sym('s');
tau = sym( optArgs.domainName ); t_sym = sym( t.name );
%% Computation of the PI matrix %% Computation of the PI matrix
% The PI matrix is a matrix that contains each coefficient of the PI_s % The PI matrix is a matrix that contains each coefficient of the PI_s
% matrix, the matrix PI_s is given as PI_s = adj(sI - A)*B % matrix, the matrix PI_s is given as PI_s = adj(sI - A)*B
...@@ -79,7 +78,7 @@ for i = 1:n_u ...@@ -79,7 +78,7 @@ for i = 1:n_u
temp = size(coef,2); temp = size(coef,2);
PI_i(j,1:size(coef,2)) = coef; PI_i(j,1:size(coef,2)) = coef;
end end
y0(i) = poly2sym(d(i,:),tau); y0(i) = poly2sym(d(i,:),t_sym);
PI(i) = {PI_i}; PI(i) = {PI_i};
end end
PI = cell2mat(PI); PI = cell2mat(PI);
...@@ -102,20 +101,20 @@ c1 = pinv(PI,1e-12)*x1; ...@@ -102,20 +101,20 @@ c1 = pinv(PI,1e-12)*x1;
Y = sym('y',[n,n_u]); Y = sym('y',[n,n_u]);
Y(1,:) = y0; Y(1,:) = y0;
for j = 1:(n-1) for j = 1:(n-1)
Y(j+1,:) = diff(y0,tau,j); Y(j+1,:) = diff(y0,t_sym,j);
end end
% The actual computation of the coefficients d done for each element of % The actual computation of the coefficients d done for each element of
% y_schlange. % y_schlange.
i_temp = 0; i_temp = 0;
for i = 1:n_u for i = 1:n_u
EQN1 = subs(Y(:,i),tau,t0) == c0((i_temp + 1):(i_temp + n)); EQN1 = subs(Y(:,i),t_sym,t0) == c0((i_temp + 1):(i_temp + n));
EQN1 = EQN1.'; EQN1 = EQN1.';
EQN2 = subs(Y(:,i),tau,t1) == c1((i_temp + 1):(i_temp + n)); EQN2 = subs(Y(:,i),t_sym,t1) == c1((i_temp + 1):(i_temp + n));
EQN2 = EQN2.'; EQN2 = EQN2.';
[Ai,Bi] = equationsToMatrix([EQN1, EQN2], d(i,:)); [Ai,Bi] = equationsToMatrix([EQN1, EQN2], d(i,:));
coefpoly = linsolve(Ai,Bi); coefpoly = linsolve(Ai,Bi);
y_schlange(i) = poly2sym(coefpoly,tau); y_schlange(i) = poly2sym(coefpoly,t_sym);
i_temp = i_temp + n; i_temp = i_temp + n;
end end
%% Computation of the input u, the states x and the output y = Cx %% Computation of the input u, the states x and the output y = Cx
...@@ -133,19 +132,18 @@ end ...@@ -133,19 +132,18 @@ end
Y_schlange = sym('y',[n,n_u]); Y_schlange = sym('y',[n,n_u]);
Y_schlange(1,:) = y_schlange; Y_schlange(1,:) = y_schlange;
for j = 1:(n-1) for j = 1:(n-1)
Y_schlange(j+1,:) = diff(y_schlange,tau,j); Y_schlange(j+1,:) = diff(y_schlange,t_sym,j);
end end
% The computation of the input. Y_schlange must be extended to have one % The computation of the input. Y_schlange must be extended to have one
% more row that containts the n-th derivative of y_schlange % more row that containts the n-th derivative of y_schlange
u = flip(charpoly(A))* [Y_schlange;diff(y_schlange,tau,n)]; u = flip(charpoly(A))* [Y_schlange;diff(y_schlange,t_sym,n)];
u = quantity.Symbolic( u.', 'grid', {t}, 'gridName', optArgs.domainName); u = quantity.Symbolic( u.', t);
% Y_schlange is now reshaped so that it corresponds to the size of the PI % Y_schlange is now reshaped so that it corresponds to the size of the PI
% matrix and the calculation of the states follows directly. % matrix and the calculation of the states follows directly.
Y_schlange = reshape(Y_schlange,n_u*n,1); Y_schlange = reshape(Y_schlange,n_u*n,1);
x = quantity.Symbolic( PI*Y_schlange, 'grid', {t}, 'gridName', optArgs.domainName); x = quantity.Symbolic( PI*Y_schlange, t);
y = quantity.Symbolic( sys.C*x.sym, ... y = quantity.Symbolic( sys.C*x.sym, t) + sys.D * u;
'grid', {t}, 'gridName', optArgs.domainName) + sys.D * u;
end end
function [u, y, x] = planTrajectory(sys, t, optArgs) function [u, y, x, W] = planTrajectory(sys, t, optArgs)
% PLANTRAJECTORY Computes a trajectory for a lumped-parameter systems % PLANTRAJECTORY Computes a trajectory for a lumped-parameter systems
% [u, y, x] = planTrajectory(sys, t, varargin) computes a transition % [u, y, x] = planTrajectory(sys, t, varargin) computes a transition
% u(t) : x(t0) = x0 --> x(t1) = x1 % u(t) : x(t0) = x0 --> x(t1) = x1
% for a state-space system sys of the form % for a state-space system sys of the form
% d/dt x = A x + bu. % d/dt x = A x + bu.
...@@ -15,40 +15,71 @@ function [u, y, x] = planTrajectory(sys, t, optArgs) ...@@ -15,40 +15,71 @@ function [u, y, x] = planTrajectory(sys, t, optArgs)
% %
arguments arguments
sys ss; sys ss;
t double; t quantity.Domain;
optArgs.x0 double = zeros(size(sys.A, 1), 1); optArgs.x0 double = zeros(size(sys.A, 1), 1);
optArgs.x1 double = zeros(size(sys.A, 1), 1); optArgs.x1 double = zeros(size(sys.A, 1), 1);
optArgs.domainName string = "t"; optArgs.w quantity.Discrete;
optArgs.method = "Chen3";
optArgs.weight = quantity.Symbolic.ones(1, t);
end end
x0 = optArgs.x0; x0 = optArgs.x0;
x1 = optArgs.x1; x1 = optArgs.x1;
% prepare time vectors if optArgs.method == "Bernstein"
t = t(:); t0 = t.lower;
t0 = t(1); t1 = t.upper;
t1 = t(end); % Compute the state transition matrix: Phi(t,tau) = expm(A*(t-tau) )
tDomain = quantity.Domain({optArgs.domainName}, t); Phi_t0 = expm(sys.A * (t.Discrete - t.lower));
invPhi_t1 = expm(sys.A * (t.upper - t.Discrete));
%% Compute the state transition matrix: Phi(t,tau) = expm(A*(t-tau) )
% Att0 = sys.A * quantity.Symbolic( sym('t') - sym('tau'), 'grid', {t, t}, 'gridName', {'t', 'tau'} ); % compute the gramian controllability matrix
Phi_t0 = expm(sys.A * quantity.Discrete( t - t0, tDomain)); %W1 = misc.ss.gramian(sys, t, t0, optArgs.domainName);
invPhi_t1 = expm(sys.A * quantity.Discrete( t1 - t, tDomain)); %int_t0^t1 expm(A*(tau-t0)) * b * b^T * expm(A^T(tau-t0)) dtau
W = cumInt( Phi_t0 * sys.b * sys.b' * expm(sys.A' * (t.Symbolic - t.lower)), t, t0, t.name);
W1_t1 = W.at(t1);
%Formel aus dem Bernstein:
u = sys.b.' * invPhi_t1.' / W1_t1 * (x1 - Phi_t0.at(t1) * x0);
%Berechnung der
x = Phi_t0 * x0 + W * invPhi_t1' / W1_t1 * (x1 - Phi_t0.at(t1) * x0);
end
%% compute the gramian controllability matrix %W2 = expm( sys.A * (t.Discrete - t1 ) ) * cumInt( invPhi_t1 * sys.B * sys.B' * invPhi_t1', t, t0, t.name);
% W1 = misc.ss.gramian(sys, t, t0, optArgs.domainName); %% Chen^3
if optArgs.method == "Chen3"
Phi_t_t0 = expm(sys.A * ( t.Symbolic - t.lower ));
Phi_t1_t = expm(sys.A * (t.upper - t.Symbolic ) );
Phi_t_t1 = expm(sys.A * (t.Symbolic - t.upper ) );
Wt0t = cumInt( Phi_t1_t * sys.B * sys.B' * Phi_t1_t' * optArgs.weight, t, t.lower, t.name );
Wt0t1 = Wt0t.at(t.upper);
u = - sys.B' * Phi_t1_t' / Wt0t1 * ( Phi_t_t0.at(t.upper) * x0 - x1 ) * optArgs.weight;
x = Phi_t_t0 * x0 - Phi_t_t1 * Wt0t / Wt0t1 * ( Phi_t_t0.at(t.upper) * x0 - x1 );
end
% int_t0^t1 expm(A*(tau-t0)) * b * b^T * expm(A^T(tau-t0)) dtau %% Chen^1
W1 = cumInt( Phi_t0 * sys.b * sys.b' * expm(sys.A' * quantity.Discrete( t - t0, tDomain)), ... if optArgs.method == "Chen1"
Phi_t0(1).domain, t0, optArgs.domainName); %% compute the gramian controllability matrix
Phi_t_t0 = expm(sys.A * ( t.Symbolic - t.lower ));
Phi_t0_t = expm(sys.A * ( t.lower - t.Symbolic)); % = inv( Phi_t_t0 )
% int Phi(t,t0) * b * b^T * Phi(t,t0)' dt
W = cumInt( Phi_t0_t * sys.b * sys.b' * Phi_t0_t' * optArgs.weight, t, t.lower, t.name);
% solution for the control (see Chen (5-15))
% u(t) = - B' * Phi'(t0, t) * W(t0, t1)^-1 * ( x0 - Phi(t0, t1) * x1)
u = - sys.b.' * Phi_t0_t.' / W.at(t.upper) * (x0 - Phi_t0_t.at(t.upper) * x1) * optArgs.weight;
% solution for the state:
% x(t) = Phi(t,t0) * ( x0 - W(t0, t) * W^-1(t0,t1) * (x0 - Phi(t0, t1*x1) );
x = Phi_t_t0 * ( x0 - W / W.at(t.upper) * (x0 - Phi_t0_t.at(t.upper) * x1) );
%
else
error("Method " + optArgs.method + " not yet implemented")
end
W1_t1 = W1.at(t1);
% Formel aus dem Bernstein:
u = sys.b.' * invPhi_t1.' / W1_t1 * (x1 - Phi_t0.at(t1) * x0);
% Berechnung der
x = Phi_t0 * x0 + W1 * invPhi_t1' / W1_t1 * (x1 - Phi_t0.at(t1) * x0);
%
y = sys.C * x + sys.D * u; y = sys.C * x + sys.D * u;
%% Alternative Lösung nach Chen: %% Alternative Lösung nach Chen:
...@@ -56,6 +87,6 @@ y = sys.C * x + sys.D * u; ...@@ -56,6 +87,6 @@ y = sys.C * x + sys.D * u;
% W0 = invPhi_t0 * W1 * invPhi_t0'; % W0 = invPhi_t0 * W1 * invPhi_t0';
% u1 = -sys.B' * invPhi_t0' / W0.at(t1) * (x0 - Phi_t1.at(t0) * x1); % u1 = -sys.B' * invPhi_t0' / W0.at(t1) * (x0 - Phi_t1.at(t0) * x1);
% xW0 = Phi_t0 * ( x0 - W0 / W0.at(t1) * (x0 - Phi_t1.at(t0) * x1)); % xW0 = Phi_t0 * ( x0 - W0 / W0.at(t1) * (x0 - Phi_t1.at(t0) * x1));
% %
% % simulation results have shown, does the average of both soultions leads to a better than each. % % simulation results have shown, does the average of both soultions leads to a better than each.
% u = (u0 + u1) / 2; % u = (u0 + u1) / 2;
...@@ -36,7 +36,7 @@ tc.verifyEqual(misc.ss.relativeDegree(mySs, "min"), 1); ...@@ -36,7 +36,7 @@ tc.verifyEqual(misc.ss.relativeDegree(mySs, "min"), 1);
tc.verifyEqual(misc.ss.relativeDegree(mySs, "max"), 2); tc.verifyEqual(misc.ss.relativeDegree(mySs, "max"), 2);
end % testRelativeDegree end % testRelativeDegree
function planPolynomialTrajectory(tc) function planPolynomialTrajectoryTest(tc)
%% %%
A = [-30 0; 0 -50]; A = [-30 0; 0 -50];
B = [6; -5]; B = [6; -5];
...@@ -44,55 +44,67 @@ function planPolynomialTrajectory(tc) ...@@ -44,55 +44,67 @@ function planPolynomialTrajectory(tc)
D = []; D = [];
S = ss(A, B, C, D); S = ss(A, B, C, D);
t = linspace(1, 2, 1e3)'; t = quantity.Domain("t", linspace(1, 2, 1e3)');
x0 = [10; 12]; x0 = [10; 12];
x1 = [-5; -5]; x1 = [-5; -5];
[trj.u, trj.y, trj.x] = misc.ss.planPolynomialTrajectory(S, t, 'x0', x0, 'x1', x1); [trj.u, trj.y, trj.x] = misc.ss.planPolynomialTrajectory(S, t, 'x0', x0, 'x1', x1);
[y, t, x] = lsim(S, trj.u.on(), t, x0);
subplot(3,1,1);
plot(t, trj.u.on());
subplot(3,1,2); [y, ~, x] = lsim(S, trj.u.on(), t.grid, x0);
plot(t, y - trj.y.on());
subplot(313); sim.y = quantity.Discrete(y, t);
plot(t, x - trj.x.on()); sim.x = quantity.Discrete(x, t);
disp(x(end,:))
tc.verifyEqual(trj.x.on(), x, 'AbsTol', 2e-2); tc.verifyEqual(trj.x.on(), x, 'AbsTol', 2e-2);
tc.verifyEqual(trj.y.on(), y, 'AbsTol', 1e-2); tc.verifyEqual(trj.y.on(), y, 'AbsTol', 1e-2);
end end
function testPlanTrajectory(tc) function testPlanTrajectoryWeighted(tc)
%% %%
A = [-30 0; 0 -50]; A = [3 0; 0 5];
B = [6; -5]; B = [6; -5];
C = [1, 1]; C = [1, 1];
D = []; D = [];
S = ss(A, B, C, D); S = ss(A, B, C, D);
t = linspace(0, 0.01, 1e2)'; I = quantity.Domain("t", linspace(0, .1, 1e2)');
x0 = [10; 12]; x0 = [10; 12];
x1 = [-5; -5]; x1 = [11; 11];
[trj.u, trj.y, trj.x] = misc.ss.planTrajectory(S, t, 'x0', x0, 'x1', x1);
[trj.u, trj.y, trj.x] = misc.ss.planTrajectory(S, I, 'x0', x0, 'x1', x1, "method", "Chen1");
[trj.sim.y, ~, trj.sim.x] = lsim(S, trj.u.on(), I.grid, x0);
trj.sim.x = quantity.Discrete( trj.sim.x, I);
trj.sim.y = quantity.Discrete( trj.sim.y, I);
[y, t, x] = lsim(S, trj.u.on(), t, x0); tc.verifyEqual(trj.x.on(), trj.sim.x.on(), 'AbsTol', 2e-2);
% subplot(3,1,1); tc.verifyEqual(trj.y.on(), trj.sim.y.on(), 'AbsTol', 1e-2);
% plot(t, trj.u.on());
%
% subplot(3,1,2);
% plot(t,y);
%
% subplot(313);
% plot(t, x);
% disp(x(end,:))
tc.verifyEqual(trj.x.on(), x, 'AbsTol', 2e-2);
tc.verifyEqual(trj.y.on(), y, 'AbsTol', 1e-2);
%% test the weighted trajectory planning
clear trj;
w = signals.GevreyFunction("timeDomain", I, "diffShift", 1).fun;
[trj.u, trj.y, trj.x] = misc.ss.planTrajectory(S, I, 'x0', x0, 'x1', x1, "weight", w);
[trj.sim.y, ~, trj.sim.x] = lsim(S, trj.u.on(), I.grid, x0);
trj.sim.x = quantity.Discrete( trj.sim.x, I);
trj.sim.y = quantity.Discrete( trj.sim.y, I);
tc.verifyEqual(trj.x.on(), trj.sim.x.on(), 'AbsTol', 6e-3);
tc.verifyEqual(trj.y.on(), trj.sim.y.on(), 'AbsTol', 5e-3);
%% test the weighted trajectory planning with Chen1 method
clear trj;
w = signals.GevreyFunction("timeDomain", I, "diffShift", 1).fun;
[trj.u, trj.y, trj.x] = misc.ss.planTrajectory(S, I, 'x0', x0, 'x1', x1, "weight", w, ...
"method", "Chen1");
[trj.sim.y, ~, trj.sim.x] = lsim(S, trj.u.on(), I.grid, x0);
trj.sim.x = quantity.Discrete( trj.sim.x, I);
trj.sim.y = quantity.Discrete( trj.sim.y, I);
tc.verifyEqual(trj.x.on(), trj.sim.x.on(), 'AbsTol', 6e-3);
tc.verifyEqual(trj.y.on(), trj.sim.y.on(), 'AbsTol', 5e-3);
end end
function testParallel(tc) function testParallel(tc)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment