# Solve Fuel-Optimal Navigation Problem using C/GMRES

In this example, you address a specialized variant of the classic Zermelo's navigation problem using Multistage Nonlinear Model Predictive control.

Traditionally, Zermelo's navigation problem deals with determining the minimum time path for a boat moving from an initial location to a target destination across a watery expanse. The boat is capable of a certain maximum speed and is usually presented as a time-optimal control problem. The goal is to derive the best possible control to reach the destination B in the least possible time.

Use Multistage Nonlinear Model Predictive Control (NMPC) to determine the optimal path for a boat to reach a destination by minimizing the fuel expenditure, while being affected by a drift.

### Problem Formulation

Let's consider a two-dimensional case where the boat is moving in the xy-plane. The state of the boat is given by its position $\left(\mathit{x},y\right)$ and its velocity $\mathit{v}$. The control inputs are the velocity $\mathit{v}$ and the direction $\alpha$ of the boat. The drift is represented by a velocity vector $d\left(x,y\right)$, which depends on the position of the boat.

The dynamics of the boat can be represented by the following differential equations:

`$\stackrel{˙}{\mathit{x}}=\mathit{v}\cdot \mathrm{cos}\left(\alpha \right)+{\mathit{d}}_{\mathit{x}}\left(\mathit{x},\mathit{y}\right)$`

`$\stackrel{˙}{\mathit{y}}=\mathit{v}\cdot \mathrm{sin}\left(\alpha \right)+{\mathit{d}}_{\mathit{y}}\left(\mathit{x},\mathit{y}\right)$`

where ${\mathit{d}}_{\mathit{x}}$ and ${\mathit{d}}_{\mathit{y}}$ are the components of the drift vector $d\left(x,y\right)$.

For this example, the drift function is defined as $\mathit{d}\left(\mathit{x},\mathit{y}\right)=\left[\begin{array}{c}3+0.2\mathit{y}\cdot \left(1-\mathit{y}\right)\\ 0\end{array}\right]$.

```% Define the destination x_d = 10; y_d = 10; % Define the initial state x_0 = 0; y_0 = 0;```

### Setting Up Nonlinear MPC Controller

Create a nonlinear multistage MPC object with two states and two inputs.

```Ts = 1e-1;% Sampling time p = 20; % Prediction horizon nx = 2; % Number of states nmv = 2; % Number of manipulated (control) variables msobj = nlmpcMultistage(p, nx, nmv); msobj.Ts = Ts; ```

For this example, the model and stage functions are encapsulated within the `Navigation` class (which is defined in the suppporting file `Navigation.m`) as static methods. To call these static methods, use the class name followed by a dot (.), and then the method name. These methods provide the MPC controller with the necessary system dynamics and optimization criteria evaluations at each step within the prediction horizon.

```% Define the State and State Jacobian functions msobj.Model.StateFcn = 'Navigation.StateFcn'; msobj.Model.StateJacFcn = 'Navigation.StateJacFcn'; msobj.Model.ParameterLength = 1; % Define the Cost and Cost Jacobian functions [msobj.Stages.CostFcn] = deal('Navigation.CostFcn'); [msobj.Stages.CostJacFcn] = deal('Navigation.CostJacFcn'); [msobj.Stages.ParameterLength] = deal(3);```

Specify control input constraints for realistic and safe operation of the boat. The direction angle constraint ($0$ to $2\pi$ radians) ensures full navigational freedom within the boat's capability. The velocity constraint ($0$ to $1$ m/s) matches the boat propulsion limits, preventing commands exceeding its power.

```% Set constraints for the direction angle control input msobj.ManipulatedVariables(1).Min = 0; msobj.ManipulatedVariables(1).Max = 6.28; % Set constraints for the velocity control input msobj.ManipulatedVariables(2).Min = 0; msobj.ManipulatedVariables(2).Max = 1;```

For this example, use the Continuation/Generalized Minimum Residual (C/GMRES) solver to handle the optimization problem within the Multistage Model Predictive Control framework.

To configure the CGMRES solver for our MPC controller, define its properties as follows:

By setting these properties, you ensure that the MPC controller is equipped with a solver that is tuned for the specific requirements and dynamics of the application, including the ability to navigate with drift and avoid obstacles.

```% msobj.Optimization.Solver = 'cgmres'; % msobj.Optimization.SolverOptions.StabilizationParameter = 1/msobj.Ts; % msobj.Optimization.SolverOptions.Restart = 1; % msobj.Optimization.SolverOptions.MaxIterations = 20;```

