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)
% T: the time for which the transition should occur
arguments
sys ss;
t double;
t quantity.Domain;
optArgs.x0 double = zeros(size(sys.A, 1), 1);
optArgs.x1 double = zeros(size(sys.A, 1), 1);
optArgs.domainName string = "t";
end
x0 = optArgs.x0;
x1 = optArgs.x1;
t0 = t(1);
t1 = t(end);
t0 = t.lower;
t1 = t.upper;
%% Definitions
A = sys.A;
......@@ -55,7 +54,7 @@ n_u = size(B,2);
d = sym('d',[n_u,(2*n)]);
% variables
s = sym('s');
tau = sym( optArgs.domainName );
t_sym = sym( t.name );
%% Computation of the PI matrix
% 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
......@@ -79,7 +78,7 @@ for i = 1:n_u
temp = size(coef,2);
PI_i(j,1:size(coef,2)) = coef;
end
y0(i) = poly2sym(d(i,:),tau);
y0(i) = poly2sym(d(i,:),t_sym);
PI(i) = {PI_i};
end
PI = cell2mat(PI);
......@@ -102,20 +101,20 @@ c1 = pinv(PI,1e-12)*x1;
Y = sym('y',[n,n_u]);
Y(1,:) = y0;
for j = 1:(n-1)
Y(j+1,:) = diff(y0,tau,j);
Y(j+1,:) = diff(y0,t_sym,j);
end
% The actual computation of the coefficients d done for each element of
% y_schlange.
i_temp = 0;
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.';
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.';
[Ai,Bi] = equationsToMatrix([EQN1, EQN2], d(i,:));
coefpoly = linsolve(Ai,Bi);
y_schlange(i) = poly2sym(coefpoly,tau);
y_schlange(i) = poly2sym(coefpoly,t_sym);
i_temp = i_temp + n;
end
%% Computation of the input u, the states x and the output y = Cx
......@@ -133,19 +132,18 @@ end
Y_schlange = sym('y',[n,n_u]);
Y_schlange(1,:) = y_schlange;
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
% The computation of the input. Y_schlange must be extended to have one
% more row that containts the n-th derivative of y_schlange
u = flip(charpoly(A))* [Y_schlange;diff(y_schlange,tau,n)];
u = quantity.Symbolic( u.', 'grid', {t}, 'gridName', optArgs.domainName);
u = flip(charpoly(A))* [Y_schlange;diff(y_schlange,t_sym,n)];
u = quantity.Symbolic( u.', t);
% Y_schlange is now reshaped so that it corresponds to the size of the PI
% matrix and the calculation of the states follows directly.
Y_schlange = reshape(Y_schlange,n_u*n,1);
x = quantity.Symbolic( PI*Y_schlange, 'grid', {t}, 'gridName', optArgs.domainName);
y = quantity.Symbolic( sys.C*x.sym, ...
'grid', {t}, 'gridName', optArgs.domainName) + sys.D * u;
x = quantity.Symbolic( PI*Y_schlange, t);
y = quantity.Symbolic( sys.C*x.sym, t) + sys.D * u;
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
% [u, y, x] = planTrajectory(sys, t, varargin) computes a transition
% u(t) : x(t0) = x0 --> x(t1) = x1
......@@ -15,40 +15,71 @@ function [u, y, x] = planTrajectory(sys, t, optArgs)
%
arguments
sys ss;
t double;
t quantity.Domain;
optArgs.x0 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
x0 = optArgs.x0;
x1 = optArgs.x1;
% prepare time vectors
t = t(:);
t0 = t(1);
t1 = t(end);
tDomain = quantity.Domain({optArgs.domainName}, t);
if optArgs.method == "Bernstein"
t0 = t.lower;
t1 = t.upper;
% Compute the state transition matrix: Phi(t,tau) = expm(A*(t-tau) )
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'} );
Phi_t0 = expm(sys.A * quantity.Discrete( t - t0, tDomain));
% compute the gramian controllability matrix
%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);
%% compute the gramian controllability matrix
% W1 = misc.ss.gramian(sys, t, t0, optArgs.domainName);
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
% int_t0^t1 expm(A*(tau-t0)) * b * b^T * expm(A^T(tau-t0)) dtau
W1 = cumInt( Phi_t0 * sys.b * sys.b' * expm(sys.A' * quantity.Discrete( t - t0, tDomain)), ...
Phi_t0(1).domain, t0, optArgs.domainName);
%W2 = expm( sys.A * (t.Discrete - t1 ) ) * cumInt( invPhi_t1 * sys.B * sys.B' * invPhi_t1', t, t0, t.name);
%% 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
%% Chen^1
if optArgs.method == "Chen1"
%% 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;
%% Alternative Lösung nach Chen:
......
......@@ -36,7 +36,7 @@ tc.verifyEqual(misc.ss.relativeDegree(mySs, "min"), 1);
tc.verifyEqual(misc.ss.relativeDegree(mySs, "max"), 2);
end % testRelativeDegree
function planPolynomialTrajectory(tc)
function planPolynomialTrajectoryTest(tc)
%%
A = [-30 0; 0 -50];
B = [6; -5];
......@@ -44,55 +44,67 @@ function planPolynomialTrajectory(tc)
D = [];
S = ss(A, B, C, D);
t = linspace(1, 2, 1e3)';
t = quantity.Domain("t", linspace(1, 2, 1e3)');
x0 = [10; 12];
x1 = [-5; -5];
[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);
[y, ~, x] = lsim(S, trj.u.on(), t.grid, x0);
subplot(3,1,1);
plot(t, trj.u.on());
subplot(3,1,2);
plot(t, y - trj.y.on());
subplot(313);
plot(t, x - trj.x.on());
disp(x(end,:))
sim.y = quantity.Discrete(y, t);
sim.x = quantity.Discrete(x, t);
tc.verifyEqual(trj.x.on(), x, 'AbsTol', 2e-2);
tc.verifyEqual(trj.y.on(), y, 'AbsTol', 1e-2);
end
function testPlanTrajectory(tc)
function testPlanTrajectoryWeighted(tc)
%%
A = [-30 0; 0 -50];
A = [3 0; 0 5];
B = [6; -5];
C = [1, 1];
D = [];
S = ss(A, B, C, D);
t = linspace(0, 0.01, 1e2)';
I = quantity.Domain("t", linspace(0, .1, 1e2)');
x0 = [10; 12];
x1 = [-5; -5];
[trj.u, trj.y, trj.x] = misc.ss.planTrajectory(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);
% plot(t,y);
%
% subplot(313);
% plot(t, x);
% disp(x(end,:))
x1 = [11; 11];
tc.verifyEqual(trj.x.on(), x, 'AbsTol', 2e-2);
tc.verifyEqual(trj.y.on(), y, 'AbsTol', 1e-2);
[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);
tc.verifyEqual(trj.x.on(), trj.sim.x.on(), 'AbsTol', 2e-2);
tc.verifyEqual(trj.y.on(), trj.sim.y.on(), '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
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