Commit 0a78d905 by Ferdinand Fischer

### 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, 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;
 ... ... @@ -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) ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!