fleshed out this example with comments

examples/mpc_ex.cpp

@@ -1,8 +1,22 @@
 // The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
 /*
 
-*/
+    This is an example illustrating the use of the linear model predictive
+    control tool from the dlib C++ Library.  To explain what it does, suppose
+    you have some process you want to control and the process dynamics are
+    described by the linear equation:
+        x_{i+1} = A*x_i + B*u_i + C
+    That is, the next state the system goes into is a linear function of its
+    current state (x_i) and the current control (u_i) plus some constant bias or
+    disturbance.  
                 
+    A model predictive controller can find the control (u) you should apply to
+    drive the state (x) to some reference value, which is what we show in this
+    example.  In particular, we will simulate a simple vehicle moving around in
+    a planet's gravity.  We will use MPC to get the vehicle to fly to and then
+    hover at a certain point in the air.
+    
+*/
 
 
 #include <dlib/gui_widgets.h>
@@ -17,61 +31,114 @@ using namespace dlib;
 
 int main()
 {
-    // state is x, y, x_vel, y_vel
-    matrix<double,4,4> A;
-    A = 1, 0, 1, 0,
-        0, 1, 0, 1,
-        0, 0, 1, 0,
-        0, 0, 0, 1;
+    const int STATES = 4;
+    const int CONTROLS = 2;
 
-    matrix<double,4,2> B;
+    // The first thing we do is setup our vehicle dynamics model (A*x + B*u + C).
+    // Our state space (the x) will have 4 dimensions, the 2D vehicle position
+    // and also the 2D velocity.  The control space (u) will be just 2 variables
+    // which encode the amount of force we apply to the vehicle along each axis.
+    // Therefore, the A matrix defines a simple constant velocity model.
+    matrix<double,STATES,STATES> A;
+    A = 1, 0, 1, 0,  // next_pos = pos + velocity
+        0, 1, 0, 1,  // next_pos = pos + velocity
+        0, 0, 1, 0,  // next_velocity = velocity
+        0, 0, 0, 1;  // next_velocity = velocity
+
+    // Here we say that the control variables effect only the velocity. That is,
+    // the control applies an acceleration to the vehicle.
+    matrix<double,STATES,CONTROLS> B;
     B = 0, 0,
         0, 0,
         1, 0,
         0, 1;
 
-    matrix<double,4,1> C;
+    // Let's also say there is a small constant acceleration in one direction.
+    // This is the force of gravity in our model. 
+    matrix<double,STATES,1> C;
     C = 0,
         0,
         0,
         0.1;
 
-    matrix<double,4,1> Q;
+
+    const int HORIZON = 30;
+    // Now we need to setup some MPC specific parameters.  To understand them,
+    // let's first talk about how MPC works.  When the MPC tool finds the "best"
+    // control to apply it does it by simulating the process for HORIZON time
+    // steps and selecting the control that leads to the best performance over
+    // the next HORIZON steps.
+    //  
+    // To be precise, each time you ask it for a control, it solves the
+    // following quadratic program:
+    //   
+    //     min     sum_i trans(x_i-target_i)*Q*(x_i-target_i) + trans(u_i)*R*u_i 
+    //    x_i,u_i
+    //
+    //     such that: x_0     == current_state 
+    //                x_{i+1} == A*x_i + B*u_i + C
+    //                lower <= u_i <= upper
+    //                0 <= i < HORIZON
+    //
+    // and reports u_0 as the control you should take given that you are currently
+    // in current_state.  Q and R are user supplied matrices that define how we
+    // penalize variations away from the target state as well as how much we want
+    // to avoid generating large control signals.  We also allow you to specify
+    // upper and lower bound constraints on the controls.  The next few lines
+    // define these parameters for our simple example.
+
+    matrix<double,STATES,1> Q;
+    // Setup Q so that the MPC only cares about matching the target position and
+    // ignores the velocity.  
     Q = 1, 1, 0, 0;
 
-    matrix<double,2,1> R, lower, upper;
+    matrix<double,CONTROLS,1> R, lower, upper;
     R = 1, 1;
     lower = -0.5, -0.5;
     upper =  0.5,  0.5;
 
-    mpc<4,2,30> controller(A,B,C,Q,R,lower,upper);
+    // Finally, create the MPC controller.
+    mpc<STATES,CONTROLS,HORIZON> controller(A,B,C,Q,R,lower,upper);
 
+
+    // Let's tell the controller to send our vehicle to a random location.  It
+    // will try to find the controls that makes the vehicle just hover at this
+    // target position.
     dlib::rand rnd;
-    matrix<double,4,1> target;
+    matrix<double,STATES,1> target;
     target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
     controller.set_target(target);
 
 
-    matrix<double,4,1> current_state;
-    current_state = 200,200,0,0;
-
+    // Now let's start simulating our vehicle.  Our vehicle moves around inside
+    // a 400x400 unit sized world.
     matrix<rgb_pixel> world(400,400);
     image_window win;
+    matrix<double,STATES,1> current_state;
+    // And we start it at the center of the world with zero velocity.
+    current_state = 200,200,0,0;
 
     int iter = 0;
     while(!win.is_closed())
     {
-        matrix<double,2,1> action = controller(current_state);
+        // Find the best control action given our current state.
+        matrix<double,CONTROLS,1> action = controller(current_state);
+        cout << "best control: " << trans(action);
 
+        // Now draw our vehicle on the world.  We will draw the vehicle as a
+        // black circle and its target position as a green circle.  
         assign_all_pixels(world, rgb_pixel(255,255,255));
         const dpoint pos = point(current_state(0),current_state(1));
         const dpoint goal = point(target(0),target(1));
         draw_solid_circle(world, goal, 9, rgb_pixel(100,255,100));
         draw_solid_circle(world, pos, 7, 0);
+        // We will also draw the control as a line showing which direction the
+        // vehicle's thruster is firing.
         draw_line(world, pos, pos-50*action, rgb_pixel(255,0,0));
-
-        current_state = A*current_state + B*action + C;
         win.set_image(world);
+
+        // Take a step in the simulation
+        current_state = A*current_state + B*action + C;
         dlib::sleep(100);
 
         // Every 100 iterations change the target to some other random location.