pp는 후진을 못하니 nlmpc로 간다!

nlobjTracking = nlmpc(3,3,2);
Ts = 0.1;
pTracking = 30;
nlobjTracking.Ts = Ts;
nlobjTracking.PredictionHorizon = pTracking;
nlobjTracking.ControlHorizon = pTracking;
nlobjTracking.MV(1).Min = -2;
nlobjTracking.MV(1).Max = 2;
nlobjTracking.MV(2).Min = -pi/8;
nlobjTracking.MV(2).Max = pi/8;
nlobjTracking.Weights.ManipulatedVariablesRate = [0.1,0.2];
nlobjTracking.Weights.OutputVariables = [1,1,3]; 
nlobjTracking.Model.StateFcn = "parkingVehicleStateFcnRRT";
nlobjTracking.Jacobian.StateFcn = "parkingVehicleStateJacobianFcnRRT";
nlobjTracking.Optimization.CustomEqConFcn = "parkingTerminalConFcn";
validateFcns(nlobjTracking,randn(3,1),randn(2,1));
x0 = egoInitialPose;
ref = xRef;
currentPose = updateVehiclePose(modelStatesSub, tftree);
lastMV = [0, 0]; % 속도와 각속도에 대한 초기 제어 입력

while ~isDestinationReached(currentPose, ref(end,:),0.1) % 목적지에 도달할 때까지 루프
    % 현재 상태를 GPS 데이터로 업데이트
    currentPose = updateVehiclePose(modelStatesSub, tftree);
    x0 = [currentPose(1), currentPose(2), currentPose(3)];

    % NMPC를 사용하여 속도 및 각속도 계산
    [mv, info] = nlmpcmove(nlobjTracking, x0, lastMV, ref);

    % 속도 및 각속도 출력
    disp(['속도: ', num2str(mv(1)), ', 각속도: ', num2str(mv(2))]);
    [pub, msg] = publish_twist_command(mv(1), mv(2), '/cmd_vel');
    send(pub, msg);
    % 차량 상태 업데이트 (시뮬레이션 또는 실제 차량)
    lastMV = mv;

    % 다음 반복을 위한 상태 업데이트
    %x0 = info.Xopt(2, :);
    vehiclePose = updateVehiclePose(modelStatesSub, tftree);

    a = vehiclePose';
    if mv(1) >= 0 
        plot(a(1),a(2),'r.', 'MarkerSize', 20);
    else
        plot(a(1),a(2),'g.', 'MarkerSize', 20);
    end
    drawnow; % 그래프를 즉시 업데이트
end

일단 이게 어제까지 작성한 코드입니다..

그러나 ref가 문제인지 자꾸 제자리에서 가만히 있으려고 해서 코드를 코치려고 합니다.

일단 nlobjTracking는 파라미터 변경 없이 코드의 전체적인 부분부터 수정을 보겠습니다.