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.h

+// Copyright (C) 2015  Davis E. King (davis@dlib.net)
+// License: Boost Software License   See LICENSE.txt for the full license.
+#ifndef DLIB_MPC_Hh_
+#define DLIB_MPC_Hh_
+
+#include "mpc_abstract.h"
+#include "../matrix.h"
+#include "../algs.h"
+
+
+namespace dlib
+{
+    template <
+        long S_,
+        long I_,
+        unsigned long horizon_
+        >
+    class mpc
+    {
+
+    public:
+
+        const static long S = S_;
+        const static long I = I_;
+        const static unsigned long horizon = horizon_;
+
+        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)
+            {
+                target[i].set_size(A.nr());
+                target[i] = 0;
+
+                controls[i].set_size(B.nc());
+                controls[i] = 0;
+            }
+            lambda = 0;
+        }
+
+        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_
+        ) : 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)
+            {
+                target[i].set_size(A.nr());
+                target[i] = 0;
+
+                controls[i].set_size(B.nc());
+                controls[i] = 0;
+            }
+
+            // Bound the maximum eigenvalue of the hessian by computing the trace of the
+            // hessian matrix. 
+            lambda = sum(R)*horizon;
+            matrix<double,S,S> temp = diagm(Q);
+            for (unsigned long c = 0; c < horizon; ++c)
+            {
+                lambda += trans(B)*temp*B;
+                temp = trans(A)*temp*A + diagm(Q);
+            }
+
+        }
+
+        const matrix<double,S,S>& get_A (
+        ) const { return A; }
+        const matrix<double,S,I>& get_B (
+        ) const { return B; }
+        const matrix<double,S,1>& get_C (
+        ) const { return C; }
+        const matrix<double,S,1>& get_Q (
+        ) const { return Q; }
+        const matrix<double,I,1>& get_R (
+        ) const { return R; }
+        const matrix<double,I,1>& get_lower_constraints (
+        ) const { return lower; }
+        const matrix<double,I,1>& get_upper_constraints (
+        ) const { return upper; }
+
+        void set_target (
+            const matrix<double,S,1>& val,
+            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;
+        }
+
+        void set_last_target (
+            const matrix<double,S,1>& val
+        )
+        {
+            set_target(val, horizon-1);
+        }
+
+        const matrix<double,S,1>& get_target (
+            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];
+        }
+
+        unsigned long get_max_iterations (
+        ) const { return max_iterations; }
+
+        void set_max_iterations (
+            unsigned long max_iter
+        ) 
+        {
+            max_iterations = max_iter;
+        }
+
+        void set_epsilon (
+            double eps_
+        )
+        {
+            // make sure requires clause is not broken
+            DLIB_ASSERT(eps_ > 0,
+                "\t void mpc::set_epsilon(eps_)"
+                << "\n\t invalid inputs were given to this function"
+                << "\n\t eps_: " << eps_ 
+                );
+            eps = eps_;
+        }
+
+        double get_epsilon (
+        ) const
+        { 
+            return eps;
+        }
+
+        matrix<double,I,1> operator() (
+            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)
+                controls[i-1] = controls[i];
+
+            solve_linear_mpc(current_state);
+
+            for (unsigned long i = 1; i < horizon; ++i)
+                target[i-1] = target[i];
+
+            return controls[0];
+        }
+
+    private:
+
+
+        // These temporary variables here just to avoid reallocating them on each call to
+        // operator().
+        matrix<double,S,1> M[horizon];
+        matrix<double,I,1> MM[horizon];
+        matrix<double,I,1> df[horizon]; 
+        matrix<double,I,1> v[horizon]; 
+        matrix<double,I,1> v_old[horizon]; 
+
+        void solve_linear_mpc (
+            const matrix<double,S,1>& initial_state
+        )
+        {
+            // make it so MM == trans(K)*Q*(M-target)
+            M[0] = A*initial_state + C;
+            for (unsigned long i = 1; i < horizon; ++i)
+                M[i] = A*M[i-1] + C;
+            for (unsigned long i = 0; i < horizon; ++i)
+                M[i] = diagm(Q)*(M[i]-target[i]);
+            for (long i = (long)horizon-2; i >= 0; --i)
+                M[i] += trans(A)*M[i+1];
+            for (unsigned long i = 0; i < horizon; ++i)
+                MM[i] = trans(B)*M[i];
+
+            for (unsigned long i = 0; i < horizon; ++i)
+                v[i] = controls[i];
+
+
+
+            unsigned long iter = 0;
+            for (; iter < max_iterations; ++iter)
+            {
+                // compute current gradient and put it into df.
+                // df == H*controls + MM;
+                M[0] = B*controls[0];
+                for (unsigned long i = 1; i < horizon; ++i)
+                    M[i] = A*M[i-1] + B*controls[i];
+                for (unsigned long i = 0; i < horizon; ++i)
+                    M[i] = diagm(Q)*M[i];
+                for (long i = (long)horizon-2; i >= 0; --i)
+                    M[i] += trans(A)*M[i+1];
+                for (unsigned long i = 0; i < horizon; ++i)
+                    df[i] = MM[i] + trans(B)*M[i] + diagm(R)*controls[i];
+
+
+
+                // Check the stopping condition, which is the magnitude of the largest element
+                // of the gradient.
+                double max_df = 0;
+                for (unsigned long i = 0; i < horizon; ++i)
+                {
+                    for (long j = 0; j < controls[i].size(); ++j)
+                    {
+                        // if this variable isn't an active constraint then we care about it's
+                        // derivative.
+                        if (!((controls[i](j) <= lower(j) && df[i](j) > 0) || 
+                              (controls[i](j) >= upper(j) && df[i](j) < 0)))
+                        {
+                            max_df = std::max(max_df, std::abs(df[i](j)));
+                        }
+                    }
+                }
+                if (max_df < eps)
+                    break;
+
+
+                // take a step based on the gradient
+                for (unsigned long i = 0; i < horizon; ++i)
+                {
+                    v_old[i] = v[i];
+                    v[i] = clamp(controls[i] - 1.0/lambda * df[i], lower, upper);
+                    controls[i] = clamp(v[i] + (std::sqrt(lambda)-1)/(std::sqrt(lambda)+1)*(v[i]-v_old[i]), lower, upper);
+                }
+            }
+        }
+
+        unsigned long max_iterations;
+        double eps;
+
+        matrix<double,S,S> A;
+        matrix<double,S,I> B;
+        matrix<double,S,1> C;
+        matrix<double,S,1> Q;
+        matrix<double,I,1> R;
+        matrix<double,I,1> lower;
+        matrix<double,I,1> upper;
+        matrix<double,S,1> target[horizon]; 
+
+        double lambda; // abound on the largest eigenvalue of the hessian matrix.
+        matrix<double,I,1> controls[horizon]; 
+
+    };
+
+}
+
+#endif // DLIB_MPC_Hh_
+

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

