1.jpg2.jpg3.jpg

MPC Control of a differential drive unicycle

This is a live script which implements a MPC controller for a differential drive unicycle. There are various topic covered here:
close all;
clear;
clc
 
addpath(genpath(pwd))
import casadi.*
 
dt = 0.01;
T = 3; % Time horizon of the optimal control problem
N = round(T/dt,0); % Number of timesteps
Tk = 0.1; % Time at which a "kick" occurs
Nk = round(Tk/dt, 0); % Time step of the kick event
rob_diam = 0.3;
 
x_start = [0; 0; 0]; % Starting position
x_target = [1 ; 1; pi]; % Arrival
 
% Cost function weights
w_x = [1e2; 1e2; 1e1];
w_u = [1e0; 1e0];
 
% Robot parameters
d = 0.1; % Distance of the wheels 10 cm
r = 0.05; % wheel radius 5 cm

4.jpg5.jpgOCP solution

Here we solve an optimal control problem for the differential drive unicycle. We have to find the control action (velocities) that drive the system from the starting to the arrival configuration
x_des = repmat(x_target, [1, N+1]);
[x_opt, u_opt] = solve_ocp(r, d, x_start, x_des, dt, N, w_x, w_u); % Check the function for details
 
Here we integrate the control action. Why this is needed?
This function is kind of a "simulator". First of all usually when solving an OCP the timestep is quite large with respect to the one of a simulator. This function allows you to set a dt of the simulator smaller in order to have more accurate results. You may also want to use a different integration scheme and compare the results with the one you are using inside the ocp formulation.
The main point in this example though, is to show where the robot would be when it gets kicked. In the ocp formulation there is no kick, and the prediction of the state will be wrong in presence of disturbances.
x_int = integrate_unicycle(r, d, x_start, u_opt, dt, N, 0);
 
% Plot the optimal trajectory
plot(x_int(1,:), x_int(2, :))
hold on
plot(x_opt(1, :), x_opt(2, :), ['--', 'r'])
hold on
scatter(x_target(1), x_target(2), 'filled')
hold off
lgd = legend("Real", "Predicted", "Target");
lgd.Location = 'northwest';
title("Trajectories without disturbances")
Let's see an animation of the system
draw_unicycle(x_opt, x_des, rob_diam)
hold off
Open the animation Here
Now show the control actions
figure(3)
plot(u_opt(1, :))
hold on
plot(u_opt(2, :))
hold off
title("Control input")
legend("omega_l", "omega_r")

6.jpgProblem of Open loop trajectory tracking

With the last parameter set as 1 we are "kicking" the robot at a certain instant so we can see how the robot behaves
x_int = integrate_unicycle(r, d, x_start, u_opt, dt, N, Nk);
figure(4)
plot(x_int(1,:), x_int(2, :))
hold on
plot(x_opt(1, :), x_opt(2, :), '--r')
hold on
scatter(x_target(1), x_target(2), 'filled')
hold off
title("Trajectories with disturbances")
lgd = legend("Real", "Predicted", "Target");
lgd.Location = 'northwest';

7.jpgMPC

This run really slow. It can be improved A LOT, but for didactic purposes it's easier to understand
T_mpc = 3; % For how long we want to control the robot (in seconds)
N_mpc = round(T_mpc/dt,0);
 
x_mpc = zeros(3, N_mpc);
u_mpc = zeros(2, N_mpc);
 
