# Trajectory Optimization and Control of Flying Robot Using Nonlinear MPC

This example shows how to find the optimal trajectory that brings a flying robot from one location to another with minimum fuel cost using a nonlinear MPC controller. In addition, another nonlinear MPC controller, along with an extended Kalman filter, drives the robot along the optimal trajectory in closed-loop simulation.

### Flying Robot

The flying robot in this example has four thrusters to move it around in a 2-D space. The model has six states:

• x(1) - `x` inertial coordinate of center of mass

• x(2) - `y` inertial coordinate of center of mass

• x(3) - `theta`, robot (thrust) direction

• x(4) - `vx`, velocity of x

• x(5) - `vy`, velocity of y

• x(6) - `omega`, angular velocity of theta For more information on the flying robot, see . The model in the paper uses two thrusts ranging from -1 to 1. However, this example assumes that there are four physical thrusts in the robot, ranging from 0 to 1, to achieve the same control freedom.

### Trajectory Planning

The robot initially rests at `[-10,-10]` with an orientation angle of `pi/2` radians (facing north). The flying maneuver for this example is to move and park the robot at the final location `[0,0]` with an angle of `0` radians (facing east) in `12` seconds. The goal is to find the optimal path such that the total amount of fuel consumed by the thrusters during the maneuver is minimized.

Nonlinear MPC is an ideal tool for trajectory planning problems because it solves an open-loop constrained nonlinear optimization problem given the current plant states. With the availability of a nonlinear dynamic model, MPC can make more accurate decisions.

In this example, the target prediction time is `12` seconds. Therefore, specify a sample time of `0.4` seconds and prediction horizon of `30` steps. Create a multistage nonlinear MPC object with `6` states and `4` inputs. By default, all the inputs are manipulated variables (MVs).

```Ts = 0.4; p = 30; nx = 6; nu = 4; nlobj = nlmpcMultistage(p,nx,nu); nlobj.Ts = Ts; ```

For a path planning problem, it is typical to allow MPC to have free moves at each prediction step, which provides the maximum number of decision variables for the optimization problem. Since planning usually runs at a much slower sampling rate than a feedback controller, the extra computation load introduced by a larger optimization problem can be accepted.

Specify the prediction model state function using the function name. You can also specify functions using a function handle. For details on the state function, open `FlyingRobotStateFcn.m`. For more information on specifying the prediction model, see Specify Prediction Model for Nonlinear MPC.

```nlobj.Model.StateFcn = "FlyingRobotStateFcn"; ```

Specify the Jacobian of the state function using a function handle. It is best practice to provide an analytical Jacobian for the prediction model. Doing so significantly improves simulation efficiency. For details on the Jacobian function, open `FlyingRobotStateJacobianFcn.m`.

```nlobj.Model.StateJacFcn = @FlyingRobotStateJacobianFcn; ```

A trajectory planning problem usually involves a nonlinear cost function, which can be used to find the shortest distance, the maximal profit, or as in this case, the minimal fuel consumption. Because the thrust value is a direct indicator of fuel consumption, compute the fuel cost as the sum of the thrust values at each prediction step from stage 1 to stage p. Specify this cost function using a named function. For more information on specifying cost functions, see Specify Cost Function for Nonlinear MPC.

```for ct = 1:p nlobj.Stages(ct).CostFcn = 'FlyingRobotCostFcn'; end ```

The goal of the maneuver is to park the robot at `[0,0]` with an angle of `0` radians at the 12th second. Specify this goal as the terminal state constraint, where every position and velocity state at the last prediction step (stage p+1) should be zero. For more information on specifying constraint functions, see Specify Constraints for Generic Nonlinear MPC.

```nlobj.Model.TerminalState = zeros(6,1); ```

It is best practice to provide analytical Jacobian functions for your stage cost and constraint functions as well. However, this example intentionally skips them so that their Jacobian is computed by the nonlinear MPC controller using the built-in numerical perturbation method.

Each thrust has an operating range between `0` and `1`, which is translated into lower and upper bounds on the MVs.

```for ct = 1:nu nlobj.MV(ct).Min = 0; nlobj.MV(ct).Max = 1; end ```

Specify the initial conditions for the robot.

```x0 = [-10;-10;pi/2;0;0;0]; % robot parks at [-10, -10], facing north u0 = zeros(nu,1); % thrust is zero ```

It is best practice to validate the user-provided model, cost, and constraint functions and their Jacobians. To do so, use the `validateFcns` command.

```validateFcns(nlobj,x0,u0); ```
```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 22 23 24 25 26 27 28 29 30] are OK. Analysis of user-provided model, cost, and constraint functions complete. ```

The optimal state and MV trajectories can be found by calling the `nlmpcmove` command once, given the current state `x0` and last MV `u0`. The optimal cost and trajectories are returned as part of the `info` output argument.

```[~,~,info] = nlmpcmove(nlobj,x0,u0); ```

