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

+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

+misc/+ss/planTrajectory.m

-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;

+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)