Improved: numerical stability

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

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

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

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;