New trajectory planner based on polynomial interpolation

+misc/+ss/planPolynomialTrajectory.m

+function [u, y, x] = planPolynomialTrajectory(sys, t, optArgs)
+%FLATKERNELCOMPUTATION solves the transition x(0) -> x(T) for a given
+%state space based on a flat output
+%
+%   [u,x,Cx] = FLATKERNELCOMPUTATION(system, xT, T) calculates
+%   the input u, the states x and the output Cx for a given state space
+%   system symbolically. The calculation is based on the flat output based approach 
+%   for trajectory planning. The system for which the
+%   transition is done is of the following form.
+%
+%   \dot(x(t)) = A * x(t) + B * u(t)
+%         y(t) = C * x(t)
+%   The transition of the states for time T
+%        x(0) = 0 -> x(T) = xT
+%
+%   The Laplace transformation of the input n is calculated with the formula
+%   u(s) = det(sI - A) * y_schlange
+%   where y_schlange is a polynomial of (2*n)-th degree and
+%   x(s) = adj(sI - A) * B * y_schlange
+%
+%   The transition is done by solving linear systems of equations for the
+%   coefficients of the polynomial y_schlange, such that all the
+%   requierments are fullfilled. For more Information see the derivation in
+%   the Documentation
+%
+%   INPUT PARAMETERS:  system: The LTI state space system defined as an ss
+%                              object in MatLab
+%                          xT: the end State defined as a column vector
+%                              from real numbers
+%                           T: the time for which the transition should occur
+arguments
+	sys ss;
+	t double;
+	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);
+
+%% Definitions
+A = sys.A;
+B = sys.B;
+
+% number of states
+n = size(A,1);
+% number of inputs
+n_u = size(B,2);
+
+% coefficients for the polynomials
+d = sym('d',[n_u,(2*n)]);
+% variables
+s = sym('s');
+tau = sym( optArgs.domainName );
+%% 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
+%  PI_s = [PI_1,PI_2,...,PI_{n_u}] where PI_i is the i-th column of the
+%  PI_s matrix. The computation of the PI matrix is done by taking each
+%  element of PI_s and with the help of the coeffs function from MatLab
+%  extracting each coefficient from the complex polynomial. The result is
+%  stored in a cell structure and then transformed into a matrix PI.
+%  Additionaly utilizing poly2sym function from MatLab each vector of
+%  coefficients d is transformed into a polynomial with tau as a variable.
+%  Each polynomial is then stored in y0. The coefficients are still unknown
+
+
+PI_s = adjoint(s*eye(n)-A)*B;
+
+for i = 1:n_u
+    PI_i = zeros(n,n);
+    for j = 1:n
+        coef = coeffs(PI_s(j,i),s,'All');
+        coef = flip(coef);
+        temp = size(coef,2);
+        PI_i(j,1:size(coef,2)) = coef;
+    end
+    y0(i) = poly2sym(d(i,:),tau);
+    PI(i) = {PI_i};
+end
+PI = cell2mat(PI);
+
+%% Computing y_schlange
+% The coefficients of y_schlange are computed as follows:
+% It is known from the derivation that each element in y_schlange must fulfill
+% certain conditions. These conditions are that y_schlange and its (n-1)
+% derivatives must be 0 for tau = 0, this is included in EQN1 and that
+% y_schlange and its (n-1) derivatives for tau = T must be equal to 
+% PI^{+}xT where PI^{+} is the pseudo inverse of PI. This is included in EQN2
+% A linear system of equations for the coefficients d is solved for every
+% element in each y_schlange and the result is stored in y_schlange 
+
+% PI^{+}xT
+c0 = pinv(PI,1e-12)*x0;
+c1 = pinv(PI,1e-12)*x1;
+% An auxiliary matrix Y containing each y(i) and its (n-1) derivatives 
+% so that the equations for the coefficients d can be formed 
+Y = sym('y',[n,n_u]);
+Y(1,:) = y0;
+for j = 1:(n-1)
+    Y(j+1,:) = diff(y0,tau,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 = EQN1.';
+    EQN2 = subs(Y(:,i),tau,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);
+    i_temp = i_temp + n;
+end
+%% Computation of the input u, the states x and the output y = Cx
+% Each input u_i is computed as the multiplication of the charateristic
+% polynomial of the A matrix with the corresponding flat output
+% y_schlange_i and its n derivatives. The states x can be calculated
+% directly by multiplying the PI matrix and y_schlange and all its (n-1)
+% derivatives. The calculation of the Output is trivial with y = Cx.
+
+
+% Y_schlange is an auxiliary matrix that stores all the elements of
+% y_schlange and its (n-1) derivatives. The first element of a column in
+% Y_schlange corresponds y_schlange_i and the last element corresponds to
+% the n-1 derivative of y_schlange_i.
+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);
+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);
+
+% 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);
+
+end

+unittests/+misc/testSs.m

@@ -4,6 +4,37 @@ function [tests] = testSs()
 tests = functiontests(localfunctions());
 end
 
+
+function planPolynomialTrajectory(tc)
+%%
+	A = [-30 0; 0 -50];
+	B = [6; -5];
+	C = [1, 1];
+	D = [];
+	
+	S = ss(A, B, C, D);
+	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());
+	
+	subplot(313);
+	plot(t, x - trj.x.on());
+	disp(x(end,:))
+
+	tc.verifyEqual(trj.x.on(), x, 'AbsTol', 2e-2);
+	tc.verifyEqual(trj.y.on(), y, 'AbsTol', 1e-2);
+
+end
+
 function testPlanTrajectory(tc)
 %%
 	A = [-30 0; 0 -50];