From 12e35e35c70b3f0c8e4a1e614bc5d8d602db5c4b Mon Sep 17 00:00:00 2001 From: WataruOhnishi Date: Mon, 20 Jul 2020 14:00:16 +0200 Subject: [PATCH] Improved: numerical stability --- src/backandforth.m | 2 +- src/int_pBasis.m | 10 ++++++---- src/outPolyBasis.m | 4 ++-- src/polySolve.m | 15 +++++++++++---- 4 files changed, 20 insertions(+), 11 deletions(-) diff --git a/src/backandforth.m b/src/backandforth.m index c040666..53f080a 100644 --- a/src/backandforth.m +++ b/src/backandforth.m @@ -15,7 +15,7 @@ if nargin < 5 showFig = false; end -if ~all(diff(BCt)>0), error('error on BCt'); end +if any(diff(BCt)<0), error('error on BCt'); end trajType = lower(trajType); nofpoly = length(BCt)-1; % number of trajectory segments diff --git a/src/int_pBasis.m b/src/int_pBasis.m index e49f3c4..9e5d59d 100644 --- a/src/int_pBasis.m +++ b/src/int_pBasis.m @@ -31,28 +31,30 @@ nofpoly = length(pBasis); % number of trajectory segments out = cell(1,nofpoly); +dT = pBasis{1}.BCt(2)-pBasis{1}.BCt(1); out{1}.BCt = pBasis{1}.BCt; out{1}.BC0 = [initval; pBasis{1}.BC0;]; temp = polyint(pBasis{1}.a_syms(1,:)); temp = [zeros(1,length(pBasis{1}.a_syms(1,:))-length(temp)+1),temp]; -temp(end) = out{1}.BC0(1)-polyval(double(temp),out{1}.BCt(1)); +temp(end) = out{1}.BC0(1)-polyval(double(temp),0); out{1}.a_syms = [temp; ... [zeros(size(pBasis{1}.a_syms,1),1),pBasis{1}.a_syms]]; out{1}.a_vpas = double(out{1}.a_syms); -out{1}.BC1 = [polyval(out{1}.a_vpas(1,:),out{1}.BCt(2));... +out{1}.BC1 = [polyval(out{1}.a_vpas(1,:),dT);... pBasis{1}.BC1;]; out{1} = orderfields(out{1}); for k = 2:nofpoly out{k}.BCt = pBasis{k}.BCt; out{k}.BC0 = out{k-1}.BC1; + dT = out{k}.BCt(2) - out{k}.BCt(1); temp = polyint(pBasis{k}.a_syms(1,:)); temp = [zeros(1,length(pBasis{k}.a_syms(1,:))-length(temp)+1),temp]; - temp(end) = out{k}.BC0(1)-polyval(double(temp),out{k}.BCt(1)); + temp(end) = out{k}.BC0(1)-polyval(double(temp),0); out{k}.a_syms = [temp; ... [zeros(size(pBasis{k}.a_syms,1),1),pBasis{k}.a_syms]]; out{k}.a_vpas = double(out{k}.a_syms); - out{k}.BC1 = [polyval(out{k}.a_vpas(1,:),out{k}.BCt(2));... + out{k}.BC1 = [polyval(out{k}.a_vpas(1,:),dT);... pBasis{k}.BC1]; out{k} = orderfields(out{k}); end diff --git a/src/outPolyBasis.m b/src/outPolyBasis.m index e123efe..d84a25b 100644 --- a/src/outPolyBasis.m +++ b/src/outPolyBasis.m @@ -55,7 +55,7 @@ y(idx) = 0; elseif k <= npoly idx = t_segmentIdx == k; - y(idx) = polyval(double(pBasis{k}.a_vpas(n,:)),t(idx)); + y(idx) = polyval(double(pBasis{k}.a_vpas(n,:)),t(idx)-pBasis{k}.BCt(1)); else idx = t_segmentIdx == npoly+1; if n == 1 @@ -73,7 +73,7 @@ y(idx) = 0; elseif k <= npoly idx = t_segmentIdx == k; - y(idx) = subs(poly2sym(pBasis{k}.a_syms(n,:)),t(idx)); + y(idx) = subs(poly2sym(pBasis{k}.a_syms(n,:)),t(idx)-pBasis{k}.BCt(1)); else idx = t_segmentIdx == npoly+1; if n == 1 diff --git a/src/polySolve.m b/src/polySolve.m index 7a21c8d..193a196 100644 --- a/src/polySolve.m +++ b/src/polySolve.m @@ -27,6 +27,8 @@ showFig = 0; end +dT = t1 - t0; +if dT < 0, error('error at BCt'); end %%%%% syms t real @@ -47,8 +49,8 @@ Eq_fin = sym('Eq_fin',[(n+1)/2,1]); for kk = 1:1:(n+1)/2 - Eq_init(kk) = subs(F(kk),t, t0); - Eq_fin(kk) = subs(F(kk),t, t1); + Eq_init(kk) = subs(F(kk),t, 0); + Eq_fin(kk) = subs(F(kk),t, dT); end S = solve([Eq_init; Eq_fin;] == [initval; finval;]); @@ -60,6 +62,11 @@ a_vpa(kk) = getfield(S,char(name(kk))); a_sym(kk) = getfield(S,char(name(kk))); end + if length(name) < length(a) + for kk = length(name)+1:length(a) + a_sym(kk) = 0; + end + end else a_sym = zeros(n+1,1); end @@ -89,11 +96,11 @@ if showFig == 1 - t_val = t0:(t1-t0)/100:t1; + t_val = 0:dT/100:dT; for kk = 1:1:(n+1)/2 sTitle = sprintf('%d derivative',kk-1); figure; - plot(t_val,polyval(a_vpas(kk,:),t_val)); + plot(t_val+t0,polyval(a_vpas(kk,:),t_val)); title(sTitle); xlabel('Time [s]'); grid on; box on;