Commit 2a30f05d authored by Jakob Gabriel's avatar Jakob Gabriel
Browse files

misc.ss.setPointChange: now also calculates multiple polynomes at once.

parent b6c78d30
......@@ -6,42 +6,53 @@ function [f, a] = setPointChange(phi0, phi1, t0, t1, n, optArgs)
% d^k / dt^k f(t)|_{t=t0,t1} = 0, k = 1, 2, ..., n
% The name of the variable can be set with "var" as name value pair.
arguments
phi0 (1,1) double;
phi1 (1,1) double;
phi0 (:,1) double;
phi1 (:,1) double;
t0 (1,1) double;
t1 (1,1) double;
n (1,1) double;
optArgs.var = sym( "t" );
end
assert(numel(phi0) == numel(phi1), "phi0 and phi1 must have same number of elements")
if ~isscalar(phi0)
f = cell(size(phi0));
a = cell(size(phi0));
optArgsNameValue = misc.struct2namevaluepair(optArgs);
for it = 1 : numel(phi0)
[f{it}, a{it}] = misc.ss.setPointChange(phi0(it), phi1(it), t0, t1, n, optArgsNameValue{:});
end % for it = 1 : numel(phi0)
f = cat(1, f{:});
a = cat(2, a{:});
else
N = 2*n+2;
N = 2*n+2;
M0 = zeros(n+1, N);
M1 = zeros(n+1, N);
M0 = zeros(n+1, N);
M1 = zeros(n+1, N);
coefficient = @(k, i, t) factorial( i + k -1 ) / factorial( i - 1) * t^(i-1);
coefficient = @(k, i, t) factorial( i + k -1 ) / factorial( i - 1) * t^(i-1);
for k = 0:n
for i = 1:( N - k )
M0(k+1, i + k) = coefficient(k, i, t0);
M1(k+1, i + k) = coefficient(k, i, t1);
for k = 0:n
for i = 1:( N - k )
M0(k+1, i + k) = coefficient(k, i, t0);
M1(k+1, i + k) = coefficient(k, i, t1);
end
end
end
a = [M0; M1] \ [phi0; zeros(n,1); phi1; zeros(n,1)];
if misc.issym( optArgs.var )
t = optArgs.var;
else
t = sym( optArgs.var );
end
a = [M0; M1] \ [phi0; zeros(n,1); phi1; zeros(n,1)];
f = sym(0);
if misc.issym( optArgs.var )
t = optArgs.var;
else
t = sym( optArgs.var );
end
for i = 1 : N
f = f + a(i) * t^(i-1);
end
f = sym(0);
end
for i = 1 : N
f = f + a(i) * t^(i-1);
end
end % if ~isscalar(phi0)
end % setPointChange()
\ No newline at end of file
......@@ -48,8 +48,9 @@ function planPolynomialTrajectoryTest(tc)
x0 = [10; 12];
x1 = [-5; -5];
[trj.u, trj.y, trj.x] = misc.ss.planPolynomialTrajectory(S, t, 'x0', x0, 'x1', x1);
warning off
[y, ~, x] = lsim(S, trj.u.on(), t.grid, x0);
warning on
tc.verifyEqual(trj.x.on(), x, "AbsTol", 2e-2);
tc.verifyEqual(trj.y.on(), y, "AbsTol", 1e-2);
......
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