Create Path Following Model Predictive Controller

Since R2024a

Step 4 of 5 in Offroad Navigation for Autonomous Haul Trucks in Open Pit Mine

• 3
• 4
• 5

This example shows how to configure and simulate a multistage nonlinear Model Predictive Controller (MPC) to follow a reference path for an offroad autonomous vehicle. It builds on previous examples that break down the high-level planning problem into smaller tasks of global path planning in a road network (Create Route Planner for Offroad Navigation Using Digital Elevation Data), terrain-aware planning to enter and exit the road network (Create Onramp and Terrain-Aware Global Planners for Offroad Navigation), and obstacle-aware local planning (Navigate Global Path Through Offroad Terrain Using Local Planner). You can then use an MPC to follow your planned paths. In this example, you adapt the nlmpcMultistage (Model Predictive Control Toolbox) object for offroad driving. MPCs use a model of vehicle kinematics to predict vehicle movements and optimize control inputs like steering, throttle, and braking to achieve outcomes, such as following a path more closely or ensuring smoother driving. The MPC predicts and optimizes by minimizing a cost function, which quantifies how far the current strategy is from the desired outcome, to meet specific goals or criteria. Unlike a simpler control method like PID, which maintains consistent performance without real time adjustments, an MPC looks ahead to plan the best moves in advance, and adjusts its strategies in real time, ensuring that the vehicle responds optimally over a defined timeline. For more information about MPC control, see What Is Model Predictive Control? (Model Predictive Control Toolbox).

Set Up Multistage Nonlinear MPC

Traditional MPCs assume a linear system, in which any change to an input like steering or acceleration leads to a proportional change in outputs such as vehicle direction or speed. However in vehicle dynamics, this relationship is not linear. Changes in inputs can have complex effects on outputs due to factors like friction, aerodynamics, and the mechanical properties of the vehicle. Therefore, for this offroad navigation application, you must implement a multistage nonlinear MPC. A multistage nonlinear MPC can understand and manage the relationship between control inputs and their outputs in a nonlinear system like a vehicle. For more information about multistage nonlinear MPCs, see Nonlinear MPC (Model Predictive Control Toolbox).

Set the parameters for the multistage nonlinear MPC such that it runs at 10 hertz, with a prediction horizon of 1 second, enabling 10 control stages for the controller.

tsMPC = 0.1;
mpcHorizon = 1;
nStage = mpcHorizon/tsMPC;

The nlmpcMultistage (Model Predictive Control Toolbox) object enables you to generalize it to any plant and control model. Because the vehicle in this example is a three degree-of-freedom (DOF) wheeled vehicle, you can represent the system using a bicycle model. The state of the vehicle is $\left[xy\theta \right]$, and the vehicle control commands are velocity and steering angle $\left[v\alpha \right]$.

Create the multistage nonlinear MPC as an nlmpcMultistage object with 10 control stages, 3 states, and 2 control inputs.

mpcverbosity("off");
stateSize = 3;
controlSize = 2;
nlobjTracking = nlmpcMultistage(nStage,stateSize,controlSize);
nlobjTracking.Optimization.Solver = "fmincon";
nlobjTracking.Ts = tsMPC;

Define Vehicle Parameters

To represent the vehicle, use the vehicle parameters defined for the MPPI controller in the Navigate Global Path Through Offroad Terrain Using Local Planner example. Maintaining consistent values between the MPPI controller and MPC ensures that paths generated by the MPPI controller are feasible for the downstream MPC to follow.

Use the exampleHelperControllerParams helper function to define the ego vehicle parameters. The ego vehicle has two axles and four wheels.

addpath(genpath("Helpers"))
[tunableControllerParams,fixedControllerParams] = exampleHelperControllerParams;
vWheelBase = fixedControllerParams.Length;

Specify the prediction model function and its analytical Jacobian function in the controller object. Because the model requires one parameter vWheelBase, set Model.ParameterLength to 1.

nlobjTracking.Model.StateFcn = "exampleHelperBicycleDerivative_MultiStage";
nlobjTracking.Model.StateJacFcn = "exampleHelperBicycleJacobian_MultiStage";
nlobjTracking.Model.ParameterLength = 1;

The multistage nonlinear MPC manipulates two variables that represent your control variables. Each manipulated variable has two sets of limits. These limits specify the minimum and maximum allowable values and the minimum and maximum allowable rates of change for the variables. For ManipulatedVariables(1), these limits are on the linear velocity and the linear acceleration of the ego vehicle. For ManipulatedVariables(2), these limits are on the steering angle and the steering angle rate of change.

