Resampling a control input sequence and testing it on Simulink using sim command

My goal is to generate a control input sequence that takes my vehicle from point A to point B in minimum time. In order to do so, I created a simulation based on the Zermelo’s navigation problem using the command fmincon.

So far, so good. The problem is that my solution (control input sequence) has an sample rate slower than I desire. To fix this problem without changing the step size using in the fmincon optimization, I basically wrote a script that resamples the original sequence to make it as fast I want.

To validate my new control sequence, I am comparing the vehicle's trajectory when I apply each of the two control sequences generated but the results do not match. One interesting thing is that the higher is my resample rate, worse is the resulting trajectory. Can anyone help me to debug it? I believe that the inconsistency is during the validation since the resampled sequence looks good to me.

Thanks in advance.

Below are the plots from the resampled sequence and the vehicle’s trajectory as well as the piece of code that I use to resample and validate my approach.

close all
  % theta_final(1:end-1) is the control sequence successfully generated using fmincon
  % tau=0:step_size:1; % step_size defines the original sequence sample rate
  [t,y]=sim('model1',1,[],[tau' theta_final(1:end-1)]); % simulate the original control sequence
  T=0.5*step_size*tfinal; % increase the original signal sample rate by 2
  [ thetaK,tt ] = ControlRateConversion( tfinal,step_size,T,theta_final(1:end-1)); %tfinal is a scaling factor
  [tVal,yVal]=sim('validation',tt(end),[],[tt thetaK]);

    %%plotting
    figure
    plot(y(:,1),y(:,2),'b','LineWidth',2) %plot states
    legend('Original trajectory','Resampled trajectory')
    title('Free final time')
    xlabel('x')
    ylabel('y')
    grid on
    hold on
    plot(yVal(:,1),yVal(:,2),'r-.','LineWidth',2) %plot states

    %%
    function [ thetaK,tt ] = ControlRateConversion( tfinal,step_size,T,theta)

    time=tfinal*(0:step_size:1e6);
    t(1)=0;
    k=1;
    thetaK(1)=theta(1);
    for i=1:length(theta)-1
        while k*T<time(i+1) && k*T>=time(i)
    %         time(i);
    %         k*T;
    %         time(i+1);
            if abs(i*tfinal*step_size-k*T) < 1e-9
                thetaK(k+1)=theta(i+1);
                t(k+1)=k*T;
            else
                thetaK(k+1)=theta(i);
                t(k+1)=k*T;
            end
            k=k+1;
        end
    end
    thetaK(k+1)=theta(end);
    t(k+1)=k*T;
    tt=t;
    figure
    stairs(0:tfinal*step_size:tfinal,theta,'*-')
    hold on
    plot(tt,thetaK,'ro')
    title('Control input rate vs Control sequence rate')
    ylabel('u*')
    xlabel('time')
    legend('Original sequence','Resampled sequence')
    thetaK=thetaK';
    tt=tt';
    end