Plot the optimal trajectory. The optimal cost is 7.8.

```FlyingRobotPlotPlanning(info,Ts); ```
```Optimal fuel consumption = 7.797825 ```   The first plot shows the optimal trajectory of the six robot states during the maneuver. The second plot shows the corresponding optimal MV profiles for the four thrusts. The third plot shows the X-Y position trajectory of the robot, moving from `[-10 -10 pi/2]` to `[0 0 0]`.

### Feedback Control for Path Following

After the optimal trajectory is found, a feedback controller is required to move the robot along the path. In theory, you can apply the optimal MV profile directly to the thrusters to implement feed-forward control. However, in practice, a feedback controller is needed to reject disturbances and compensate for modeling errors.

You can use different feedback control techniques for tracking. In this example, you use a generic nonlinear MPC controller to move the robot to the final location. In this path tracking problem, you track references for all six states (the number of outputs equals the number of states).

```ny = 6; nlobj_tracking = nlmpc(nx,ny,nu); ```
```In standard cost function, zero weights are applied by default to one or more OVs because there are fewer MVs than OVs. ```

Use the same state function and its Jacobian function.

```nlobj_tracking.Model.StateFcn = nlobj.Model.StateFcn; nlobj_tracking.Jacobian.StateFcn = nlobj.Model.StateJacFcn; ```

For tracking control applications, reduce the computational effort by specifying shorter prediction horizon (no need to look far into the future) and control horizon (for example, free moves are allocated at the first few prediction steps).

```nlobj_tracking.Ts = Ts; nlobj_tracking.PredictionHorizon = 10; nlobj_tracking.ControlHorizon = 4; ```

The default cost function in nonlinear MPC is a standard quadratic cost function suitable for reference tracking and disturbance rejection. For tracking, tracking error has higher priority (larger penalty weights on outputs) than control efforts (smaller penalty weights on MV rates).

```nlobj_tracking.Weights.ManipulatedVariablesRate = 0.2*ones(1,nu); nlobj_tracking.Weights.OutputVariables = 5*ones(1,nx); ```

Set the same bounds for the thruster inputs.

```for ct = 1:nu nlobj_tracking.MV(ct).Min = 0; nlobj_tracking.MV(ct).Max = 1; end ```

Also, to reduce fuel consumption, it is clear that `u1` and `u2` cannot be positive at any time during the operation. Therefore, implement equality constraints such that `u(1)*u(2)` must be `0` for all prediction steps. Apply similar constraints for `u3` and `u4`.

```nlobj_tracking.Optimization.CustomEqConFcn = ... @(X,U,data) [U(1:end-1,1).*U(1:end-1,2); U(1:end-1,3).*U(1:end-1,4)]; ```

Validate your prediction model and custom functions, and their Jacobians.

```validateFcns(nlobj_tracking,x0,u0); ```
```Model.StateFcn is OK. Jacobian.StateFcn is OK. No output function specified. Assuming "y = x" in the prediction model. Optimization.CustomEqConFcn is OK. Analysis of user-provided model, cost, and constraint functions complete. ```

### Nonlinear State Estimation

In this example, only the three position states (x, y and angle) are measured. The velocity states are unmeasured and must be estimated. Use an extended Kalman filter (EKF) from Control System Toolbox™ for nonlinear state estimation.

Because an EKF requires a discrete-time model, you use the trapezoidal rule to transition from x(k) to x(k+1), which requires the solution of `nx` nonlinear algebraic equations. For more information, open `FlyingRobotStateFcnDiscreteTime.m`.

```DStateFcn = @(xk,uk,Ts) FlyingRobotStateFcnDiscreteTime(xk,uk,Ts); ```

Measurement can help the EKF correct its state estimation. Only the first three states are measured.

```DMeasFcn = @(xk) xk(1:3); ```

Create the EKF, and indicate that the measurements have little noise.

```EKF = extendedKalmanFilter(DStateFcn,DMeasFcn,x0); EKF.MeasurementNoise = 0.01; ```

### Closed-Loop Simulation of Tracking Control

Simulate the system for `32` steps with correct initial conditions.

```Tsteps = 32; xHistory = x0'; uHistory = []; lastMV = zeros(nu,1); ```

The reference signals are the optimal state trajectories computed at the planning stage. When passing these trajectories to the nonlinear MPC controller, the current and future trajectory is available for previewing.

```Xopt = info.Xopt; Xref = [Xopt(2:p+1,:);repmat(Xopt(end,:),Tsteps-p,1)]; ```

Use `nlmpcmove` and `nlmpcmoveopt` command for closed-loop simulation.

