Bspine trajectory planning

clc;clear;close;

t= [0,5,7,8,10,15,18];
q= [3, 2, 0, -2, -5, 0, 6, 12, 0, -3, 8]';

n=size(q,1)-1;

p=4;
NodeVector=[0, 0, 0, 0, 0, 2.5, 6, 7.5, 9, 12.5, 16.5, 18, 18, 18, 18, 18];
v0=2;
vn=-3;

a0=0;
an=0;

Base = zeros(n+1, 1)';

for a=1:size(t,2)
    
   for b=p+1:size(NodeVector,2)-p
       if t(a)>=NodeVector(b) && t(a)<NodeVector(b+1)||(a==size(t,2)&&t(a)==NodeVector(b))
            i=b;
       end
   end
   
   u=t(a);
   if u==18
      i=i-1; 
   end
    [B,D]= MyBaseFun(i, p , u, NodeVector);
    Base(a,1:size(B,2))=B';
    
end

i=5;
u=t(1);
der=3;
[B,D]= MyBaseFun(i, p , u, NodeVector);
[Ders]=MyDersBasisFuns( i,u,p,B,D,der,NodeVector);

Ders=[Ders,zeros(size(Ders,1),size(Base,2)-size(Ders,2))]
   
i=11;
u=t(size(t,2));
der=3;
[B,D]= MyBaseFun(i, p , u, NodeVector);
[Ders2]=MyDersBasisFuns( i,u,p,B,D,der,NodeVector);

A=[Base(1,:);Ders(2,:);Ders(3,:);Base(2:size(Base,1)-1,:);Ders2(3,:);Ders2(2,:);Base(size(Base,1),:)]

Point=inv(A)*q
Point=Point'

n=size(Point,2)-1;
d=1;
for u = min(t) :0.1: max(t)
  
    for k=p+1:size(NodeVector,2)-p
       if u>=NodeVector(k) && u<NodeVector(k+1)||(u==max(t))
            i=k;
       end
    end
    
   if u==18
      i=i-1; 
   end
   
    [B,D]= MyBaseFun(i, p , u, NodeVector);
    [Ders]=MyDersBasisFuns( i,u,p,B,D,der,NodeVector);
    Ders=[Ders,zeros(size(Ders,1),size(Base,2)-size(Ders,2))]
    Base2(1:size(B,2),1)=B';
    Base2=[Base2;zeros(size(Base,2)-size(Base2,1),1)];
    Ptemp = Point * Base2;
    Vtemp=Point*Ders(2,:)';
    Atemp=Point*Ders(3,:)';
    Jtemp=Point*Ders(4,:)';
    ut(d)=u;
    pt(d)=Ptemp;
    vt(d)=Vtemp;
    at(d)=Atemp;
    jt(d)=Jtemp;  
    d=d+1;
end

figure('name','BSpine位移曲线');

plot(ut,pt);

figure('name','BSpine速度曲线');

plot(ut,vt);

figure('name','BSpine加速度曲线');

plot(ut,at);

figure('name','BSpine Jerk曲线');

plot(ut,jt);