The state and stage functions require state and stage parameters. Use `getSimulationData` to initialize data structure. For more information on simulation data structure for multistage MPC, see `getSimulationData`.

```xref = [10; 5]; obs = [2; 5; 2]; pvcost = [xref; p]; pvstate = 1; simdata = getSimulationData(msobj); simdata.StateFcnParameter = pvstate; simdata.StageParameter = repmat(pvcost, p+1, 1);```

Validate the model functions.

`validateFcns(msobj,rand(nx,1),rand(nmv,1),simdata)`
```Model.StateFcn is OK. Model.StateJacFcn is OK. "CostFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK. "CostJacFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK. Analysis of user-provided model, cost, and constraint functions complete. ```

### Closed-Loop Simulation in MATLAB

Simulate the system for 2 seconds with a target trajectory to follow.

```% Define the simulation duration in seconds. Duration = 15; % Initial state and control vectors. x0 = zeros(nx,1); u0 = zeros(nmv,1); % Preallocate history arrays states and control inputs, % for later plotting. xHistory1 = x0.'; % State history uHistory1 = u0.'; % Control input history % Initialize the control input for the first step. uk = u0; % Simulation loop to propagate the system dynamics % over the specified duration. for k = 1:(Duration/Ts) % Current state at iteration k. xk = xHistory1(k,:).'; % Compute the optimal control action using nlmpcmove. [uk, simdata, info] = nlmpcmove(msobj, xk, uk, simdata); % Simulate the system for the next control interval using ode45. odefun = @(t,xk) Navigation.StateFcn(xk,uk); [TOUT, XOUT] = ode45(odefun, [0 Ts], xk); % Log the states and control inputs for analysis and plotting. xHistory1(k+1,:) = XOUT(end,:); uHistory1(k+1,:) = uk; end % Time vector for plotting. time = 0:Ts:Duration;```

Plot the state and control input histories.

```figure; tiledlayout(2,2); ax = gobjects(4,1); ax(1) = nexttile; plot(time, xHistory1(:,1)); ax(2) = nexttile; plot(time, xHistory1(:,2)); ax(3) = nexttile; plot(time, uHistory1(:,1)); ax(4) = nexttile; plot(time, uHistory1(:,2)); title(ax(1), 'State 1: Position x'); title(ax(2), 'State 2: Position y'); title(ax(3), 'Control Input: Direction Angle'); title(ax(4), 'Control Input: Velocity'); xlabel(ax,'Time (s)'); ylabel(ax(1), 'Position x'); ylabel(ax(2), 'Position y'); ylabel(ax(3), 'Direction Angle (rad)'); ylabel(ax(4), 'Velocity (m/s)');```

Adjust the state dynamics of the boat to account for drift.

```msobj.Model.StateFcn = 'Navigation.StateFcnWithDrift'; msobj.Model.StateJacFcn = 'Navigation.StateJacFcnWithDrift'; simdata.InitialGuess = [];```

Simulate the system for 2 seconds with a target trajectory to follow.

```% Preallocate history arrays for storing states and control inputs for plotting. xHistory2 = x0.'; % State history uHistory2 = u0.'; % Control input history % Initialize the control input for the first step. uk = u0; % Simulation loop to propagate the system dynamics over the specified duration. for k = 1:(Duration/Ts) % Current state at iteration k. xk = xHistory2(k,:).'; % Compute the optimal control action using nlmpcmove. [uk, simdata, info] = nlmpcmove(msobj, xk, uk, simdata); % Simulate the system for the next control interval using ode45. odefun = @(t,xk) Navigation.StateFcnWithDrift(xk,uk); [TOUT, XOUT] = ode45(odefun, [0 Ts], xk); % Log the states and control inputs for analysis and plotting. xHistory2(k+1,:) = XOUT(end,:); uHistory2(k+1,:) = uk; end ```

Plot the state and control input histories.

```figure; tiledlayout(2,2); ax = gobjects(4,1); ax(1) = nexttile; plot(time, xHistory2(:,1)); ax(2) = nexttile; plot(time, xHistory2(:,2)); ax(3) = nexttile; plot(time, uHistory2(:,1)); ax(4) = nexttile; plot(time, uHistory2(:,2)); title(ax(1), 'State 1: Position x'); title(ax(2), 'State 2: Position y'); title(ax(3), 'Control Input: Direction Angle'); title(ax(4), 'Control Input: Velocity'); xlabel(ax,'Time (s)'); ylabel(ax(1), 'Position x'); ylabel(ax(2), 'Position y'); ylabel(ax(3), 'Direction Angle (rad)'); ylabel(ax(4), 'Velocity (m/s)');```

