| 30 | // ---------------------------------------------------------------------------- |
| 31 | |
| 32 | int main() |
| 33 | { |
| 34 | const int STATES = 4; |
| 35 | const int CONTROLS = 2; |
| 36 | |
| 37 | // The first thing we do is setup our vehicle dynamics model (A*x + B*u + C). |
| 38 | // Our state space (the x) will have 4 dimensions, the 2D vehicle position |
| 39 | // and also the 2D velocity. The control space (u) will be just 2 variables |
| 40 | // which encode the amount of force we apply to the vehicle along each axis. |
| 41 | // Therefore, the A matrix defines a simple constant velocity model. |
| 42 | matrix<double,STATES,STATES> A; |
| 43 | A = 1, 0, 1, 0, // next_pos = pos + velocity |
| 44 | 0, 1, 0, 1, // next_pos = pos + velocity |
| 45 | 0, 0, 1, 0, // next_velocity = velocity |
| 46 | 0, 0, 0, 1; // next_velocity = velocity |
| 47 | |
| 48 | // Here we say that the control variables effect only the velocity. That is, |
| 49 | // the control applies an acceleration to the vehicle. |
| 50 | matrix<double,STATES,CONTROLS> B; |
| 51 | B = 0, 0, |
| 52 | 0, 0, |
| 53 | 1, 0, |
| 54 | 0, 1; |
| 55 | |
| 56 | // Let's also say there is a small constant acceleration in one direction. |
| 57 | // This is the force of gravity in our model. |
| 58 | matrix<double,STATES,1> C; |
| 59 | C = 0, |
| 60 | 0, |
| 61 | 0, |
| 62 | 0.1; |
| 63 | |
| 64 | |
| 65 | const int HORIZON = 30; |
| 66 | // Now we need to setup some MPC specific parameters. To understand them, |
| 67 | // let's first talk about how MPC works. When the MPC tool finds the "best" |
| 68 | // control to apply it does it by simulating the process for HORIZON time |
| 69 | // steps and selecting the control that leads to the best performance over |
| 70 | // the next HORIZON steps. |
| 71 | // |
| 72 | // To be precise, each time you ask it for a control, it solves the |
| 73 | // following quadratic program: |
| 74 | // |
| 75 | // min sum_i trans(x_i-target_i)*Q*(x_i-target_i) + trans(u_i)*R*u_i |
| 76 | // x_i,u_i |
| 77 | // |
| 78 | // such that: x_0 == current_state |
| 79 | // x_{i+1} == A*x_i + B*u_i + C |
| 80 | // lower <= u_i <= upper |
| 81 | // 0 <= i < HORIZON |
| 82 | // |
| 83 | // and reports u_0 as the control you should take given that you are currently |
| 84 | // in current_state. Q and R are user supplied matrices that define how we |
| 85 | // penalize variations away from the target state as well as how much we want |
| 86 | // to avoid generating large control signals. We also allow you to specify |
| 87 | // upper and lower bound constraints on the controls. The next few lines |
| 88 | // define these parameters for our simple example. |
| 89 |
nothing calls this directly
no test coverage detected