```hbar = waitbar(0,'Simulation Progress'); options = nlmpcmoveopt; for k = 1:Tsteps % Obtain plant output measurements with sensor noise. yk = xHistory(k,1:3)' + randn*0.01; % Correct state estimation based on the measurements. xk = correct(EKF, yk); % Compute the control moves with reference previewing. [uk,options] = nlmpcmove(nlobj_tracking,xk,lastMV,Xref(k:min(k+9,Tsteps),:),[],options); % Predict the state for the next step. predict(EKF,uk,Ts); % Store the control move and update the last MV for the next step. uHistory(k,:) = uk'; %#ok<*SAGROW> lastMV = uk; % Update the real plant states for the next step by solving the % continuous-time ODEs based on current states xk and input uk. ODEFUN = @(t,xk) FlyingRobotStateFcn(xk,uk); [TOUT,YOUT] = ode45(ODEFUN,[0 Ts], xHistory(k,:)'); % Store the state values. xHistory(k+1,:) = YOUT(end,:); % Update the status bar. waitbar(k/Tsteps, hbar); end close(hbar) ```

Compare the planned and actual closed-loop trajectories.

```FlyingRobotPlotTracking(info,Ts,p,Tsteps,xHistory,uHistory); ```
```Actual fuel consumption = 10.706603 ```   The nonlinear MPC feedback controller successfully moves the robot (blue blocks), following the optimal trajectory (yellow blocks), and parks it at the final location (red block) in the last figure.

The actual fuel cost is higher than the planned cost. The main reason for this result is that, since we used shorter prediction and control horizons in the feedback controller, the control decision at each interval is suboptimal compared to the optimization problem used in the planning stage.

### Identify a Neual State Space Model for the Flying Robot System

In industrial applications, sometimes it is very difficult to manually derive a nonlinear state space dynamic model using first principles due to our lack of domain knowledge. One of the popular alternative approach is black-box modeling based on experiment data. In this section, we will showcase how to train a neural network to approximate the state function and then use it as the prediction model with nonlinear MPC.

The black-box model we want to identify is called "idNeuralStateSpace", a feature from System Identification Toolbox.

```if ~mpcchecktoolboxinstalled('ident') disp('System Identification Toolbox is required to run this example.') return end ```

The traing procedure is implemented in a supporting file `trainFlyingRobotNeuralStateSpaceModel`, which can be used a simple template and modified to fit your application. It takes about 10 minutes to complete, depending on your computer. To try it, change "ConductTraining = true" below.

```IsTraining = false; if IsTraining nss = trainFlyingRobotNeuralStateSpaceModel; else load nssModel.mat %#ok<*UNRCH> end ```

### Generate M files and Use them for Prediction

After idNeuralStateSpace system is train, we can automatically generate M files for the state function and its analytical Jacobian function by using the `generateMATLABFunction` command.

```nssStateFcnName = 'nssStateFcn'; generateMATLABFunction(nss,nssStateFcnName); ```

### Use the Neural Network Model as the Prediction Model in Nonlinear MPC

The generated M files are compatible with the interface required by nonlinear MPC objecy, and therefore, we can directly use them in the nonlinear MPC object

```nlobj_tracking.Model.StateFcn = nssStateFcnName; nlobj_tracking.Jacobian.StateFcn = [nssStateFcnName 'Jacobian']; ```

### Run Simulation again with the Neural State Space Prediction Model

Use `nlmpcmove` and `nlmpcmoveopt` command for closed-loop simulation.

```EKF = extendedKalmanFilter(DStateFcn,DMeasFcn,x0); xHistory = x0'; uHistory = []; lastMV = zeros(nu,1); hbar = waitbar(0,'Simulation Progress'); options = nlmpcmoveopt; for k = 1:Tsteps % Obtain plant output measurements with sensor noise. yk = xHistory(k,1:3)'; % Correct state estimation based on the measurements. xk = correct(EKF, yk); % Compute the control moves with reference previewing. [uk,options] = nlmpcmove(nlobj_tracking,xk,lastMV,Xref(k:min(k+9,Tsteps),:),[],options); % Predict the state for the next step. predict(EKF,uk,Ts); % Store the control move and update the last MV for the next step. uHistory(k,:) = uk'; lastMV = uk; % Update the real plant states for the next step by solving the % continuous-time ODEs based on current states xk and input uk. ODEFUN = @(t,xk) FlyingRobotStateFcn(xk,uk); [TOUT,YOUT] = ode45(ODEFUN,[0 Ts], xHistory(k,:)'); % Store the state values. xHistory(k+1,:) = YOUT(end,:); % Update the status bar. waitbar(k/Tsteps, hbar); end close(hbar) ```

Compare the planned and actual closed-loop trajectories. The response is close to what first-principle based prediction model produces.

```FlyingRobotPlotTracking(info,Ts,p,Tsteps,xHistory,uHistory); ```
```Actual fuel consumption = 16.503876 ```   ### References

 Y. Sakawa. "Trajectory planning of a free-flying robot by using the optimal control." Optimal Control Applications and Methods, Vol. 20, 1999, pp. 235-248.