C++ (Cpp) index_norm_inf Examples

Example #1

File: vector_operations.hpp Project: bollig/viennacl-dev

 std::size_t index_norm_inf(viennacl::vector_expression<LHS, RHS, OP> const & vec)
 {
   viennacl::vector<typename viennacl::result_of::cpu_value_type<LHS>::type> temp = vec;
   return index_norm_inf(temp);
 }

Example #2

File: lyap_controller.cpp Project: AndyZelenak/pid_lyap

////////////////////////////////////////////////////////////////////
// Calculate a stabilizing control effort
////////////////////////////////////////////////////////////////////
void calculate_u(ublas_vector &D, ublas_vector &open_loop_dx_dt, const double &V_dot_target, boost::numeric::ublas::matrix<double> &dx_dot_du)
{
  u.clear();

  // Find the largest |D_i|
  // It will be the base for the u_i calculations.
  // If all D_i's are ~0, use V2.
  int largest_D_index = index_norm_inf(D);

  if ( fabs(D[largest_D_index]) > switch_threshold ) // Use V1
  {
    ublas_vector P = element_prod(x-setpoint,open_loop_dx_dt); // x_i*x_i_dot

    // Start with the u that has the largest effect
    if ( fabs( D[largest_D_index] + ( sum( element_prod(D,D) ) - pow(D[largest_D_index],2.0) )/D[largest_D_index]) > 0.0001 ) 
      u(largest_D_index) = (V_dot_target-sum(P)) / 
        ( D[largest_D_index] + ( sum( element_prod(D,D) ) - pow(D[largest_D_index],2.0) )/D[largest_D_index] );

    // Now scale the other u's (if any) according to u_max*Di/D_max
    for ( int i=0; i<num_inputs; i++ )
      if ( i != largest_D_index ) // Skip this entry, we already did it
      {
        //if ( fabs(D[i]) > 0.1*fabs(D[largest_D_index]) ) // If this D has a significant effect. Otherwise this u will remain zero
        //{
          u[i] = u[largest_D_index]*D[i]/D[largest_D_index];
        //}
      }
  } // End of V1 calcs

  else // Use V2
  {

    //ublas_vector dV2_dx = (x-setpoint)*(0.9+0.1*sum( element_prod(x-setpoint,x-setpoint) ));

    ublas_vector dV2_dx = (x-setpoint)*(0.9+0.1*tanh(g)+0.05*sum( element_prod(x-setpoint,x-setpoint) )*pow(cosh(g),-2));

    // The first entry in dV2_dx is unique because the step is specified in x1, so replace the previous
    dV2_dx[0] = (x[0]-setpoint[0])*(0.9+0.1*tanh(g))+
	0.05*sum(element_prod(x-setpoint,x-setpoint))*(pow((x[0]-setpoint[0]),-1)*tanh(g)+(x[0]-setpoint[0])*pow(cosh(g),-2));

    //MATLAB equivalent:
    //P_star = dV2_dx.*f_at_u_0(1:num_states);

    ublas_vector P_star = element_prod(dV2_dx, open_loop_dx_dt);

    //MATLAB equivalent:
    //D_star = zeros(num_inputs,1);
    //for i=1:num_inputs
    //  for j=1:num_states
    //    D_star(i) = D_star[i]+dV2_dx(j)*dx_dot_du(i,j);
    //  end
    //end

    ublas_vector D_star(num_inputs);


    for (int i=0; i<num_inputs-1; i++)
      for (int j=0; j<num_inputs-1; j++)
        D_star[i] = D_star[i]+dV2_dx[j]*dx_dot_du(i,j);


    // The first input is unique.
    // MATLAB equivalent:
    //u(epoch,1) = (V_dot_target-sum(P_star)) / ...
    //( D_star(1) + (sum( D_star.*D_star )- D_star(1)^2) /...
    //D_star(1) );

    u[0] = (V_dot_target-sum(P_star)) / 
      ( D_star[0] + (sum( element_prod(D_star,D_star) )-pow(D_star[0],2)) / D_star[0] );

     // For the other inputs
     // MATLAB equivalent:
     //for i=2:num_inputs
     //  u(epoch,i) = u(epoch,1)*D_star(i)/D_star(1);
     //end

     for (int i=1; i<num_inputs; i++)
     {
       //u[i] = u[0]*D_star[i]/D_star(0);
       u[i] = u[0]*D_star[i]/0.0;
     }

     // Check for NaN (caused by D_star(1)==0).
     // It means the system is likely uncontrollable.
     // MATLAB equivalent:
     //if ~isfinite( u(epoch,:) )
     //  u(epoch,:)= zeros(num_inputs,1);
     //end
  
     for (int i=0; i<num_inputs; i++)
     {
       if ( (boost::math::isnan)(u[i]) )
       {
         u.clear(); // Reset u to zeros
         //ROS_INFO("isnan");
         break; // Short circuit, could save time for long u vectors
       }
     }
  } // End of V2 calcs
}