typename boost::enable_if< 
  is_readable_matrix<Matrix>,
typename pp::topology_traits<StateSpace>::point_type >::type 
  sample_gaussian_point(const StateSpace& space, 
                        const typename pp::topology_traits<StateSpace>::point_type& mean, 
                        const Matrix& cov) {
  typedef typename pp::topology_traits<StateSpace>::point_difference_type DiffType;
  typedef typename mat_traits<Matrix>::value_type ValueType;
  typedef typename mat_traits<Matrix>::size_type SizeType;
  using std::sqrt;
  using ReaK::to_vect;
  using ReaK::from_vect;
  
  mat< ValueType, mat_structure::square> L;
  try {
    decompose_Cholesky(cov,L);
  } catch(singularity_error&) { 
    mat< ValueType, mat_structure::diagonal> E(cov.get_row_count());
    mat< ValueType, mat_structure::square> U(cov.get_row_count()), V(cov.get_row_count());
    decompose_SVD(cov,U,E,V);
    for(SizeType i = 0; i < cov.get_row_count(); ++i)
      E(i,i) = sqrt(E(i,i));
    L = U * E;
  };
  
  boost::variate_generator< global_rng_type&, boost::normal_distribution<ValueType> > var_rnd(get_global_rng(), boost::normal_distribution<ValueType>());
  
  vect_n<ValueType> z = to_vect<ValueType>(space.difference(mean, mean));
  for(SizeType i = 0; i < z.size(); ++i)
    z[i] = var_rnd();
  
  return space.adjust(mean, from_vect<DiffType>(L * z));
};
typename pp::topology_traits<StateSpace>::point_type 
  sample_gaussian_point(const StateSpace& space, 
                        const typename pp::topology_traits<StateSpace>::point_type& mean, 
                        const mat<ValueType, mat_structure::diagonal>& cov) {
  typedef typename pp::topology_traits<StateSpace>::point_difference_type DiffType;
  typedef typename mat_traits< mat<ValueType, mat_structure::diagonal> >::size_type SizeType;
  using std::sqrt;
  using ReaK::to_vect;
  using ReaK::from_vect;
  
  boost::variate_generator< global_rng_type&, boost::normal_distribution<ValueType> > var_rnd(get_global_rng(), boost::normal_distribution<ValueType>());
  
  vect_n<ValueType> z = to_vect<ValueType>(space.difference(mean, mean));
  for(SizeType i = 0; i < z.size(); ++i)
    z[i] = var_rnd() * sqrt(cov(i,i));
  
  return space.adjust(mean, from_vect<DiffType>(z));
};
Ejemplo n.º 3
0
 void hamming_mod_integrate_impl(
     const StateSpace& space,
     const StateSpaceSystem& sys,
     const typename pp::topology_traits<StateSpace>::point_type& start_point,
     typename pp::topology_traits<StateSpace>::point_type& end_point,
     const InputTrajectory& u_traj,
     double start_time,
     double end_time,
     double time_step) {
   if ((time_step == 0.0) ||
       ((time_step > 0.0) && (start_time > end_time)) ||
       ((time_step < 0.0) && (start_time < end_time)))
     throw impossible_integration(start_time, end_time, time_step);
   
   typedef typename pp::topology_traits<StateSpace>::point_type PointType;
   typedef typename pp::topology_traits<StateSpace>::point_difference_type PointDiffType;
   
   typedef typename pp::spatial_trajectory_traits<InputTrajectory>::const_waypoint_descriptor InputWaypoint;
   typedef typename pp::spatial_trajectory_traits<InputTrajectory>::point_type InputType;
   std::pair< InputWaypoint, InputType> u_wp = u_traj.get_waypoint_at_time(start_time);
   
   double back_time = start_time;
   PointDiffType dp = sys.get_state_derivative(space, start_point, u_wp.second.pt, start_time);
   PointType w = start_point;
   PointType Y_n[3];
   PointDiffType Yp_n[3];
   
   for(std::size_t i = 0; i < 3; ++i) {
     PointDiffType k1 = time_step * dp;
     Y_n[i] = space.adjust(w, -0.25 * k1);
     
     back_time -= time_step * 0.25;
     u_wp = u_traj.move_time_diff_from(u_wp, -0.25 * time_step);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     PointDiffType k2 = time_step * Yp_n_1;
     Y_n[i] = space.adjust(Y_n[i], (5.0 / 32.0) * k1 - (9.0 / 32.0) * k2);
     
     back_time -= time_step * 0.125;
     u_wp = u_traj.move_time_diff_from(u_wp, -0.125 * time_step);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     PointDiffType k3 = time_step * Yp_n[i];
     Y_n[i] = space.adjust(Y_n[i], (1250865.0 / 351520.0) * k2 - (276165.0 / 351520.0) * k1 - (1167360.0 / 351520.0) * k3);
     
     back_time -= 57.0 * time_step / 104.0;
     u_wp = u_traj.move_time_diff_from(u_wp, -57.0 / 104.0 * time_step);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     PointDiffType k4 = time_step * Yp_n[i];
     Y_n[i] = space.adjust(w, 8.0 * k2 - (439.0 / 216.0) * k1 - (3680.0 / 513.0) * k3 + (845.0 / 4104.0) * k4);
     
     back_time -= time_step / 13.0;
     u_wp = u_traj.move_time_diff_from(u_wp, -time_step / 13.0);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     PointDiffType k5 = time_step * Yp_n[i];
     Y_n[i] = space.adjust(w, (8.0 / 27.0) * k1 - 2.0 * k2 + (3544.0 / 2565.0) * k3 - (1859.0 / 4104.0) * k4 + (11.0 / 40.0) * k5);
     
     back_time += time_step * 0.5;
     u_wp = u_traj.move_time_diff_from(u_wp, 0.5 * time_step);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     Y_n[i] = space.adjust(w, (-16.0 / 135.0) * k1 - (6656.0 / 12825.0) * k3 - (28561.0 / 56430.0) * k4 + (9.0 / 50.0) * k5 - (2.0 * time_step / 55.0) * Yp_n[i]);
     
     back_time -= time_step * 0.5;
     u_wp = u_traj.move_time_diff_from(u_wp, -0.5 * time_step);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     
     w = Y_n[i];
     dp = Yp_n[i];
   };
   
   PointType C = start_point;
   PointType P_n = start_point; // this makes sure the initial error sweep is zero.
   
   double t = start_time;
   end_point = start_point;
   u_wp = u_traj.get_waypoint_at_time(t);
   dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
   
   while(((time_step > 0.0) && (t < end_time)) || 
         ((time_step < 0.0) && (t > end_time))) {
     
     PointType P = space.adjust(Y_n[2], (time_step * 4.0 / 3.0) * (2.0 * (dp + Yp_n[1]) - Yp_n[0]));
     PointType M = space.adjust(P, (112.0 / 121.0) * space.difference(C, P_n));
     
     t += time_step;
     u_wp = u_traj.move_time_diff_from(u_wp, time_step);
     PointDiffType Mp = sys.get_state_derivative(space, M, u_wp.second.pt, t);
     
     C = space.adjust(end_point, 0.125 * space.difference(end_point, Y_n[1]) + (time_step * 3.0 / 8.0) * (Mp + 2.0 * dp - Yp_n[0]));
     
     Y_n[2] = Y_n[1];
     Y_n[1] = Y_n[0];
     Y_n[0] = end_point;
     end_point = space.adjust(C, (9.0 / 121.0) * space.difference(P, C));
     P_n = P;
     
     Yp_n[1] = Yp_n[0];
     Yp_n[0] = dp;
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
     
   };
 };