Computes the Control Constraint Function

constraint

@(@\newcommand{\B}[1]{{\bf #1}} \newcommand{\R}[1]{{\rm #1}}@)@ Computes the Control Constraint Function
Syntax
L = control::constraint(delta_t, p, x, y)

Prototype


template <class Scalar> MATRIX(Scalar)
constraint(
     const Scalar&         delta_t ,
     const VECTOR(Scalar)& p       ,
     const MATRIX(Scalar)& x       ,
     const MATRIX(Scalar)& y       )

Definition
The constraint equation is @(@ L(x, y) = 0 @)@ where @(@ L : \B{R}^{2 \times J} \times \B{R}^{4 \times J} \rightarrow \B{R}^{4 \times J} @)@ is defined by @[@ L_{i,0} (x, y) = y_{i,0} - p_i @]@ for @(@ i = 0, \ldots , 3 @)@. @[@ \begin{array}{rcl} a_j (y) & = & [ ( y_{0,j} + 1 )^2 + y_{1,j}^2 ]^{-3/2} - 1 \\ L_{0,j} (x, y) & = & y_{2,j} \\ L_{1,j} (x, y) & = & y_{3,j} \\ L_{2,j} (x, y) & = & 2 y_{3,j} - (1 + y_{0,j}) a_j(y) + x_{0,j} \\ L_{3,j} (x, y) & = & - 2 y_{2,j} - y_{1,j} a_j(y) + x_{1,j} \end{array} @]@ for @(@ j = 1, \ldots , J - 1 @)@.

Example


bool test_control_constraint(void)
{    bool ok = true;
     double eps99   = 99.0 * std::numeric_limits<double>::epsilon();
     //
     VECTOR(double) p(4);
     MATRIX(double) x(2, 3);
     MATRIX(double) y(4, 3);
     for(size_t i = 0; i < 4; i++)
     {    p[i] = double(5 - i);
          for(size_t j = 0; j < 3; j++)
          {    y(i, j) = double(i + j + 1);
               if( i < 2 )
                    x(i, j) = double(j - i + 2);
          }
     }
     double delta_t = 0.5;
     MATRIX(double) L = control::constraint(delta_t, p, x, y);
     ok &= L.rows() == 4;
     ok &= L.cols() == 3;
     //
     for(size_t i = 0; i < 4; i++)
     {    double check = y(i, 0) - p[i];
          ok &= CppAD::NearEqual(L(i, 0), check, eps99, eps99);
     }
     for(size_t j = 1; j < 3; j++)
     {    // use notation in Park2006 reference
          double u1 = x(0,j-1);
          double u2 = x(1,j-1);
          double x1 = y(0,j-1);
          double x2 = y(1,j-1);
          double x3 = y(2,j-1);
          double x4 = y(3,j-1);
          double r  = sqrt( (x1 + 1.0) * (x1 + 1.0) + x2 * x2 );
          double a  = 1.0 / (r * r * r) - 1.0;
          //
          double dxdt[4];
          dxdt[0] = x3;
          dxdt[1] = x4;
          dxdt[2] = + 2.0 * x4 - (1.0 + x1) * a + u1;
          dxdt[3] = - 2.0 * x3 -         x2 * a + u2;
          //
          for(size_t i = 0; i < 4; i++)
          {    double check = y(i,j) - y(i,j-1) - dxdt[i] * delta_t;
               ok &= CppAD::NearEqual(L(i, j), check, eps99, eps99);
          }
     }
     //
     return ok;
}

Input File: src/control.hpp