Cleaned up this code, filled out spec, added asserts.

dlib/control/mpc.h

@@ -27,6 +27,14 @@ namespace dlib
         mpc(
         ) 
         {
+            A = 0;
+            B = 0;
+            C = 0;
+            Q = 0;
+            R = 0;
+            lower = 0;
+            upper = 0;
+
             max_iterations = 0;
             eps = 0.01;
             for (unsigned long i = 0; i < horizon; ++i)
@@ -50,6 +58,48 @@ namespace dlib
             const matrix<double,I,1>& upper_
         ) : A(A_), B(B_), C(C_), Q(Q_), R(R_), lower(lower_), upper(upper_)
         {
+            // make sure requires clause is not broken
+            DLIB_ASSERT(A.nr() > 0 && B.nc() > 0,
+                "\t mpc::mpc()"
+                << "\n\t invalid inputs were given to this function"
+                << "\n\t A.nr(): " <<  A.nr()
+                << "\n\t B.nc(): " <<  B.nc()
+                );
+
+            DLIB_ASSERT(A.nr() == A.nc() && 
+                        A.nr() == B.nr() && 
+                        A.nr() == C.nr() && 
+                        A.nr() == Q.nr(),
+                "\t mpc::mpc()"
+                << "\n\t invalid inputs were given to this function"
+                << "\n\t A.nr(): " <<  A.nr()
+                << "\n\t A.nc(): " <<  A.nc()
+                << "\n\t B.nr(): " <<  B.nr()
+                << "\n\t C.nr(): " <<  C.nr()
+                << "\n\t Q.nr(): " <<  Q.nr()
+                );
+            DLIB_ASSERT(
+                        B.nc() == R.nr() && 
+                        B.nc() == lower.nr() && 
+                        B.nc() == upper.nr() ,
+                "\t mpc::mpc()"
+                << "\n\t invalid inputs were given to this function"
+                << "\n\t B.nr(): " <<  B.nr()
+                << "\n\t B.nc(): " <<  B.nc()
+                << "\n\t lower.nr(): " <<  lower.nr()
+                << "\n\t upper.nr(): " <<  upper.nr()
+                );
+            DLIB_ASSERT(min(Q) >= 0 &&
+                        min(R) >  0 &&
+                        min(upper-lower) >= 0,
+                "\t mpc::mpc()"
+                << "\n\t invalid inputs were given to this function"
+                << "\n\t min(Q): " << min(Q) 
+                << "\n\t min(R): " << min(R) 
+                << "\n\t min(upper-lower): " << min(upper-lower) 
+                );
+
+
             max_iterations = 10000;
             eps = 0.01;
             for (unsigned long i = 0; i < horizon; ++i)
@@ -93,6 +143,13 @@ namespace dlib
             const unsigned long time
         )
         {
+            DLIB_ASSERT(time < horizon,
+                "\t void mpc::set_target(eps_)"
+                << "\n\t invalid inputs were given to this function"
+                << "\n\t time: " << time 
+                << "\n\t horizon: " << horizon 
+                );
+
             target[time] = val;
         }
 
@@ -107,6 +164,14 @@ namespace dlib
             const unsigned long time
         ) const
         {
+            // make sure requires clause is not broken
+            DLIB_ASSERT(time < horizon,
+                "\t matrix mpc::get_target(eps_)"
+                << "\n\t invalid inputs were given to this function"
+                << "\n\t time: " << time 
+                << "\n\t horizon: " << horizon 
+                );
+
             return target[time];
         }
 
