MCPcopy Create free account
hub / github.com/davisking/dlib / main

Function main

examples/mpc_ex.cpp:32–153  ·  view source on GitHub ↗

Source from the content-addressed store, hash-verified

30// ----------------------------------------------------------------------------
31
32int 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

Callers

nothing calls this directly

Calls 10

assign_all_pixelsFunction · 0.85
rgb_pixelClass · 0.85
get_random_doubleMethod · 0.80
set_targetMethod · 0.80
transFunction · 0.50
pointClass · 0.50
draw_solid_circleFunction · 0.50
draw_lineFunction · 0.50
sleepFunction · 0.50
set_imageMethod · 0.45

Tested by

no test coverage detected