Построение кривой Безье во время моделирования в MATLAB Simulink с использованием нелинейного MPC

В настоящее время я делаю модель для имитации объезда двух автомобилей в Simulink, для следования по пути я использую встроенный нелинейный MPC в Simulink. Он получает опорные точки в качестве входных данных.

Для построения опорных точек я разработал функцию MATLAB. Вот код:

function bezierPoints = buildBezier(egoX, egoY, egoV, targetX, targetY, targetV)

bezierPoints = zeros(3,1);
pts = [];
yaw = [0];
timeGap = (abs(egoX - targetX))/(egoV - targetV);

counter = 0;

if (egoX - targetX) < -1.5
   %counter = 1;

   P0 = [egoX ; egoY];
   P1 = P0;
   P2 = [egoX + 2 ; egoY];
   P3 = P2;

   t = linspace(0,1,80);
   yaw = linspace(0,0,80);
   pts = [kron((1-t).^3,P0) + kron(3*(1-t).^2.*t,P1) + kron(3*(1-t).*t.^2,P2) + kron(t.^3,P3), yaw'];
    
   counter = counter + 1;

elseif (egoX - targetX) > -1.5 && (egoX - targetX) <= 1
   %counter = 1;

   finalX = egoX + egoV*timeGap;
   intermediateX = (egoX + finalX)/2;
   P0 = [egoX ; egoY];
   P1 = [intermediateX ; -0.5];
   P2 = [intermediateX ; 0.5];
   P3 = [finalX ; 0.5];

   t = linspace(0,1,80);
   pts =  [kron((1-t).^3,P0) + kron(3*(1-t).^2.*t,P1) + kron(3*(1-t).*t.^2,P2) + kron(t.^3,P3)];
   for i=1:(size(pts,2)-1)
       yaw = [ yaw , atan((pts(2,i+1)-pts(2,i))/(pts(1,i+1)-pts(1,i)))];
   end
   pts = [pts, yaw'];

   counter = counter + 1;

elseif (egoX - targetX) > 1
   %counter = 1;

   finalX = egoX + egoV*timeGap;
   intermediateX = (egoX + finalX)/2;
   P0 = [egoX ; egoY];
   P1 = [intermediateX ; 0.5];
   P2 = [intermediateX ; -0.5];
   P3 = [finalX ; -0.5];

   t = linspace(0,1,80);
   pts =  [kron((1-t).^3,P0) + kron(3*(1-t).^2.*t,P1) + kron(3*(1-t).*t.^2,P2) + kron(t.^3,P3)];
   for i=1:(size(pts,2)-1)
       yaw = [yaw , atan((pts(2,i+1)-pts(2,i))/(pts(1,i+1)-pts(1,i)))];
   end 
   pts = [pts, yaw'];
    
   counter = counter + 1;
end
bezierPoints = pts(:,counter);  
end

Я знаю, что есть некоторые проблемы в коде, и сама система довольно глупа, но основная проблема сейчас в том, что нелинейный MPC не может получить сигнал переменного размера (MATLAB говорит, что он переменного размера, но на самом деле размер всегда один и тот же, потому что я определяю количество точек в этой строке t = linspace(0,1,80);). Simulink показывает следующие ошибки.

Я попробовал пару вещей. Я сделал переменную bezierPoints постоянной, попробовал другой метод построения кривой Безье отсюда: Как преобразовать код квадратичной кривой Безье в кубическую кривую Безье?. В приведенном выше коде я использовал метод отсюда: Кривые Безье и тензорное произведение Кронекера. Я переместил присвоение переменной bezierPoints в конец функции (я думал, что, возможно, несколько присвоений вызывают эту ошибку из-за разных размеров векторов). Но ничего не сработало.

Итак, теперь я связываю эту проблему с методом, который я использую для построения кривой Безье. Есть ли способ построить эту кривую во время моделирования в виде массива фиксированного размера? До этой функции я предопределял кривую перед симуляцией и использовал блок From Workspace с привязкой ко времени для симуляции, но теперь мне нужно построить ее во время симуляции в зависимости от положения и скорости обеих машин.

Спасибо!