Added initial version of model predictive control example program.

examples/CMakeLists.txt

@@ -71,6 +71,7 @@ add_example(max_cost_assignment_ex)
 add_example(member_function_pointer_ex)
 add_example(mlp_ex)
 add_example(model_selection_ex)
+add_example(mpc_ex)
 add_example(multiclass_classification_ex)
 add_example(multithreaded_object_ex)
 add_example(object_detector_advanced_ex)

examples/mpc_ex.cpp

@@ -0,0 +1,89 @@
+// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
+/*
+
+*/
+
+
+
+#include <dlib/gui_widgets.h>
+#include <dlib/control.h>
+#include <dlib/image_transforms.h>
+
+
+using namespace std;
+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;
+
+    matrix<double,4,2> B;
+    B = 0, 0,
+        0, 0,
+        1, 0,
+        0, 1;
+
+    matrix<double,4,1> C;
+    C = 0,
+        0,
+        0,
+        0.1;
+
+    matrix<double,4,1> Q;
+    Q = 1, 1, 0, 0;
+
+    matrix<double,2,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);
+
+    dlib::rand rnd;
+    matrix<double,4,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;
+
+    matrix<rgb_pixel> world(400,400);
+    image_window win;
+
+    int iter = 0;
+    while(!win.is_closed())
+    {
+        matrix<double,2,1> action = controller(current_state);
+
+        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);
+        draw_line(world, pos, pos-50*action, rgb_pixel(255,0,0));
+
+        current_state = A*current_state + B*action + C;
+        win.set_image(world);
+        dlib::sleep(100);
+
+        // Every 100 iterations change the target to some other random location. 
+        ++iter;
+        if (iter > 100)
+        {
+            iter = 0;
+            target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
+            controller.set_target(target);
+        }
+    }
+}
+
+//  ----------------------------------------------------------------------------
+