Set these limits for linear velocity and linear acceleration.

$$-8\text{m/s}\le \text{Linear}\text{Velocity}\le 16\text{m/s}$$

$$-0.6{\text{m/s}}^2\le \text{Linear}\text{Acceleration}\le 0.6{\text{m/s}}^2$$

nlobjTracking.UseMVRate = true;
nlobjTracking.ManipulatedVariables(1).Min = -tunableControllerParams.MaxReverseVelocity;
nlobjTracking.ManipulatedVariables(1).Max = tunableControllerParams.MaxVelocity(1);
nlobjTracking.ManipulatedVariables(1).RateMin = -tunableControllerParams.MaxAcceleration(1);
nlobjTracking.ManipulatedVariables(1).RateMax = tunableControllerParams.MaxAcceleration(1);

Set these limits for steering angle and the steering angle rate of change.

$$-\frac{\pi }{5}\text{rad}\le \text{Steering}\text{Angle}\le \frac{\pi }{5}\text{rad}$$

$-0.1\text{rad/s}\le \text{Steering}\text{Angle}\text{Rate}\text{of}\text{Change}\le 0.1\text{rad/s}$

nlobjTracking.ManipulatedVariables(2).Min = -tunableControllerParams.MaxSteeringAngle;
nlobjTracking.ManipulatedVariables(2).Max = tunableControllerParams.MaxSteeringAngle;
nlobjTracking.ManipulatedVariables(2).RateMin = -tunableControllerParams.MaxSteeringAngleRate;
nlobjTracking.ManipulatedVariables(2).RateMax = tunableControllerParams.MaxSteeringAngleRate;

Create a straight-line reference path for the multistage nonlinear MPC to follow, using the maximum allowable linear velocity to calculate the path increments over a specified number of points, ensuring the path adheres to the velocity constraints.

vNom = nlobjTracking.ManipulatedVariables(1).Max;
ds = tsMPC*vNom;
nPt = 100;
refPath = linspace(0,ds*nPt,nPt+1)'.*[1 0 0];

Define the cost function and the gradient function using the provided helper functions: exampleHelperBicycleCost_MultiStage and exampleHelperBicycleGradient_MultiStage.

The cost function is defined as

$J={\sum }_{k=1}^p(x_k-x_{re{f}_k}{)}^{\prime }{W}_x(x_k-x_{re{f}_k})+{\sum }_{k=1}^p{u}^{\prime }{W}_{u}u$,

where:

• $p$ is the prediction horizon.

• $k$ is the iteration index, in the range [1, $p$].

• $x_k$ and $x_{{\text{ref}}_k}$ are the current state and reference state vectors, respectively.

• $u$ is the control input vector.

• $W_x$ and $W_u$ are the weights on the states and control input vectors, respectively.

for ct = 1:nStage+1
    nlobjTracking.Stages(ct).CostFcn = "exampleHelperBicycleCost_MultiStage";
    nlobjTracking.Stages(ct).CostJacFcn = "exampleHelperBicycleGradient_MultiStage";
    nlobjTracking.Stages(ct).ParameterLength = 4;
end

Validate the nonlinear MPC design by simulating its performance with specific initial conditions and parameters, including vehicle dynamics and a reference path, to ensure it operates as expected for path tracking. The MPC uses a structured simulation data object to customize the validation process, incorporating vehicle wheelbase and path information.

