1 | clc |
---|
2 | echo on |
---|
3 | clc |
---|
4 | echo on |
---|
5 | %********************************************************* |
---|
6 | % |
---|
7 | % Model predictive control example |
---|
8 | % |
---|
9 | %********************************************************* |
---|
10 | % |
---|
11 | % In this example, we solve quadratic problems using |
---|
12 | % semidefinite programming, second order cone programming |
---|
13 | % and quadratic programming. |
---|
14 | |
---|
15 | % MPC settings |
---|
16 | N = 5; |
---|
17 | pause % Strike any key to continue. |
---|
18 | |
---|
19 | % Create numerics for a discretized double integrator |
---|
20 | A = [2 -1;1 0]; |
---|
21 | B = [1;0]; |
---|
22 | C = [0.5 0.5]; |
---|
23 | |
---|
24 | [H,S] = create_CHS(A,B,C,N) |
---|
25 | pause % Strike any key to continue. |
---|
26 | |
---|
27 | % Initial state |
---|
28 | x = [2;0]; |
---|
29 | |
---|
30 | % Define free variables |
---|
31 | t = sdpvar(1,1); |
---|
32 | U = sdpvar(N,1); |
---|
33 | pause % Strike any key to continue. |
---|
34 | |
---|
35 | % Define the prediction vector |
---|
36 | Y = H*x+S*U; |
---|
37 | pause % Strike any key to continue. |
---|
38 | |
---|
39 | % Control constraints |
---|
40 | F = set(-1 < U < 1); |
---|
41 | |
---|
42 | % Terminal constraint |
---|
43 | F = F+set(-1 < Y(N) < 1); |
---|
44 | pause % Strike any key to continue. |
---|
45 | |
---|
46 | % Our goal is to minimize the quadratic function Y'*Y+U'*U |
---|
47 | % |
---|
48 | % Performance constraint written as an SDP using Schur complement |
---|
49 | % (very inefficient way to solve a QP...) |
---|
50 | F = F+set([t Y' U';Y eye(N) zeros(N,N);U zeros(N,N) eye(N)]>0) |
---|
51 | pause % Strike any key to continue. |
---|
52 | |
---|
53 | % Solve |
---|
54 | sol = solvesdp(F,t); |
---|
55 | pause % Strike any key to continue. |
---|
56 | |
---|
57 | % Look at solution |
---|
58 | double(U) |
---|
59 | double(Y(N)) |
---|
60 | pause % Strike any key to continue. |
---|
61 | clc |
---|
62 | % More efficient implementation using SOCP... (if SOCP solver available) |
---|
63 | F = set(-1 < U < 1) + set(-1 < Y(N) < 1) + set(cone([Y;U],t)); |
---|
64 | sol = solvesdp(F,t); |
---|
65 | pause % Strike any key to continue. |
---|
66 | clc |
---|
67 | |
---|
68 | % Even more efficient implementation if QP solver is available |
---|
69 | F = set(-1 < U < 1) + set(-1 < Y(N) < 1); |
---|
70 | sol = solvesdp(F,Y'*Y+U'*U); |
---|
71 | pause % Strike any key to continue. |
---|
72 | |
---|
73 | |
---|
74 | echo off |
---|