for i=1:N_mpc
disp(strcat("Solving... iteration: ", int2str(i), " / ", int2str(N_mpc)))
if i == 1
x0 = x_start;
else
% In a real case here we wold put a sensor reading
x0 = x_mpc(:, i-1);
if i == Nk % Here we kick the robot
x0 = x0 + [0; 0.25; 0]; % this is the displacement that it gets
end
end
% Basically we are iteratively solving OCPs with different initial
% conditions
[x_s, u_s] = solve_ocp(r,d, x0, x_des, dt, N, w_x, w_u);
x_mpc(:, i) = x_s(:, 2);
u_mpc(:, i) = u_s(:, 1);
end
Solving... iteration: 1 / 300 Solving... iteration: 2 / 300 Solving... iteration: 3 / 300 Solving... iteration: 4 / 300 Solving... iteration: 5 / 300 Solving... iteration: 6 / 300 Solving... iteration: 7 / 300 Solving... iteration: 8 / 300 Solving... iteration: 9 / 300 Solving... iteration: 10 / 300 Solving... iteration: 11 / 300 Solving... iteration: 12 / 300 Solving... iteration: 13 / 300 Solving... iteration: 14 / 300 Solving... iteration: 15 / 300 Solving... iteration: 16 / 300 Solving... iteration: 17 / 300 Solving... iteration: 18 / 300 Solving... iteration: 19 / 300 Solving... iteration: 20 / 300 Solving... iteration: 21 / 300 Solving... iteration: 22 / 300 Solving... iteration: 23 / 300 Solving... iteration: 24 / 300 Solving... iteration: 25 / 300 Solving... iteration: 26 / 300 Solving... iteration: 27 / 300 Solving... iteration: 28 / 300 Solving... iteration: 29 / 300 Solving... iteration: 30 / 300 Solving... iteration: 31 / 300 Solving... iteration: 32 / 300 Solving... iteration: 33 / 300 Solving... iteration: 34 / 300 Solving... iteration: 35 / 300 Solving... iteration: 36 / 300 Solving... iteration: 37 / 300 Solving... iteration: 38 / 300 Solving... iteration: 39 / 300 Solving... iteration: 40 / 300 Solving... iteration: 41 / 300 Solving... iteration: 42 / 300 Solving... iteration: 43 / 300 Solving... iteration: 44 / 300 Solving... iteration: 45 / 300 Solving... iteration: 46 / 300 Solving... iteration: 47 / 300 Solving... iteration: 48 / 300 Solving... iteration: 49 / 300 Solving... iteration: 50 / 300 Solving... iteration: 51 / 300 Solving... iteration: 52 / 300 Solving... iteration: 53 / 300 Solving... iteration: 54 / 300 Solving... iteration: 55 / 300 Solving... iteration: 56 / 300 Solving... iteration: 57 / 300 Solving... iteration: 58 / 300 Solving... iteration: 59 / 300 Solving... iteration: 60 / 300 Solving... iteration: 61 / 300 Solving... iteration: 62 / 300 Solving... iteration: 63 / 300 Solving... iteration: 64 / 300 Solving... iteration: 65 / 300 Solving... iteration: 66 / 300 Solving... iteration: 67 / 300 Solving... iteration: 68 / 300 Solving... iteration: 69 / 300 Solving... iteration: 70 / 300 Solving... iteration: 71 / 300 Solving... iteration: 72 / 300 Solving... iteration: 73 / 300 Solving... iteration: 74 / 300 Solving... iteration: 75 / 300 Solving... iteration: 76 / 300 Solving... iteration: 77 / 300 Solving... iteration: 78 / 300 Solving... iteration: 79 / 300 Solving... iteration: 80 / 300 Solving... iteration: 81 / 300 Solving... iteration: 82 / 300 Solving... iteration: 83 / 300 Solving... iteration: 84 / 300 Solving... iteration: 85 / 300 Solving... iteration: 86 / 300 Solving... iteration: 87 / 300 Solving... iteration: 88 / 300 Solving... iteration: 89 / 300 Solving... iteration: 90 / 300 Solving... iteration: 91 / 300 Solving... iteration: 92 / 300 Solving... iteration: 93 / 300 Solving... iteration: 94 / 300 Solving... iteration: 95 / 300 Solving... iteration: 96 / 300 Solving... iteration: 97 / 300 Solving... iteration: 98 / 300 Solving... iteration: 99 / 300 Solving... iteration: 100 / 300 Solving... iteration: 101 / 300 Solving... iteration: 102 / 300 Solving... iteration: 103 / 300 Solving... iteration: 104 / 300 Solving... iteration: 105 / 300 Solving... iteration: 106 / 300 Solving... iteration: 107 / 300 Solving... iteration: 108 / 300 Solving... iteration: 109 / 300 Solving... iteration: 110 / 300 Solving... iteration: 111 / 300 Solving... iteration: 112 / 300 Solving... iteration: 113 / 300 Solving... iteration: 114 / 300 Solving... iteration: 115 / 300 Solving... iteration: 116 / 300 Solving... iteration: 117 / 300 Solving... iteration: 118 / 300 Solving... iteration: 119 / 300 Solving... iteration: 120 / 300 Solving... iteration: 121 / 300 Solving... iteration: 122 / 300 Solving... iteration: 123 / 300 Solving... iteration: 124 / 300 Solving... iteration: 125 / 300 Solving... iteration: 126 / 300 Solving... iteration: 127 / 300 Solving... iteration: 128 / 300 Solving... iteration: 129 / 300 Solving... iteration: 130 / 300 Solving... iteration: 131 / 300 Solving... iteration: 132 / 300 Solving... iteration: 133 / 300 Solving... iteration: 134 / 300 Solving... iteration: 135 / 300 Solving... iteration: 136 / 300 Solving... iteration: 137 / 300 Solving... iteration: 138 / 300 Solving... iteration: 139 / 300 Solving... iteration: 140 / 300 Solving... iteration: 141 / 300 Solving... iteration: 142 / 300 Solving... iteration: 143 / 300 Solving... iteration: 144 / 300 Solving... iteration: 145 / 300 Solving... iteration: 146 / 300 Solving... iteration: 147 / 300 Solving... iteration: 148 / 300 Solving... iteration: 149 / 300 Solving... iteration: 150 / 300 Solving... iteration: 151 / 300 Solving... iteration: 152 / 300 Solving... iteration: 153 / 300 Solving... iteration: 154 / 300 Solving... iteration: 155 / 300 Solving... iteration: 156 / 300 Solving... iteration: 157 / 300 Solving... iteration: 158 / 300 Solving... iteration: 159 / 300 Solving... iteration: 160 / 300 Solving... iteration: 161 / 300 Solving... iteration: 162 / 300 Solving... iteration: 163 / 300 Solving... iteration: 164 / 300 Solving... iteration: 165 / 300 Solving... iteration: 166 / 300 Solving... iteration: 167 / 300 Solving... iteration: 168 / 300 Solving... iteration: 169 / 300 Solving... iteration: 170 / 300 Solving... iteration: 171 / 300 Solving... iteration: 172 / 300 Solving... iteration: 173 / 300 Solving... iteration: 174 / 300 Solving... iteration: 175 / 300 Solving... iteration: 176 / 300 Solving... iteration: 177 / 300 Solving... iteration: 178 / 300 Solving... iteration: 179 / 300 Solving... iteration: 180 / 300 Solving... iteration: 181 / 300 Solving... iteration: 182 / 300 Solving... iteration: 183 / 300 Solving... iteration: 184 / 300 Solving... iteration: 185 / 300 Solving... iteration: 186 / 300 Solving... iteration: 187 / 300 Solving... iteration: 188 / 300 Solving... iteration: 189 / 300 Solving... iteration: 190 / 300 Solving... iteration: 191 / 300 Solving... iteration: 192 / 300 Solving... iteration: 193 / 300 Solving... iteration: 194 / 300 Solving... iteration: 195 / 300 Solving... iteration: 196 / 300 Solving... iteration: 197 / 300 Solving... iteration: 198 / 300 Solving... iteration: 199 / 300 Solving... iteration: 200 / 300 Solving... iteration: 201 / 300 Solving... iteration: 202 / 300 Solving... iteration: 203 / 300 Solving... iteration: 204 / 300 Solving... iteration: 205 / 300 Solving... iteration: 206 / 300 Solving... iteration: 207 / 300 Solving... iteration: 208 / 300 Solving... iteration: 209 / 300 Solving... iteration: 210 / 300 Solving... iteration: 211 / 300 Solving... iteration: 212 / 300 Solving... iteration: 213 / 300 Solving... iteration: 214 / 300 Solving... iteration: 215 / 300 Solving... iteration: 216 / 300 Solving... iteration: 217 / 300 Solving... iteration: 218 / 300 Solving... iteration: 219 / 300 Solving... iteration: 220 / 300 Solving... iteration: 221 / 300 Solving... iteration: 222 / 300 Solving... iteration: 223 / 300 Solving... iteration: 224 / 300 Solving... iteration: 225 / 300 Solving... iteration: 226 / 300 Solving... iteration: 227 / 300 Solving... iteration: 228 / 300 Solving... iteration: 229 / 300 Solving... iteration: 230 / 300 Solving... iteration: 231 / 300 Solving... iteration: 232 / 300 Solving... iteration: 233 / 300 Solving... iteration: 234 / 300 Solving... iteration: 235 / 300 Solving... iteration: 236 / 300 Solving... iteration: 237 / 300 Solving... iteration: 238 / 300 Solving... iteration: 239 / 300 Solving... iteration: 240 / 300 Solving... iteration: 241 / 300 Solving... iteration: 242 / 300 Solving... iteration: 243 / 300 Solving... iteration: 244 / 300 Solving... iteration: 245 / 300 Solving... iteration: 246 / 300 Solving... iteration: 247 / 300 Solving... iteration: 248 / 300 Solving... iteration: 249 / 300 Solving... iteration: 250 / 300 Solving... iteration: 251 / 300 Solving... iteration: 252 / 300 Solving... iteration: 253 / 300 Solving... iteration: 254 / 300 Solving... iteration: 255 / 300 Solving... iteration: 256 / 300 Solving... iteration: 257 / 300 Solving... iteration: 258 / 300 Solving... iteration: 259 / 300 Solving... iteration: 260 / 300 Solving... iteration: 261 / 300 Solving... iteration: 262 / 300 Solving... iteration: 263 / 300 Solving... iteration: 264 / 300 Solving... iteration: 265 / 300 Solving... iteration: 266 / 300 Solving... iteration: 267 / 300 Solving... iteration: 268 / 300 Solving... iteration: 269 / 300 Solving... iteration: 270 / 300 Solving... iteration: 271 / 300 Solving... iteration: 272 / 300 Solving... iteration: 273 / 300 Solving... iteration: 274 / 300 Solving... iteration: 275 / 300 Solving... iteration: 276 / 300 Solving... iteration: 277 / 300 Solving... iteration: 278 / 300 Solving... iteration: 279 / 300 Solving... iteration: 280 / 300 Solving... iteration: 281 / 300 Solving... iteration: 282 / 300 Solving... iteration: 283 / 300 Solving... iteration: 284 / 300 Solving... iteration: 285 / 300 Solving... iteration: 286 / 300 Solving... iteration: 287 / 300 Solving... iteration: 288 / 300 Solving... iteration: 289 / 300 Solving... iteration: 290 / 300 Solving... iteration: 291 / 300 Solving... iteration: 292 / 300 Solving... iteration: 293 / 300 Solving... iteration: 294 / 300 Solving... iteration: 295 / 300 Solving... iteration: 296 / 300 Solving... iteration: 297 / 300 Solving... iteration: 298 / 300 Solving... iteration: 299 / 300 Solving... iteration: 300 / 300
Now you can see that the kick is compensated
x_int_mpc = integrate_unicycle(r, d, x_start, u_mpc, dt, N_mpc, Nk);
figure()
plot(x_int_mpc(1,:), x_int_mpc(2, :))
hold on
plot(x_int(1, :), x_int(2, :), '--r')
hold on
scatter(x_target(1), x_target(2), 'filled')
hold off
lgd = legend("Closed loop (MPC)", "Open loop", "target");
lgd.Location='northwest';
% Display the movement
draw_unicycle(x_int_mpc, x_des, rob_diam)
hold off
Open the animation Here

