Commit a88ead10 authored by Ferdinand Fischer's avatar Ferdinand Fischer
Browse files

separated computation of state and input transition matrices

parent 1c1ac195
......@@ -426,8 +426,9 @@ classdef (InferiorClasses = {?quantity.Discrete, ?quantity.Function, ?quantity.S
Psi = signals.PolynomialOperator(Psi);
end
function [Phi, Psi, F0] = stateTransitionMatrix(obj, optArgs)
function [Phi, F0] = stateTransitionMatrix(obj, optArgs)
% STATETRANSITIONMATRIX computation of the
% TODO
% state-transition matrix
% [Phi, Psi] = stateTransitioMatrix(obj, varargin) computes
% the state-transition matrix of the boundary value problem
......@@ -441,25 +442,18 @@ classdef (InferiorClasses = {?quantity.Discrete, ?quantity.Function, ?quantity.S
arguments
obj signals.PolynomialOperator;
optArgs.N double = 1;
optArgs.B signals.PolynomialOperator = signals.PolynomialOperator.empty(1, 0);
optArgs.F0;
end
n = size(obj(1).coefficient, 2);
B = optArgs.B;
N = optArgs.N;
if isempty(B)
m = 0;
else
m = size(B(1).coefficient, 2);
end
nA = length(obj); % As order of a polynom is zero based
% initialization of the ProgressBar
counter = N - 1;
pbar = misc.ProgressBar(...
'name', 'Trajectory Planning (Woittennek)', ...
'name', 'Computation of the state transition matrix', ...
'terminalValue', counter, ...
'steps', counter, ...
'initialStart', true);
......@@ -492,22 +486,97 @@ classdef (InferiorClasses = {?quantity.Discrete, ?quantity.Function, ?quantity.S
end
z = F0(1).grid{1};
Phi(:,:,1) = F0.subs("zeta", 0);
On = quantity.Discrete.zeros([n n], z, "gridName", "z");
for k = 2 : N
pbar.raise();
% compute the temporal matrices:
sumPhi = On;
for l = 2 : min(nA, k)
sumPhi = sumPhi + obj(l).coefficient * Phi(:, :, k-l+1);
end
% compute the integration of the fundamental matrix with the temporal
% values:
% Phi_k(z) = int_0_z Phi_0(z, zeta) * M(zeta) d_zeta
Phi(:, :, k) = cumInt(F0 * sumPhi.subs("z", "zeta"), "zeta", z(1), "z");
end
%
pbar.stop();
% TODO: warning schreiben, dass überprüft ab welcher Ordnung
% die Matrizen nur noch numerisches Rauschen enthalten!
Phi = signals.PolynomialOperator(Phi);
end
function [Psi] = inputTransitionMatrix(obj, B, Phi0, optArgs)
% STATETRANSITIONMATRIX computation of the
% TODO
% state-transition matrix
% [Phi, Psi] = stateTransitioMatrix(obj, varargin) computes
% the state-transition matrix of the boundary value problem
% dz x(z,s) = A(z,s) x(xi,s) + B(z,s) * u(s)
% so that Phi is the solution of
% dz Phi(z,xi,s) = A(z,s) Phi(z,xi,s)
% and Psi is given by
% Psi(z,xi,s) = int_xi_z Phi(z,zeta,s) B(zeta,s) d zeta
% if B(z,s) is defined as signals.PolynomialOperator object by
% name-value-pair.
arguments
obj signals.PolynomialOperator;
B signals.PolynomialOperator
Phi0 quantity.Discrete;
optArgs.N double = 1;
optArgs.F0;
end
n = size(obj(1).coefficient, 2);
N = optArgs.N;
if isempty(B)
Psi(:,:,1) = quantity.Discrete.empty();
m = 0;
else
Psi(:,:,1) = cumInt( F0 * B(1).coefficient.subs("z", "zeta"), "zeta", z(1), "z");
m = size(B(1).coefficient, 2);
end
nA = length(obj); % As order of a polynom is zero based
On = quantity.Discrete.zeros([n n], z, "gridName", "z");
% initialization of the ProgressBar
counter = N - 1;
pbar = misc.ProgressBar(...
'name', 'Computation of the state transition input matrix', ...
'terminalValue', counter, ...
'steps', counter, ...
'initialStart', true);
%% computation of transition matrix as power series
% The fundamental matrix is considered in the laplace space as the state
% transition matrix of the ODE with complex variable s:
% dz w(z,s) = A(z, s) w(z,s) * B(z) v(s),
% Hence, fundamental matrix is a solution of the ODE:
% dz Phi(z,zeta) = A(Z) Phi(z,zeta)
% Phi(zeta,zeta) = I
%
% Psi(z,xi,s) = int_zeta^z Phi(z, xi, s) B(xi) d_xi
%
% At this, A(z, s) can be described by
% A(z,s) = A_0(z) + A_1(z) s + A_2(z) s^2 + ... + A_d(z) s^d
%
% With this, the recursion formula
% Psi_k(z, zeta) = int_zeta^z Phi_0(z, xi) * sum_i^d A_i(xi) *
% Psi_{k-i}(z, xi) + B_k(xi) d_xi
z = Phi0(1).domain(1);
Psi(:,:,1) = cumInt( Phi0 * B(1).coefficient.subs("z", "zeta"), "zeta", z.lower, "z");
Om = quantity.Discrete.zeros([n m], z, "gridName", "z");
for k = 2 : N
pbar.raise();
% compute the temporal matrices:
sumPhi = On;
sumPsi = Om;
for l = 2 : min(nA, k)
sumPhi = sumPhi + obj(l).coefficient * Phi(:, :, k-l+1);
sumPsi = sumPsi + obj(l).coefficient * Psi(:, :, k-l+1);
end
if size(B, 1) >= k && ~isempty(B)
......@@ -516,21 +585,16 @@ classdef (InferiorClasses = {?quantity.Discrete, ?quantity.Function, ?quantity.S
% compute the integration of the fundamental matrix with the temporal
% values:
% Phi_k(z) = int_0_z Phi_0(z, zeta) * M(zeta) d_zeta
Phi(:, :, k) = cumInt(F0 * sumPhi.subs("z", "zeta"), "zeta", z(1), "z");
Psi(:, :, k) = cumInt(F0 * sumPsi.subs("z", "zeta"), "zeta", z(1), "z");
Psi(:, :, k) = cumInt(Phi0 * sumPsi.subs("z", "zeta"), "zeta", z.lower, "z");
end
%
pbar.stop();
% TODO: warning schreiben, dass überprüft ab welcher Ordnung
% die Matrizen nur noch numerisches Rauschen enthalten!
Phi = signals.PolynomialOperator(Phi);
Psi = signals.PolynomialOperator(Psi);
end
function newOperator = subs(obj, varargin)
newOperator = obj.M.subs(varargin{:});
......
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