@@ -126,7 +191,7 @@ namespace dlib
         {
             // make sure requires clause is not broken
             DLIB_ASSERT(eps_ > 0,
-                "\tvoid mpc::set_epsilon(eps_)"
+                "\t void mpc::set_epsilon(eps_)"
                 << "\n\t invalid inputs were given to this function"
                 << "\n\t eps_: " << eps_ 
                 );
@@ -143,6 +208,15 @@ namespace dlib
             const matrix<double,S,1>& current_state
         )
         {
+            // make sure requires clause is not broken
+            DLIB_ASSERT(min(R) > 0 && A.nr() == current_state.size(),
+                "\t matrix mpc::operator(current_state)"
+                << "\n\t invalid inputs were given to this function"
+                << "\n\t min(R): " << min(R) 
+                << "\n\t A.nr(): " << A.nr() 
+                << "\n\t current_state.size(): " << current_state.size() 
+                );
+
             // Shift the inputs over by one time step so we can use them to warm start the
             // optimizer.
             for (unsigned long i = 1; i < horizon; ++i)
@@ -171,11 +245,6 @@ namespace dlib
             const matrix<double,S,1>& initial_state
         )
         {
-            DLIB_CASSERT(min(Q) >= 0, "");
-            DLIB_CASSERT(min(R) >  0, "");
-
-
-
             // make it so MM == trans(K)*Q*(M-target)
             M[0] = A*initial_state + C;
             for (unsigned long i = 1; i < horizon; ++i)

dlib/control/mpc_abstract.h

@@ -25,17 +25,40 @@ namespace dlib
                 I_ >= 0
             
             WHAT THIS OBJECT REPRESENTS
-                Based largely on 
-                  A Fast Gradient method for embedded linear predictive control
-                  by Markus Kogel and Rolf Findeisen
-
+                This object implements a linear model predictive controller.  To explain
+                what that means, 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, or alternatively to make the
+                state track some reference time-varying sequence.  It does this 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 this object for a control, it solves the
+                following quadratic program:
         
-                  min     sum_i ( 0.5*trans(x_i)*Q*x_i + 0.5*trans(u_i)*R*u_i )
-                x_i,u_i
+                    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
-                           0 <= i < horizon
+                    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.  
+                
+                Finally, the algorithm we use to solve this quadratic program is based
+                largely on the method described in:
+                  A Fast Gradient method for embedded linear predictive control (2011)
+                  by Markus Kogel and Rolf Findeisen
         !*/
 
     public:
@@ -49,8 +72,8 @@ namespace dlib
         /*!
             ensures
                 - #get_max_iterations() == 0
-                - The values of the A,B,C,Q,R,lower, and upper parameter matrices are
-                  undefined.  To use this object you must initialize it via the constructor
+                - The A,B,C,Q,R,lower, and upper parameter matrices are filled with zeros.
+                  Therefore, to use this object you must initialize it via the constructor
                   that supplies these parameters.
         !*/
 
@@ -71,7 +94,7 @@ namespace dlib
                 - B.nc() == R.nr() == lower.nr() == upper.nr()
                 - min(Q) >= 0
                 - min(R) > 0
-                - min(upper-lower) > 0
+                - min(upper-lower) >= 0
             ensures
                 - #get_A() == A
                 - #get_B() == B
@@ -83,28 +106,64 @@ namespace dlib
                 - for all valid i:
                     - get_target(i) == a vector of all zeros
                     - get_target(i).size() == A.nr()
+                - #get_max_iterations() == 10000 
+                - #get_epsilon() == 0.01
         !*/
 
         const matrix<double,S,S>& get_A (
         ) const; 
+        /*!
+            ensures
+                - returns the A matrix from the quadratic program defined above. 
+        !*/
 
         const matrix<double,S,I>& get_B (
         ) const; 
+        /*!
+            ensures
+                - returns the B matrix from the quadratic program defined above. 
+        !*/
 
         const matrix<double,S,1>& get_C (
         ) const;
+        /*!
+            ensures
+                - returns the C matrix from the quadratic program defined above. 
+        !*/
 
         const matrix<double,S,1>& get_Q (
         ) const;
+        /*!
+            ensures
+                - returns the diagonal of the Q matrix from the quadratic program defined
+                  above. 
+        !*/
 
         const matrix<double,I,1>& get_R (
         ) const;
+        /*!
+            ensures
+                - returns the diagonal of the R matrix from the quadratic program defined
+                  above. 
+        !*/
 
         const matrix<double,I,1>& get_lower_constraints (
         ) const;
+        /*!
+            ensures
+                - returns the lower matrix from the quadratic program defined above.  All
+                  controls generated by this object will have values no less than this
+                  lower bound.  That is, any control u will satisfy min(u-lower) >= 0.
+        !*/
 
         const matrix<double,I,1>& get_upper_constraints (
         ) const;
+        /*!
+            ensures
+                - returns the upper matrix from the quadratic program defined above.  All
+                  controls generated by this object will have values no larger than this
+                  upper bound.  That is, any control u will satisfy min(upper-u) >= 0.
+        !*/
 
         const matrix<double,S,1>& get_target (
             const unsigned long time
@@ -112,6 +171,10 @@ namespace dlib
         /*!
             requires
                 - time < horizon
+            ensures
+                - This object will try to find the control sequence that results in the
+                  process obtaining get_target(time) state at the indicated time.  Note
+                  that the next time instant after "right now" is time 0. 
         !*/
 
         void set_target (
@@ -178,6 +241,23 @@ namespace dlib
         matrix<double,I,1> operator() (
             const matrix<double,S,1>& current_state
         );
+        /*!
+            requires
+                - min(R) > 0
+                - A.nr() == current_state.size()
+            ensures
+                - Solves the model predictive control problem defined by the arguments to
+                  this objects constructor, assuming that the starting state is given by
+                  current_state.  Then we return the control that should be taken in the
+                  current state that best optimizes the quadratic objective function
+                  defined above.
+                - We also shift over the target states so that you only need to update the
+                  last one (if you are using non-zero target states) via a call to
+                  set_last_target()).  In particular, for all valid t, it will be the case
+                  that:
+                    - #get_target(t) == get_target(t+1)
+                    - #get_target(horizon-1) == get_target(horizon-1)
+        !*/
 
     };