nlmpc를 활용한 주차 알고리즘

while ~isDestinationReached(currentPose, xRef(end,:),0.1) % 목적지에 도달할 때까지 루프
    % 현재 상태를 GPS 데이터로 업데이트
    currentPose = updateVehiclePose(modelStatesSub, tftree);
    x = [currentPose(1); currentPose(2); currentPose(3)];
    xk = x;
    onlinedata.ref = Xref(k:min(k+pTracking-1,Tsteps),:);
    [mv,onlinedata,info] = mexfcn(xk,mv,onlinedata);

    % 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);
    % 차량 상태 업데이트 (시뮬레이션 또는 실제 차량)
    while sqrt(sum((currentPose(1:2)'-xRef(k,1:2)).^2)) <0.5
        k = k+1
        plot(xRef(k,1),xRef(k,2),'k.', 'MarkerSize', 10);
    end
    % 다음 반복을 위한 상태 업데이트
    %x0 = info.Xopt(2, :);

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

이제 추종을 하러 가봅시다...

추종하는데 자꾸 오른쪽으로 가는 경향이 있어 목표에 차량 각도는 빼줬습니다.

자꾸 왼쪽으로 가서 차량 도달범위에 들어오지 못해 후진하는 경향이 있어 도달 범위를 넓혀줌

전후진을 반복해야 하는 구간에선 자꾸 이상한 동작을 반복함...

시뮬레이션이 느려서 그런건지 ts를 증가시켜봄

혼란스럽다....

후.....
다른팀을 보니 이전에 이 알고리즘 말고 다른 알고리즘을 사용하는 곳도 있네요 ㅎㅎ..