[37] | 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 |
---|