Skip to content

Commit

Permalink
Cleaned up this code, filled out spec, added asserts.
Browse files Browse the repository at this point in the history
  • Loading branch information
@davisking
davisking committed May 29, 2015
1 parent 78dc4a1 commit b975cdf
Show file tree
Hide file tree
Showing 2 changed files with 167 additions and 18 deletions.
81 changes: 75 additions & 6 deletions dlib/control/mpc.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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;
}

Expand All @@ -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];
}

Expand All @@ -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_
);
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand Down
104 changes: 92 additions & 12 deletions dlib/control/mpc_abstract.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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.
!*/

Expand All @@ -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
Expand All @@ -83,35 +106,75 @@ 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
) 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 (
Expand Down Expand Up @@ -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)
!*/

};

Expand Down

0 comments on commit b975cdf

Please sign in to comment.