diff --git a/+misc/+ss/planPolynomialTrajectory.m b/+misc/+ss/planPolynomialTrajectory.m
index 1414951f805608618e5423ec0f072f3b9d170336..3fc01a7607220264e649c6f0cc43cf2e6ecce921 100644
--- a/+misc/+ss/planPolynomialTrajectory.m
+++ b/+misc/+ss/planPolynomialTrajectory.m
@@ -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
diff --git a/+misc/+ss/planTrajectory.m b/+misc/+ss/planTrajectory.m
index 1cfb386dcc68d95a2428a036ad47c7066a0c984a..cddbb2b649f4139c1ca8c0165ddb7e6a3e7cabc7 100644
--- a/+misc/+ss/planTrajectory.m
+++ b/+misc/+ss/planTrajectory.m
@@ -1,6 +1,6 @@
-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, y, x] = planTrajectory(sys, t, varargin) computes a transition
% u(t) : x(t0) = x0 --> x(t1) = x1
% for a state-space system sys of the form
% d/dt x = A x + bu.
@@ -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);
-
-%% 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));
-
-invPhi_t1 = expm(sys.A * quantity.Discrete( t1 - t, tDomain));
+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 gramian controllability matrix
+ %W1 = misc.ss.gramian(sys, t, t0, optArgs.domainName);
+
+ %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
-% W1 = misc.ss.gramian(sys, t, 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
-% 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);
+%% 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:
@@ -56,6 +87,6 @@ y = sys.C * x + sys.D * u;
% W0 = invPhi_t0 * W1 * invPhi_t0';
% 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));
-%
+%
% % simulation results have shown, does the average of both soultions leads to a better than each.
% u = (u0 + u1) / 2;
diff --git a/+unittests/+misc/+ss/testSs.m b/+unittests/+misc/+ss/testSs.m
index 924a32d552143ac58021438e36d088370bb8cabc..0603c2e087b64b3c868fbbb0a4d197262baa92c4 100644
--- a/+unittests/+misc/+ss/testSs.m
+++ b/+unittests/+misc/+ss/testSs.m
@@ -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);
-
- subplot(3,1,1);
- plot(t, trj.u.on());
- subplot(3,1,2);
- plot(t, y - trj.y.on());
+ [y, ~, x] = lsim(S, trj.u.on(), t.grid, x0);
- 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);
+ x1 = [11; 11];
+
+ [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);
-% subplot(3,1,1);
-% 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);
+ 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)