simdata = getSimulationData(nlobjTracking,"TerminalState");
simdata.StateFcnParameter = vWheelBase;
simdata.StageParameter = reshape([refPath(1:(nStage+1),:)'; repmat(nStage,1,nStage+1)],[],1);

validateFcns(nlobjTracking,[-14.8 0 0],[0.1 0],simdata);

Extract MPC Reference Signal from Reference Path

At each time step in the prediction horizon, the MPC attempts to minimize its cost function. For this example, the cost function exampleHelperBicycleCost_MultiStage is consists of these parts:

$J={\sum }_{k=1}^p(x_k-x_{re{f}_k}{)}^{\prime }{W}_x(x_k-x_{re{f}_k})+{\sum }_{k=1}^p{u}^{\prime }{W}_{u}u$,

where:

• ${\left(x_k-x_{{\text{ref}}_k}\right)}^{\prime }{W}_x\left(x_k-x_{{\text{ref}}_k}\right)$ penalizes how far the current state is from a desired state at the current time step $k$.

• ${\underset{}{\overset{\dot{}}{u}}}^{\prime }{W}_u\underset{}{\overset{\dot{}}{u}}$ penalizes the rate of change in control input for the current time step $k$.

The nlmpcMultistage object provides $\underset{}{\overset{\dot{}}{u}}$, but requires you to provide the desired reference states to the MPC. Because upstream planners are unaware of downstream subsystems that use the reference path, you must incorporate logic to transform the reference path into a compatible format. First, define an optimal control sequence as one that both follows the path as closely as possible and conforms to velocity constraints. This approach relies on upstream planners to avoid collisions and fulfill the reachability criteria.

These two requirements mean that an optimal set of reference states is equivalent to a sequence of SE(2) states evenly spaced along the reference path, spacing determined by the time step (tsMPC) and optimal velocity (vNom).

arclengthInterval = tsMPC*vNom;

You must also ensure that the controller follows the reference path as closely as possible, including when transitioning from forward to reverse motion. Use these helper functions to ensure that the controller meets these requirements.

• exampleHelperFindPivots — Identifies the indices within an SE(2) path where the direction of motion transitions between forward and reverse.

• exampleHelperEvenlyInterpSegment — Fits a piecewise, semicontinuous set of arcs along a reference path, and samples new states at uniform arc lengths.

• exampleHelperGenerateMPCReferenceSegment — Finds the closest point in a path to the input state, selects the next nStage+1 states, appends MPC parameters, and reshapes the stage parameters as a vector for the MPC.

Follow Road Network with Only MPC

Having designed the necessary components, you can now use them together to follow paths on the road network.

Prepare Simulation Inputs

Start by loading a reference path, generated by the route planner in the Create Route Planner for Offroad Navigation Using Digital Elevation Data example, and prepare the visualization for the simulation.

exampleHelperCreateBehaviorModelInputs
f = figure;
f.Visible = "on";
cla
binMap = binaryOccupancyMap(worldMat);
show(binMap);
title("MPC Following Reference Path")
hold on
axis equal
curPose = smoothedReferencePath(1,:);
goalPose = smoothedReferencePath(300,:);
prevVel = [0 0];
curIdx = 1;
nPts = 50;
exampleHelperPlotLines(smoothedReferencePath,"k-");
hLocalPath = exampleHelperPlotLines(curPose,"r-");
hPivot = exampleHelperPlotLines(curPose,"mO");
hMPCRef = exampleHelperPose2Quiver(curPose,{'Color','b'},ArrowSize=0.1);
hMPCSoln = exampleHelperPose2Quiver(curPose,{'Color','g','LineWidth',2},ArrowSize=0.2);
fUpdateQuiv = @(f,poses)set(f,"XData",poses(:,1),"YData",poses(:,2),"UData",cos(poses(:,3)),"VData",sin(poses(:,3)));
scatter(goalPose(1),goalPose(2),"filled")
hTruckLoc = exampleHelperPose2Quiver(curPose,{'Color','k','LineWidth',2},ArrowSize=0.1);
legend({'Reference Path','Path Segment','Pivot Points','MPC Reference Path','MPC Solution','Goal','Truck Position'})

% Reset MPC initial guess
simdata.InitialGuess = [];

Simulate Path Following

To simulate following the road network with the MPC, follow these steps:

1. Simulate until the current position is within 5 meters of the goal position on the path.

2. Identify pivot points, and update the visualization for the chosen path segment.

3. Generate and follow reference paths for each segment, using the MPC to calculate optimal control commands.

4. Update the current pose and velocity based on the controller output, adjusting the path segment index as needed.

5. Repeat the process for subsequent segments until you achieve the target proximity.

while vecnorm(curPose(1:2)-goalPose(1:2),2,2) > 5

    % Get next segment of N points from reference path
    initRefPath = smoothedReferencePath(curIdx:curIdx+nPts,:);
    [segmentList,pivotIdx,nSeg] = exampleHelperFindPivots(initRefPath);

    % Update visualization with pivot points and initial reference path
    set(hPivot,"XData",initRefPath(pivotIdx,1),"YData",initRefPath(pivotIdx,2));
    set(hLocalPath,"XData",initRefPath(:,1),"YData",initRefPath(:,2));
    exampleHelperPaddedZoom(initRefPath,2,"PaddingFactor",1,"HalfSize",[50 50])

    % Iterate through each path segment to follow the path
    for iSeg = 1:nSeg
        segment = segmentList(iSeg).States;
        refPath = exampleHelperEvenlyInterpSegment(segment,vNom,nlobjTracking.Ts);
        fUpdateQuiv(hMPCRef,refPath)
        while vecnorm(curPose(1:2)-segment(end,1:2),2,2) > 1            
            % Generate reference states over prediction horizon
            simdata.StageParameter = exampleHelperGenerateMPCReferenceSegment(curPose,refPath,nlobjTracking.PredictionHorizon);

            % Step the MPC controller to get optimal velocity
            [curVel,simdata,info] = nlmpcmove(nlobjTracking,curPose,prevVel,simdata);
            fUpdateQuiv(hMPCSoln,info.Xopt)
            drawnow limitrate

            % Update pose and velocity states for next iteration
            curPose = info.Xopt(2,:);
            prevVel = curVel;
            set(hTruckLoc,"XData",curPose(1,1),"YData",curPose(1,2),"UData",cos(curPose(:,3)),"VData",sin(curPose(:,3)));
            if vecnorm(curPose(1:2)-goalPose(1:2),2,2) < 5
                break;
            end           
        end
    end

    % Advance current index for next loop iteration without exceeding path length
    curIdx = min(curIdx+50,size(smoothedReferencePath,1));
end
hold off

Note that the MPC occasionally strays from the reference path, particularly around sharp turns. Although this deviation does not cause issues in this case, such deviations can cause issues if the turns are close to obstacles. To mitigate this, introduce the offroadControllerMPPI local planner from the Navigate Global Path Through Offroad Terrain Using Local Planner example as a bridge between the route planner and the MPC. The MPPI planner generates paths that avoid collisions and respects the constraints of the kinematic model of the vehicle, making paths that are easier for the MPC to follow accurately.

Follow Optimized MPPI Paths with MPC

To incorporate the MPPI controller into the MPC path-following controller, first set up the MPPI controller and maps. Then, during simulation, you can use the MPPI controller to plan local optimal trajectories, generate velocity commands, and handle path-following issues such as deviating too much from the path.

Set Up Planners and Data

Load the data from the Create Route Planner for Offroad Navigation Using Digital Elevation Data and Create Onramp and Terrain-Aware Global Planners for Offroad Navigation examples. The first MAT file contains the digital elevation map and path list representing the road network. The second MAT file contains a planned reference path through the road network, a smoothed version of the planned reference path, and parameters for the terrain-aware offramp planner.

load("OpenPitMinePart1Data.mat","dem","pathList")
load("OpenPitMinePart2Data.mat","originalReferencePath", ...
     "smoothedReferencePath","fixedTerrainAwareParams","tuneableTerrainAwareParams")

Create cost maps from the digital elevation map. Convert obstacles to occupancyMap from binaryOccupancyMap since the offroadControllerMPPI runs faster with occupancyMap.

[costMap,maxSlope] = exampleHelperDem2mapLayers(dem,tuneableTerrainAwareParams.MaxAngle, ...
                                                    fixedTerrainAwareParams.Resolution);
obstacles = getLayer(costMap,"terrainObstacles");
obstaclesMap = occupancyMap(obstacles.occupancyMatrix, obstacles.Resolution);
obstaclesMap.GridLocationInWorld = obstacles.GridLocationInWorld;

Set the vehicle parameters, and inflate the road network.

vehDims = exampleHelperVehicleGeometry(fixedControllerParams.Length,fixedControllerParams.Width,"collisionChecker");
collisionChecker = inflationCollisionChecker(vehDims,3);
exampleHelperInflateRoadNetwork(obstaclesMap,pathList,collisionChecker.InflationRadius*1.5);

Compute the maximum distance traversable in an iteration of the MPPI controller, and create an egocentric local map based on that distance.

maxDistance = (tunableControllerParams.MaxVelocity(1)*tunableControllerParams.LookaheadTime);
localMap = occupancyMap(2*maxDistance,2*maxDistance,obstaclesMap.Resolution);
localMap.GridOriginInLocal = -localMap.GridSize([2 1])/(2*localMap.Resolution);

Define the haul truck kinematics. A simple bicycle kinematics model is used to represent the haul truck. Configure the vehicle-specific parameters in the model.

vehicleObj = bicycleKinematics("VehicleInputs","VehicleSpeedHeadingRate");
vehicleObj.WheelBase = vehDims.Wheelbase;
maxForwardVel = tunableControllerParams.MaxVelocity(1);
maxReverseVel = -tunableControllerParams.MaxReverseVelocity;
vehicleObj.VehicleSpeedRange = [maxReverseVel maxForwardVel]; % [min, max velocity]
vehicleObj.MaxSteeringAngle = tunableControllerParams.MaxSteeringAngle;

Create the MPPI controller and update the parameters.

mppi = offroadControllerMPPI(smoothedReferencePath,Map=localMap,VehicleModel=vehicleObj);
mppi.LookaheadTime = tunableControllerParams.LookaheadTime; % In seconds
mppi.NumTrajectory = tunableControllerParams.NumTrajectory;
mppi.Parameter.ObstacleSafetyMargin = tunableControllerParams.ObstacleSafetyMargin; % In meters
mppi.Parameter.CostWeight.PathFollowing = tunableControllerParams.PathFollowingCost;
mppi.Parameter.CostWeight.PathAlignment = tunableControllerParams.PathAlignmentCost;
mppi.Parameter.CostWeight.ObstacleRepulsion = tunableControllerParams.ObstacleRepulsion;
mppi.Parameter.VehicleCollisionInformation = struct("Dimension", [vehDims.Length, vehDims.Width], "Shape", "Rectangle");

Simulate Path Following

Initialize the simulation parameters, such as the current pose and the goal position.

cla
curPose = mppi.ReferencePath(1,:);
goalPose = smoothedReferencePath(300,:);
curVel = [0 0];
prevVel = [0 0];
adjustedPath = 0;
curIdx = 1;

Center and show the map around the current pose of the haul truck.

move(localMap,curPose(1:2));
syncWith(localMap,obstaclesMap);
show(localMap);
title("MPC Following MPPI Planned Paths")
hold on
axis equal

Plot the smoothed reference path and initialize the plots for the MPPI controller output path, the pivot points, the MPC reference path, and the output path from the MPC.

exampleHelperPlotLines(smoothedReferencePath,"k-");
hMPPI = exampleHelperPlotLines(curPose,{'r-.',});
hPivot = exampleHelperPlotLines(curPose,'mO');
hMPCRef = exampleHelperPose2Quiver(curPose,{'Color','b'},ArrowSize=0.1);
hMPCSoln = exampleHelperPose2Quiver(curPose,{'Color','g','LineWidth',2},ArrowSize=0.2);
fUpdateQuiv = @(f,poses)set(f,"XData",poses(:,1),"YData",poses(:,2),"UData",cos(poses(:,3)),"VData",sin(poses(:,3)));
scatter(goalPose(1),goalPose(2),"filled")
hTruckLoc = exampleHelperPose2Quiver(curPose,{'Color','k','LineWidth',2},ArrowSize=0.1);
legend(["Global Path","MPPI Path","Pivot Points","MPC RefPath","MPC Solution", "Goal", "Truck Position"])

Reset the initial guess of the MPC, and follow these steps to simulate MPC path following supplemented with the MPPI controller:

Step 1: Simulate until the current position of the haul truck is within 2 meters of the goal position.

Step 2: Split the reference path at direction changes (forward to backward or vice versa) to enable the haul truck to stop and proceed during a direction change in the reference path.

Step 3: Iterate over each of the reference path segments and assign it to MPPI.

Step 4: Use MPPI to find an optimal path for the MPC to follow. Simulate until the current position of the haul truck is near the end of the path segment.

Step 5: Use MPC to compute the optimal control commands for following the MPPI-planned path, updating the haul truck's pose and velocity iteratively.

Step 6: Invoke MPPI at a fixed rate to adapt to the dynamic environment and to generate a stable trajectory.

Step 7: Continue through the path segments until you achieve the target proximity to the goal.

simdata.InitialGuess = [];
mppiReplanInterval = 0.5; %Invoke mppi at every 0.5 second.
mppiStepsPerCall = mppiReplanInterval/tsMPC; % Find the loop iteration at which to invoke mppi.
iterationCount = 0;

% STEP 1 - Simulate until the current position of the haul truck is within 2 meters of the goal position.
while vecnorm(curPose(1:2)-goalPose(1:2),2,2) > 2       
    % STEP 2 - Split the reference path at direction changes (forward to backward or vice versa) to enable 
    % the haul truck to stop and proceed during a direction change in the reference path
    % Split the reference path at direction changes (forward to backward or vice versa).
    initRefPath = smoothedReferencePath;
    [segmentList, pivotIdx, nSeg] = exampleHelperFindPivots(initRefPath);
    simdata.InitialGuess = zeros(nlobjTracking.PredictionHorizon*7+3,1);
    
    % Update visualization with pivot points    
    set(hPivot,"XData",initRefPath(pivotIdx,1),"YData",initRefPath(pivotIdx,2));
       
    % STEP 3 - Iterate over each of the reference path segments and assign it to MPPI.
    % Iterate through each path segment to follow the path
    goalReached = false;
    for iSeg = 1:nSeg    
        segment = segmentList(iSeg).States;
        mppi.ReferencePath = segment;
        
        % STEP 4 - Use MPPI to find an optimal path for MPC to follow. Simulate until the current position
        %  of the haul truck is near the end of the path segment.
        while vecnorm(curPose(1:2)-segment(end,1:2),2,2) > 1.5            
            % Update the position of the local map, and synchronize with
            % obstacles for MPPI to follow the reference path
            move(localMap, curPose(1:2),MoveType="absolute", SyncWith=obstaclesMap);
            
            % Find the local path using MPPI
            [velcmds, curPath, mppiInfo] = mppi(curPose(:)',curVel(:)');
            
            % Update visualization with MPPI path
            set(hMPPI,"XData",curPath(:,1),"YData",curPath(:,2));
            
            % STEP 5 - Use MPC to compute the optimal control commands for following the MPPI-planned path, 
            % updating the haul truck's pose and velocity iteratively.
            while vecnorm(curPose(1:2)-curPath(end,1:2),2,2) >.3
                refPath = exampleHelperEvenlyInterpSegment(curPath, vNom, nlobjTracking.Ts, velcmds);
                
                % If the reference path only contains a single state after interpolation
                % assign the reference path with current pose and end pose of the MPPI path.
                if size(refPath,1) == 1
                    refPath = [curPose; curPath(end,:)];
                end
                fUpdateQuiv(hMPCRef,refPath);
                
                % Generate reference states over prediction horizon
                [mpcRef, idx] = exampleHelperGenerateMPCReferenceSegment(curPose, refPath, nlobjTracking.PredictionHorizon);
                simdata.StageParameter = mpcRef;

                % Step the MPC controller to get optimal velocity
                [curVel, simdata, info] = nlmpcmove(nlobjTracking, curPose,prevVel, simdata);
                fUpdateQuiv(hMPCSoln,info.Xopt);

                % Update pose and velocity states for next iteration
                curPose = info.Xopt(2,:);
                prevVel = curVel;
                show(localMap, FastUpdate=1);
                set(hTruckLoc,"XData",curPose(1,1),"YData",curPose(1,2),"UData",cos(curPose(:,3)),"VData",sin(curPose(:,3)));
                drawnow

                % Stop the simulation when the haul truck reaches the goal.
                if vecnorm(curPose(1:2)-goalPose(1:2),2,2) < 1
                    goalReached = true;
                    break;
                end
                
                % STEP 6 - Invoke MPPI at a fixed rate to adapt to the dynamic environment and to generate a stable trajectory.
                % Trigger replanning of the MPPI local planner at every 0.5 seconds.
                iterationCount = iterationCount + 1;
                if mod(iterationCount, mppiStepsPerCall) == 0
                    break;
                end
            end
            if goalReached
                break;
            end
        end       
    end
end

rmpath(genpath("Helpers"))

Conclusion

In this example, you developed a multistage nonlinear MPC for a three-DOF vehicle. You learned how to configure an nlmpcMultistage object for the three-DOF plant, and how to design state-update and cost functions to enable the haul truck to accurately track a reference path. You also learned how to design a reference signal such that the vehicle can optimally move along a reference path. Lastly, you saw how you can design an MPC to work with different upstream subsystems, such as a simple route planner or MPPI controller.

Next, in the Model and Control Autonomous Vehicle in Offroad Scenario example, you will integrate all of the subsystems from this example using Simulink® and Stateflow®, creating an integrated model to serve as the planning stack for an autonomous haul truck in a pit mine.