merged

dlib/control.h

@@ -4,6 +4,7 @@
 #define DLIB_CONTRoL_
 
 #include "control/lspi.h"
+#include "control/mpc.h"
 
 #endif // DLIB_CONTRoL_
 

dlib/control/mpc_abstract.h

+// Copyright (C) 2015  Davis E. King (davis@dlib.net)
+// License: Boost Software License   See LICENSE.txt for the full license.
+#undef DLIB_MPC_ABSTRACT_Hh_
+#ifdef DLIB_MPC_ABSTRACT_Hh_
+
+#include "../matrix.h"
+
+namespace dlib
+{
+    template <
+        long S_,
+        long I_,
+        unsigned long horizon_
+        >
+    class mpc
+    {
+        /*!
+            REQUIREMENTS ON horizon_
+                horizon_ > 0
+
+            REQUIREMENTS ON S_
+                S_ >= 0
+
+            REQUIREMENTS ON I_
+                I_ >= 0
+            
+            WHAT THIS OBJECT REPRESENTS
+                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 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.  
+                
+                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:
+
+        const static long S = S_;
+        const static long I = I_;
+        const static unsigned long horizon = horizon_;
+
+        mpc(
+        );
+        /*!
+            ensures
+                - #get_max_iterations() == 0
+                - 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.
+        !*/
+
+        mpc (
+            const matrix<double,S,S>& A,
+            const matrix<double,S,I>& B,
+            const matrix<double,S,1>& C,
+            const matrix<double,S,1>& Q,
+            const matrix<double,I,1>& R,
+            const matrix<double,I,1>& lower,
+            const matrix<double,I,1>& upper
+        ); 
+        /*!
+            requires
+                - A.nr() > 0
+                - B.nc() > 0
+                - A.nr() == A.nc() == B.nr() == C.nr() == Q.nr()
+                - B.nc() == R.nr() == lower.nr() == upper.nr()
+                - min(Q) >= 0
+                - min(R) > 0
+                - min(upper-lower) >= 0
+            ensures
+                - #get_A() == A
+                - #get_B() == B
+                - #get_C() == C
+                - #get_Q() == Q
+                - #get_R() == R
+                - #get_lower_constraints() == lower
+                - #get_upper_constraints() == upper 
+                - 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
+        ) const;
+        /*!
+            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 (
+            const matrix<double,S,1>& val,
+            const unsigned long time
+        );
+        /*!
+            requires
+                - time < horizon
+            ensures
+                - #get_target(time) == val
+        !*/
+
+        void set_last_target (
+            const matrix<double,S,1>& val
+        );
+        /*!
+            ensures
+                - performs: set_target(val, horizon-1)
+        !*/
+
+        unsigned long get_max_iterations (
+        ) const; 
+        /*!
+            ensures
+                - When operator() is called it solves an optimization problem to
+                  get_epsilon() precision to determine the next control action.  In
+                  particular, we run the optimizer until the magnitude of each element of
+                  the gradient vector is less than get_epsilon() or until
+                  get_max_iterations() solver iterations have been executed.
+        !*/
+
+        void set_max_iterations (
+            unsigned long max_iter
+        );
+        /*!
+            ensures
+                - #get_max_iterations() == max_iter
+        !*/
+
+        void set_epsilon (
+            double eps
+        );
+        /*!
+            requires
+                - eps > 0
+            ensures
+                - #get_epsilon() == eps
+        !*/
+
+        double get_epsilon (
+        ) const;
+        /*!
+            ensures
+                - When operator() is called it solves an optimization problem to
+                  get_epsilon() precision to determine the next control action.  In
+                  particular, we run the optimizer until the magnitude of each element of
+                  the gradient vector is less than get_epsilon() or until
+                  get_max_iterations() solver iterations have been executed.  This means
+                  that smaller epsilon values will give more accurate outputs but may take
+                  longer to compute.
+        !*/
+
+        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)
+        !*/
+
+    };
+
+}
+
+#endif // DLIB_MPC_ABSTRACT_Hh_
+

dlib/test/CMakeLists.txt

@@ -84,6 +84,7 @@ set (tests
    md5.cpp
    member_function_pointer.cpp
    metaprogramming.cpp
+   mpc.cpp
    multithreaded_object.cpp
    numerical_integration.cpp
    object_detector.cpp

dlib/test/makefile

@@ -99,6 +99,7 @@ SRC += max_sum_submatrix.cpp
 SRC += md5.cpp
 SRC += member_function_pointer.cpp
 SRC += metaprogramming.cpp
+SRC += mpc.cpp
 SRC += multithreaded_object.cpp
 SRC += numerical_integration.cpp
 SRC += object_detector.cpp

docs/docs/optimization.xml

@@ -47,6 +47,7 @@
          <item>solve_qp3_using_smo</item> 
          <item>solve_qp4_using_smo</item> 
          <item>oca</item> 
+         <item>mpc</item> 
          <item>solve_least_squares</item> 
          <item>solve_least_squares_lm</item> 
          <item>solve_trust_region_subproblem</item> 
@@ -761,6 +762,23 @@ Or it can alternatively solve:
                                  
       </component>
 
+   <!-- ************************************************************************* -->
+      
+      <component>
+         <name>mpc</name>
+         <file>dlib/control.h</file>
+         <spec_file link="true">dlib/control/mpc_abstract.h</spec_file>
+         <description>
+                This object implements a linear model predictive controller.  
+                In particular, it solves a certain quadratic program using the method
+                described in the paper:
+                <blockquote>
+                  A Fast Gradient method for embedded linear predictive control (2011)
+                  by Markus Kogel and Rolf Findeisen
+                </blockquote>
+         </description>
+      </component>
+
    <!-- ************************************************************************* -->
       
       

docs/docs/term_index.xml

@@ -158,6 +158,7 @@
          <term file="optimization.html" name="solve_qp3_using_smo"            include="dlib/optimization.h"/>
          <term file="optimization.html" name="solve_qp4_using_smo"            include="dlib/optimization.h"/>
          <term file="optimization.html" name="oca"                            include="dlib/optimization.h"/>
+         <term file="optimization.html" name="mpc"                            include="dlib/control.h"/>
          <term link="optimization.html#find_min_bobyqa" name="BOBYQA"         include="dlib/optimization.h"/>
          <term file="optimization.html" name="find_max"                       include="dlib/optimization.h"/>
          <term file="optimization.html" name="find_max_single_variable"       include="dlib/optimization.h"/>