Trajectory following

It is really the same as before, but in this case the reference is moving
% Define a Circle
a = 1; b=1; radius_tracking = 0.5;
theta0 = 0;
dtheta_des = 2; % Desired angular velocity
 
circle_des = circle(a, b, radius_tracking, theta0, dtheta_des, dt, N+1);
traj_d = circle_des;
traj_d(3, :) = circle_des(3, :) + pi/2;% sum 90 deg to have the tangent to the point
 
w_x = [1e2; 1e2; 1e1];
w_u = [3*1e-1; 3*1e-1];
x0 = [1; 1; 0];
Open loop trajectory tracking
[x_s, u_s] = solve_ocp(r, d, x0, traj_d, dt, N, w_x, w_u);
draw_unicycle(x_s, traj_d, rob_diam)
hold off
Open the animation Here
This has the same problem seen before. So let's close the loop with MPC
for i=1:N_mpc
disp(strcat("Solving... iteration: ", int2str(i), " / ", int2str(N_mpc)))
if i == 1
x0 = x0;
else
% In a real case here we wold put a sensor reading
x0 = x_mpc(:, i-1);
end
 
circle_des = circle(a, b, radius_tracking, circle_des(3, 2), dtheta_des, dt, N); % receding horizon
traj_d = circle_des;
traj_d(3, :) = circle_des(3, :) + pi/2; % sum 90 deg to have the tangent to the point
[x_s, u_s] = solve_ocp(r, d, x0, traj_d, dt, N, w_x, w_u);
x_mpc(:, i) = x_s(:, 2);
u_mpc(:, i) = u_s(:, 1);
 
