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));
};
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);
};
};