### Obstacle Avoidance

To enhance the capabilities of our MPC controller and ensure safe navigation, you can introduce obstacle avoidance feature.

For this example, the obstacle is modeled as a circle, and its presence is accounted for within the MPC framework using inequality constraints. These constraints are defined by the `Navigation.IneqConFcn` function, which calculates the distance between the boat's current position and the center of the obstacle; the boat is required to maintain a distance greater than the obstacle's radius to avoid collision.

Update the `msobj` object to incorporate obstacle avoidance into each stage of the MPC controller:

```[msobj.Stages.IneqConFcn] = deal('Navigation.IneqConFcn'); [msobj.Stages.IneqConJacFcn] = deal('Navigation.IneqConJacFcn'); [msobj.Stages.ParameterLength] = deal(6);```

By assigning `IneqConFcn` and `IneqConJacFcn` to all stages of the MPC controller, you ensure that the obstacle avoidance behavior is considered at every prediction horizon step. Here, `ParameterLength` is set to 6 to account for the additional parameters required by these functions, which include the radius and center of the obstacle. This update equips the MPC controller with the necessary tools to navigate safely and efficiently in environments with static obstacles.

```pvcost = [xref; p; obs]; pvstate = 1; simdata = getSimulationData(msobj); simdata.StateFcnParameter = pvstate; simdata.StageParameter = repmat(pvcost, p+1, 1); validateFcns(msobj,rand(nx,1),rand(nmv,1),simdata)```
```Model.StateFcn is OK. Model.StateJacFcn is OK. "CostFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK. "CostJacFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK. "IneqConFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK. "IneqConJacFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK. Analysis of user-provided model, cost, and constraint functions complete. ```

Perform a closed-loop simulation with obstacle avoidance.

```% Preallocate history arrays for storing states and control inputs % for later plotting. xHistory3 = x0.'; % State history uHistory3 = u0.'; % Control input history % Initialize the control input for the first step. uk = u0; % Simulation loop to propagate the system dynamics % over the specified duration. for k = 1:(Duration/Ts) % Current state at iteration k. xk = xHistory3(k,:).'; % Compute the optimal control action using nlmpcmove. [uk, simdata, info] = nlmpcmove(msobj, xk, uk, simdata); % Simulate the system for the next control interval using ode45. odefun = @(t,xk) Navigation.StateFcnWithDrift(xk,uk); [TOUT, XOUT] = ode45(odefun, [0 Ts], xk); % Log the states and control inputs for analysis and plotting. xHistory3(k+1,:) = XOUT(end,:); uHistory3(k+1,:) = uk; end ```

Plot the state and control input histories.

```figure; tiledlayout(2,2); ax = gobjects(4,1); ax(1) = nexttile; plot(time, xHistory3(:,1)); ax(2) = nexttile; plot(time, xHistory3(:,2)); ax(3) = nexttile; plot(time, uHistory3(:,1)); ax(4) = nexttile; plot(time, uHistory3(:,2)); title(ax(1), 'State 1: Position x'); title(ax(2), 'State 2: Position y'); title(ax(3), 'Control Input: Direction Angle'); title(ax(4), 'Control Input: Velocity'); xlabel(ax,'Time (s)'); ylabel(ax(1), 'Position x'); ylabel(ax(2), 'Position y'); ylabel(ax(3), 'Direction Angle (rad)'); ylabel(ax(4), 'Velocity (m/s)');```

Plot the trajectories with (red) and without (blue) obstacle avoidance. Use the helper function `plotDriftVelocityMagnitude` to create a contour plot of velocity magnitude with drift. Here, the magnitude of the velocity is represented using a color scale from blue to yellow.

```figure; plotDriftVelocityMagnitude(0,15,-2,7); hold on; axis equal plot(xHistory2(:,1), xHistory2(:,2), 'LineWidth', 2) plot(xHistory3(:,1), xHistory3(:,2), 'LineWidth', 2) theta = linspace(0, 2*pi); circle_x = obs(2) + obs(1)*cos(theta); circle_y = obs(3) + obs(1)*sin(theta); plot(circle_x, circle_y); rectangle( ... 'Position', [obs(2)-obs(1) obs(3)-obs(1) obs(1)*2 obs(1)*2], ... 'Curvature', 1, ... 'FaceColor','w')```