end
Solving... iteration: 1 / 300 Solving... iteration: 2 / 300 Solving... iteration: 3 / 300 Solving... iteration: 4 / 300 Solving... iteration: 5 / 300 Solving... iteration: 6 / 300 Solving... iteration: 7 / 300 Solving... iteration: 8 / 300 Solving... iteration: 9 / 300 Solving... iteration: 10 / 300 Solving... iteration: 11 / 300 Solving... iteration: 12 / 300 Solving... iteration: 13 / 300 Solving... iteration: 14 / 300 Solving... iteration: 15 / 300 Solving... iteration: 16 / 300 Solving... iteration: 17 / 300 Solving... iteration: 18 / 300 Solving... iteration: 19 / 300 Solving... iteration: 20 / 300 Solving... iteration: 21 / 300 Solving... iteration: 22 / 300 Solving... iteration: 23 / 300 Solving... iteration: 24 / 300 Solving... iteration: 25 / 300 Solving... iteration: 26 / 300 Solving... iteration: 27 / 300 Solving... iteration: 28 / 300 Solving... iteration: 29 / 300 Solving... iteration: 30 / 300 Solving... iteration: 31 / 300 Solving... iteration: 32 / 300 Solving... iteration: 33 / 300 Solving... iteration: 34 / 300 Solving... iteration: 35 / 300 Solving... iteration: 36 / 300 Solving... iteration: 37 / 300 Solving... iteration: 38 / 300 Solving... iteration: 39 / 300 Solving... iteration: 40 / 300 Solving... iteration: 41 / 300 Solving... iteration: 42 / 300 Solving... iteration: 43 / 300 Solving... iteration: 44 / 300 Solving... iteration: 45 / 300 Solving... iteration: 46 / 300 Solving... iteration: 47 / 300 Solving... iteration: 48 / 300 Solving... iteration: 49 / 300 Solving... iteration: 50 / 300 Solving... iteration: 51 / 300 Solving... iteration: 52 / 300 Solving... iteration: 53 / 300 Solving... iteration: 54 / 300 Solving... iteration: 55 / 300 Solving... iteration: 56 / 300 Solving... iteration: 57 / 300 Solving... iteration: 58 / 300 Solving... iteration: 59 / 300 Solving... iteration: 60 / 300 Solving... iteration: 61 / 300 Solving... iteration: 62 / 300 Solving... iteration: 63 / 300 Solving... iteration: 64 / 300 Solving... iteration: 65 / 300 Solving... iteration: 66 / 300 Solving... iteration: 67 / 300 Solving... iteration: 68 / 300 Solving... iteration: 69 / 300 Solving... iteration: 70 / 300 Solving... iteration: 71 / 300 Solving... iteration: 72 / 300 Solving... iteration: 73 / 300 Solving... iteration: 74 / 300 Solving... iteration: 75 / 300 Solving... iteration: 76 / 300 Solving... iteration: 77 / 300 Solving... iteration: 78 / 300 Solving... iteration: 79 / 300 Solving... iteration: 80 / 300 Solving... iteration: 81 / 300 Solving... iteration: 82 / 300 Solving... iteration: 83 / 300 Solving... iteration: 84 / 300 Solving... iteration: 85 / 300 Solving... iteration: 86 / 300 Solving... iteration: 87 / 300 Solving... iteration: 88 / 300 Solving... iteration: 89 / 300 Solving... iteration: 90 / 300 Solving... iteration: 91 / 300 Solving... iteration: 92 / 300 Solving... iteration: 93 / 300 Solving... iteration: 94 / 300 Solving... iteration: 95 / 300 Solving... iteration: 96 / 300 Solving... iteration: 97 / 300 Solving... iteration: 98 / 300 Solving... iteration: 99 / 300 Solving... iteration: 100 / 300 Solving... iteration: 101 / 300 Solving... iteration: 102 / 300 Solving... iteration: 103 / 300 Solving... iteration: 104 / 300 Solving... iteration: 105 / 300 Solving... iteration: 106 / 300 Solving... iteration: 107 / 300 Solving... iteration: 108 / 300 Solving... iteration: 109 / 300 Solving... iteration: 110 / 300 Solving... iteration: 111 / 300 Solving... iteration: 112 / 300 Solving... iteration: 113 / 300 Solving... iteration: 114 / 300 Solving... iteration: 115 / 300 Solving... iteration: 116 / 300 Solving... iteration: 117 / 300 Solving... iteration: 118 / 300 Solving... iteration: 119 / 300 Solving... iteration: 120 / 300 Solving... iteration: 121 / 300 Solving... iteration: 122 / 300 Solving... iteration: 123 / 300 Solving... iteration: 124 / 300 Solving... iteration: 125 / 300 Solving... iteration: 126 / 300 Solving... iteration: 127 / 300 Solving... iteration: 128 / 300 Solving... iteration: 129 / 300 Solving... iteration: 130 / 300 Solving... iteration: 131 / 300 Solving... iteration: 132 / 300 Solving... iteration: 133 / 300 Solving... iteration: 134 / 300 Solving... iteration: 135 / 300 Solving... iteration: 136 / 300 Solving... iteration: 137 / 300 Solving... iteration: 138 / 300 Solving... iteration: 139 / 300 Solving... iteration: 140 / 300 Solving... iteration: 141 / 300 Solving... iteration: 142 / 300 Solving... iteration: 143 / 300 Solving... iteration: 144 / 300 Solving... iteration: 145 / 300 Solving... iteration: 146 / 300 Solving... iteration: 147 / 300 Solving... iteration: 148 / 300 Solving... iteration: 149 / 300 Solving... iteration: 150 / 300 Solving... iteration: 151 / 300 Solving... iteration: 152 / 300 Solving... iteration: 153 / 300 Solving... iteration: 154 / 300 Solving... iteration: 155 / 300 Solving... iteration: 156 / 300 Solving... iteration: 157 / 300 Solving... iteration: 158 / 300 Solving... iteration: 159 / 300 Solving... iteration: 160 / 300 Solving... iteration: 161 / 300 Solving... iteration: 162 / 300 Solving... iteration: 163 / 300 Solving... iteration: 164 / 300 Solving... iteration: 165 / 300 Solving... iteration: 166 / 300 Solving... iteration: 167 / 300 Solving... iteration: 168 / 300 Solving... iteration: 169 / 300 Solving... iteration: 170 / 300 Solving... iteration: 171 / 300 Solving... iteration: 172 / 300 Solving... iteration: 173 / 300 Solving... iteration: 174 / 300 Solving... iteration: 175 / 300 Solving... iteration: 176 / 300 Solving... iteration: 177 / 300 Solving... iteration: 178 / 300 Solving... iteration: 179 / 300 Solving... iteration: 180 / 300 Solving... iteration: 181 / 300 Solving... iteration: 182 / 300 Solving... iteration: 183 / 300 Solving... iteration: 184 / 300 Solving... iteration: 185 / 300 Solving... iteration: 186 / 300 Solving... iteration: 187 / 300 Solving... iteration: 188 / 300 Solving... iteration: 189 / 300 Solving... iteration: 190 / 300 Solving... iteration: 191 / 300 Solving... iteration: 192 / 300 Solving... iteration: 193 / 300 Solving... iteration: 194 / 300 Solving... iteration: 195 / 300 Solving... iteration: 196 / 300 Solving... iteration: 197 / 300 Solving... iteration: 198 / 300 Solving... iteration: 199 / 300 Solving... iteration: 200 / 300 Solving... iteration: 201 / 300 Solving... iteration: 202 / 300 Solving... iteration: 203 / 300 Solving... iteration: 204 / 300 Solving... iteration: 205 / 300 Solving... iteration: 206 / 300 Solving... iteration: 207 / 300 Solving... iteration: 208 / 300 Solving... iteration: 209 / 300 Solving... iteration: 210 / 300 Solving... iteration: 211 / 300 Solving... iteration: 212 / 300 Solving... iteration: 213 / 300 Solving... iteration: 214 / 300 Solving... iteration: 215 / 300 Solving... iteration: 216 / 300 Solving... iteration: 217 / 300 Solving... iteration: 218 / 300 Solving... iteration: 219 / 300 Solving... iteration: 220 / 300 Solving... iteration: 221 / 300 Solving... iteration: 222 / 300 Solving... iteration: 223 / 300 Solving... iteration: 224 / 300 Solving... iteration: 225 / 300 Solving... iteration: 226 / 300 Solving... iteration: 227 / 300 Solving... iteration: 228 / 300 Solving... iteration: 229 / 300 Solving... iteration: 230 / 300 Solving... iteration: 231 / 300 Solving... iteration: 232 / 300 Solving... iteration: 233 / 300 Solving... iteration: 234 / 300 Solving... iteration: 235 / 300 Solving... iteration: 236 / 300 Solving... iteration: 237 / 300 Solving... iteration: 238 / 300 Solving... iteration: 239 / 300 Solving... iteration: 240 / 300 Solving... iteration: 241 / 300 Solving... iteration: 242 / 300 Solving... iteration: 243 / 300 Solving... iteration: 244 / 300 Solving... iteration: 245 / 300 Solving... iteration: 246 / 300 Solving... iteration: 247 / 300 Solving... iteration: 248 / 300 Solving... iteration: 249 / 300 Solving... iteration: 250 / 300 Solving... iteration: 251 / 300 Solving... iteration: 252 / 300 Solving... iteration: 253 / 300 Solving... iteration: 254 / 300 Solving... iteration: 255 / 300 Solving... iteration: 256 / 300 Solving... iteration: 257 / 300 Solving... iteration: 258 / 300 Solving... iteration: 259 / 300 Solving... iteration: 260 / 300 Solving... iteration: 261 / 300 Solving... iteration: 262 / 300 Solving... iteration: 263 / 300 Solving... iteration: 264 / 300 Solving... iteration: 265 / 300 Solving... iteration: 266 / 300 Solving... iteration: 267 / 300 Solving... iteration: 268 / 300 Solving... iteration: 269 / 300 Solving... iteration: 270 / 300 Solving... iteration: 271 / 300 Solving... iteration: 272 / 300 Solving... iteration: 273 / 300 Solving... iteration: 274 / 300 Solving... iteration: 275 / 300 Solving... iteration: 276 / 300 Solving... iteration: 277 / 300 Solving... iteration: 278 / 300 Solving... iteration: 279 / 300 Solving... iteration: 280 / 300 Solving... iteration: 281 / 300 Solving... iteration: 282 / 300 Solving... iteration: 283 / 300 Solving... iteration: 284 / 300 Solving... iteration: 285 / 300 Solving... iteration: 286 / 300 Solving... iteration: 287 / 300 Solving... iteration: 288 / 300 Solving... iteration: 289 / 300 Solving... iteration: 290 / 300 Solving... iteration: 291 / 300 Solving... iteration: 292 / 300 Solving... iteration: 293 / 300 Solving... iteration: 294 / 300 Solving... iteration: 295 / 300 Solving... iteration: 296 / 300 Solving... iteration: 297 / 300 Solving... iteration: 298 / 300 Solving... iteration: 299 / 300 Solving... iteration: 300 / 300
Finally check the solution
x_int_mpc = integrate_unicycle(r, d, x_start, u_mpc, dt, N_mpc, 0);
figure()
plot(x_int_mpc(1,:), x_int_mpc(2, :))
axis equal
title("MPC trajectory tracking")
xlabel("x [m]")
ylabel('y [m]')
hold off