Skip to content

D
dlib
Commit c4e4cd00 authored by Davis King's avatar Davis King
Browse files
Options

merged

parents d5170a39 eadb9ee1
Show whitespace changes
Inline Side-by-side
Showing with 944 additions and 0 deletions
dlib/control.h
View file @ c4e4cd00
......@@ -4,6 +4,7 @@
#define DLIB_CONTRoL_
#include "control/lspi.h"
#include "control/mpc.h"
#endif // DLIB_CONTRoL_
......
dlib/control/mpc.h 0 → 100644
View file @ c4e4cd00
// 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 0 → 100644
View file @ c4e4cd00
// 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
View file @ c4e4cd00
......@@ -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
View file @ c4e4cd00
......@@ -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 0 → 100644
View file @ c4e4cd00
// 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
View file @ c4e4cd00
......@@ -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
View file @ c4e4cd00
......@@ -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"/>
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment