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

:trajectory planning: allow to specify the name for the domain of the state transition matrix

parent ce85800b
function W = gramian(sys, t, t0)
function W = gramian(sys, t, t0, gridName)
% GRAMIAN controllability matrix.
% W = gramian(sys, t, t0) computes the gramian controllability matrix of
% the state space system sys. For each timestep t. If t0 is not set, the
......@@ -14,14 +14,16 @@ function W = gramian(sys, t, t0)
% W(t0, t1) = int_t0^t1 expm(A*(tau-t0)) * b * b^T * expm(A^T(tau-t0)) dtau
% = int_t0^t1 expm(A*(t1 - tau) * b * b^T * expm(A^T(t1 - tau)) dtau
%
if nargin == 2
t0 = 0;
arguments
sys ss;
t double;
t0 double = 0;
gridName = "t";
end
n = size(sys.A, 1);
Q = [-sys.A sys.B*sys.B'; ...
zeros(n) sys.A'] * quantity.Discrete( t - t0, 'grid', t, 'gridName', 't');
zeros(n) sys.A'] * quantity.Discrete( t - t0, 'grid', t, 'gridName', gridName);
temp = expm(Q);
F3 = temp(n+1:end,n+1:end);
......
function [u, y, x] = planTrajectory(sys, t, varargin)
function [u, y, x] = planTrajectory(sys, t, optArgs)
% PLANTRAJECTORY Computes a trajectory for a lumped-parameter systems
% [u, y, x] = planTrajectory(sys, t, varargin) computes a transition
% u(t) : x(t0) = x0 --> x(t1) = x1
......@@ -13,12 +13,16 @@ function [u, y, x] = planTrajectory(sys, t, varargin)
% x(t) = Phi(t0, t) * W(t0, t) * W0^(-1)(t0, t1) * ( Phi(t0, t1) * x1 - x0)
% y(t) = C x(t)
%
p = misc.Parser();
p.addParameter('x0', zeros(size(sys.A, 1), 1));
p.addParameter('x1', zeros(size(sys.A, 1), 1));
p.parse(varargin{:});
x0 = p.Results.x0;
x1 = p.Results.x1;
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;
% prepare time vectors
t = t(:);
......@@ -27,12 +31,12 @@ t1 = t(end);
%% 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, 'grid', {t}, 'gridName', {'t'} ));
Phi_t0 = expm(sys.A * quantity.Discrete( t - t0, 'grid', {t}, 'gridName', {optArgs.domainName} ));
invPhi_t1 = expm(sys.A * quantity.Discrete( t1 - t, 'grid', {t}, 'gridName', {'t'} ));
invPhi_t1 = expm(sys.A * quantity.Discrete( t1 - t, 'grid', {t}, 'gridName', {optArgs.domainName} ));
%% compute the gramian controllability matrix
W1 = misc.ss.gramian(sys, t, t0);
W1 = misc.ss.gramian(sys, t, t0, optArgs.domainName);
W1_t1 = W1.at(t1);
% Formel aus dem Bernstein:
u = sys.b.' * invPhi_t1.' / W1_t1 * (x1 - Phi_t0.at(t1) * x0);
......
......@@ -5,7 +5,7 @@ arguments
names cell;
dim = 1;
end
sizes = [];
for k = 1:length(names)
if isfield(optArg, names{k})
sizes(k) = size(optArg.(names{k}), dim);
......
......@@ -330,9 +330,11 @@ classdef (InferiorClasses = {?quantity.Symbolic}) Discrete ...
% now the common domains, i.e., zeta = z must be merged:
% for this, find the index of the common domain in list of
% temporary combined domain
intersectDomain = intersect( originalDomain( ~logOfDomain ), ...
g(1).domain );
% Before, do a cast to quantity.Domain in order to handle also
% quantity.EquidistantDomain objects.
intersectDomain = intersect( ...
quantity.Domain( originalDomain( ~logOfDomain ) ), ...
quantity.Domain( g(1).domain ) );
if ~isempty(intersectDomain)
......
......@@ -50,8 +50,8 @@ classdef (InferiorClasses = {?quantity.EquidistantDomain}) Domain < ...
upper = obj.grid(end);
end % get.upper()
function o = quantity.Equidistant(obj)
function d = quantity.EquidistantDomain(obj)
d = quantity.Domain(obj.name, obj.grid);
end
end
......
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