dlib/test/mpc.cpp

+// Copyright (C) 2015  Davis E. King (davis@dlib.net)
+// License: Boost Software License   See LICENSE.txt for the full license.
+
+
+#include <string>
+#include <sstream>
+
+#include <dlib/control.h>
+#include "tester.h"
+
+namespace  
+{
+    using namespace test;
+    using namespace std;
+    using namespace dlib;
+  
+    logger dlog("test.mpc");
+
+    template <
+        typename EXP1,
+        typename EXP2,
+        typename T, long NR, long NC, typename MM, typename L
+        >
+    unsigned long solve_qp_box_using_smo ( 
+        const matrix_exp<EXP1>& _Q,
+        const matrix_exp<EXP2>& _b,
+        matrix<T,NR,NC,MM,L>& alpha,
+        matrix<T,NR,NC,MM,L>& lower,
+        matrix<T,NR,NC,MM,L>& upper,
+        T eps,
+        unsigned long max_iter
+    )
+    /*!
+        ensures
+            - solves: 0.5*trans(x)*Q*x + trans(b)*x where x is box constrained.
+    !*/
+    {
+        const_temp_matrix<EXP1> Q(_Q);
+        const_temp_matrix<EXP2> b(_b);
+        //cout << "IN QP SOLVER" << endl;
+        //cout << "max eig: " << max(real_eigenvalues(Q)) << endl;
+        //cout << "min eig: " << min(real_eigenvalues(Q)) << endl;
+        //cout << "Q: \n" << Q << endl;
+        //cout << "b: \n" << b << endl;
+
+        // make sure requires clause is not broken
+        DLIB_ASSERT(Q.nr() == Q.nc() &&
+                     alpha.size() == lower.size() &&
+                     alpha.size() == upper.size() &&
+                     is_col_vector(b) &&
+                     is_col_vector(alpha) &&
+                     is_col_vector(lower) &&
+                     is_col_vector(upper) &&
+                     b.size() == alpha.size() &&
+                     b.size() == Q.nr() &&
+                     alpha.size() > 0 &&
+                     0 <= min(alpha-lower) &&
+                     0 <= max(upper-alpha) &&
+                     eps > 0 &&
+                     max_iter > 0,
+                     "\t unsigned long solve_qp_box_using_smo()"
+                     << "\n\t Invalid arguments were given to this function"
+                     << "\n\t Q.nr():               " << Q.nr()
+                     << "\n\t Q.nc():               " << Q.nc()
+                     << "\n\t is_col_vector(b):     " << is_col_vector(b)
+                     << "\n\t is_col_vector(alpha): " << is_col_vector(alpha)
+                     << "\n\t is_col_vector(lower): " << is_col_vector(lower)
+                     << "\n\t is_col_vector(upper): " << is_col_vector(upper)
+                     << "\n\t b.size():             " << b.size() 
+                     << "\n\t alpha.size():         " << alpha.size() 
+                     << "\n\t lower.size():         " << lower.size() 
+                     << "\n\t upper.size():         " << upper.size() 
+                     << "\n\t Q.nr():               " << Q.nr() 
+                     << "\n\t min(alpha-lower):     " << min(alpha-lower) 
+                     << "\n\t max(upper-alpha):     " << max(upper-alpha) 
+                     << "\n\t eps:                  " << eps 
+                     << "\n\t max_iter:             " << max_iter 
+        );
+
+
+        // Compute f'(alpha) (i.e. the gradient of f(alpha)) for the current alpha.  
+        matrix<T,NR,NC,MM,L> df = Q*alpha + b;
+        matrix<T,NR,NC,MM,L> QQ = reciprocal_max(diag(Q));
+
+
+        unsigned long iter = 0;
+        for (; iter < max_iter; ++iter)
+        {
+            T max_df = 0;
+            long best_r =0;
+            for (long r = 0; r < Q.nr(); ++r)
+            {
+                if (alpha(r) <= lower(r) && df(r) > 0)
+                    ;//alpha(r) = lower(r);
+                else if (alpha(r) >= upper(r) && df(r) < 0)
+                    ;//alpha(r) = upper(r);
+                else if (std::abs(df(r)) > max_df)
+                {
+                    best_r = r;
+                    max_df = std::abs(df(r));
+                }
+            }
+
+            //for (long r = 0; r < Q.nr(); ++r)
+            long r = best_r;
+            {
+
+                const T old_alpha = alpha(r);
+                alpha(r) = -(df(r)-Q(r,r)*alpha(r))*QQ(r);
+                if (alpha(r) < lower(r))
+                    alpha(r) = lower(r);
+                else if (alpha(r) > upper(r))
+                    alpha(r) = upper(r);
+
+                const T delta = old_alpha-alpha(r);
+
+                // Now update the gradient. We will perform the equivalent of: df = Q*alpha + b;
+                for(long k = 0; k < df.nr(); ++k)
+                    df(k) -= Q(r,k)*delta;
+            }
+
+            if (max_df < eps)
+                break;
+        }
+        //cout << "df: \n" << trans(df) << endl;
+        //cout << "objective value: " << 0.5*trans(alpha)*Q*alpha + trans(b)*alpha << endl;
+
+        return iter+1;
+    }
+
+// ----------------------------------------------------------------------------------------
+
+    namespace impl_mpc
+    {
+        template <long N>
+        void pack(
+            matrix<double,0,1>& out,
+            const std::vector<matrix<double,N,1> >& item
+        )
+        {
+            DLIB_CASSERT(item.size() != 0,"");
+            out.set_size(item.size()*item[0].size());
+            long j = 0;
+            for (unsigned long i = 0; i < item.size(); ++i)
+                for (long r = 0; r < item[i].size(); ++r)
+                    out(j++) = item[i](r);
+        }
+
+        template <long N>
+        void pack(
+            matrix<double,0,1>& out,
+            const matrix<double,N,1>& item,
+            const long num
+        )
+        {
+            out.set_size(item.size()*num);
+            long j = 0;
+            for (long r = 0; r < num; ++r)
+                for (long i = 0; i < item.size(); ++i)
+                    out(j++) = item(i);
+        }
+
+        template <long N>
+        void unpack(
+            std::vector<matrix<double,N,1> >& out,
+            const matrix<double,0,1>& item 
+        )
+        {
+            DLIB_CASSERT(out.size() != 0,"");
+            DLIB_CASSERT((long)out.size()*out[0].size() == item.size(),"");
+            long j = 0;
+            for (unsigned long i = 0; i < out.size(); ++i)
+                for (long r = 0; r < out[i].size(); ++r)
+                    out[i](r) = item(j++);
+        }
+    }
+
+    template <long S, long I>
+    unsigned long solve_linear_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,
+        const std::vector<matrix<double,S,1> >& target, 
+        const matrix<double,S,1>& initial_state,
+        std::vector<matrix<double,I,1> >& controls // input and output
+    )
+    {
+        using namespace impl_mpc;
+        DLIB_CASSERT(target.size() == controls.size(),"");
+
+        matrix<double> K(B.nr()*controls.size(), B.nc()*controls.size());
+        matrix<double,0,1> M(B.nr()*controls.size());
+
+        // compute powers of A: Apow[i] == A^i
+        std::vector<matrix<double,S,S> > Apow(controls.size());
+        Apow[0] = identity_matrix(A);
+        for (unsigned long i = 1; i < Apow.size(); ++i)
+            Apow[i] = A*Apow[i-1];
+
+        // fill in K
+        K = 0;
+        for (unsigned long r = 0; r < controls.size(); ++r)
+            for (unsigned long c = 0; c <= r; ++c)
+                set_subm(K,r*B.nr(),c*B.nc(), B.nr(), B.nc()) = Apow[r-c]*B;
+
+        // fill in M
+        set_subm(M,0*A.nr(),0,A.nr(),1) = A*initial_state + C;
+        for (unsigned long i = 1; i < controls.size(); ++i)
+            set_subm(M,i*A.nr(),0,A.nr(),1) = A*subm(M,(i-1)*A.nr(),0,A.nr(),1) + C;
+
+        //cout << "M: \n" << M << endl;
+        //cout << "K: \n" << K << endl;
+
+        matrix<double,0,1> t, v, lower, upper;
+        pack(t, target);
+        pack(v, controls);
+        pack(lower, _lower, controls.size());
+        pack(upper, _upper, controls.size());
+
+
+        matrix<double> QQ(K.nr(),K.nr()), RR(K.nc(),K.nc());
+        QQ = 0;
+        RR = 0;
+        for (unsigned long c = 0; c < controls.size(); ++c)
+        {
+            set_subm(QQ,c*Q.nr(),c*Q.nr(),Q.nr(),Q.nr()) = diagm(Q);
+            set_subm(RR,c*R.nr(),c*R.nr(),R.nr(),R.nr()) = diagm(R);
+        }
+
+        matrix<double> m1 = trans(K)*QQ*K+RR;
+        matrix<double> m2 = trans(K)*QQ*(M-t);
+
+
+        // run the solver...
+        unsigned long iter;
+        iter = solve_qp_box_using_smo(
+            m1,
+            m2,
+            v,
+            lower,
+            upper,
+            0.00000001,
+            100000);
+
+        //cout << "iterations: " << iter << endl;
+
+        unpack(controls, v);
+        return iter;
+    }
+
+
+
+    class test_mpc : public tester
+    {
+    public:
+        test_mpc (
+        ) :
+            tester ("test_mpc",
+                    "Runs tests on the mpc object.")
+        {}
+
+        void perform_test (
+        )
+        {
+            // a basic position + velocity model
+            matrix<double,2,2> A;
+            A = 1, 1,
+            0, 1;
+            matrix<double,2,1> B, C;
+            B = 0,
+            1;
+
+            C = 0.02,0.1; // no constant bias
+
+            matrix<double,2,1> Q;
+            Q = 2, 0; // only care about getting the position right
+            matrix<double,1,1> R, lower, upper;
+            R = 1;
+
+            lower = -0.2;
+            upper =  0.2;
+
+            std::vector<matrix<double,1,1> > controls(30);
+            std::vector<matrix<double,2,1> > target(30);
+            for (unsigned long i = 0; i < controls.size(); ++i)
+            {
+                controls[i] = 0;
+                target[i] = 0;
+            }
+
+            mpc<2,1,30> solver(A,B,C,Q,R,lower,upper);
+            solver.set_epsilon(0.00000001);
+            solver.set_max_iterations(10000);
+            matrix<double,2,1> initial_state;
+            initial_state = 0;
+            initial_state(0) = 5;
+            for (int i = 0; i < 30; ++i)
+            {
+                print_spinner();
+                matrix<double,1,1> control = solver(initial_state);
+
+                for (unsigned long i = 1; i < controls.size(); ++i)
+                    controls[i-1] = controls[i];
+
+                // Compute the correct control via SMO and make sure it matches.
+                solve_linear_mpc(A,B,C,Q,R,lower,upper, target, initial_state, controls);
+                dlog << LINFO << "ERROR: " << length(control-controls[0]);
+                DLIB_TEST(length(control-controls[0]) < 1e-7);
+
+                initial_state = A*initial_state + B*control + C;
+                //cout << control(0) << "\t" << trans(initial_state);
+            }
+        }
+    } a;
+
+}
+
+
+
+

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"/>