boost::function<Real (Time)> Generalized_HullWhite::vol() const {
std::vector<Real> volvals, volPeriod;
volvals.push_back(sigma_(0.0));
volPeriod.push_back(0.0);
for (Size i=0;i<volperiods_.size();i++) {
volvals.push_back(sigma_(volperiods_[i]+0.001));
volPeriod.push_back(volperiods_[i]);
}
return Impl::PiecewiseLinearCurve(volPeriod, volvals);
}
boost::function<Real (Time)> GeneralizedHullWhite::vol() const {
std::vector<Real> volvals;
volvals.push_back(sigma_(0.0001));
for (Size i=0;i<sigma_.size()-1;i++)
volvals.push_back(
sigma_(
(speedstructure_[i+1]-speedstructure_[0])/365.0
- 0.00001));
return PiecewiseLinearCurve(volperiods_, volvals);
}
/**
* Compute force and potential for interaction.
*
* @param rr squared distance between particles
* @param a type of first interacting particle
* @param b type of second interacting particle
* @returns tuple of unit "force" @f$ -U'(r)/r @f$ and potential @f$ U(r) @f$
*/
std::tuple<float_type, float_type> operator()(float_type rr, unsigned a, unsigned b) const
{
float_type r_sigma = sqrt(rr) / sigma_(a, b);
float_type exp_dr = exp(r_min_sigma_(a, b) - r_sigma);
float_type eps_exp_dr = epsilon_(a, b) * exp_dr;
float_type fval = 2 * eps_exp_dr * (exp_dr - 1) * r_sigma / rr;
float_type en_pot = eps_exp_dr * (exp_dr - 2);
return std::make_tuple(fval, en_pot);
}
Real Generalized_HullWhite::Vr(Time s, Time t) const {
QL_REQUIRE(s<=t, "s should be less than t");
Real temp = 0.0;
Time u0 = 0.0, u1 = 0.0;
for (Size i=0; i<volperiods_.size(); ++i) {
u1 = volperiods_[i];
if (u1>s && u0<t) {
u0 = (u0>s)?u0:s;
u1 = (u1<t)?u1:t;
temp += sigma_(u0)*sigma_(u0) / (2 * a()) * (std::exp(2*a()*u1) - std::exp(2*a()*u0));
}
u0 = u1;
}
if (u0<t) {
u0 = (u0>s)?u0:s;
temp += sigma_(u0)*sigma_(u0) / (2 * a()) * (std::exp(2*a()*t) - std::exp(2*a()*u0));
}
return std::exp(-2.0*a()*t) * temp;
}
el_sem_naive::el_sem_naive(SEXP weights_r, SEXP y_r, SEXP omega_r, SEXP b_r , SEXP dual_r, int meanEst)
{
mat y = as<arma::mat>(y_r);
n_ = y.n_cols;
v_ = y.n_rows;
counter_ = 0;
conv_crit_ = 1.0;
//find appropriate spots to put in b_weights_r and omega_weights so that we can build sigma
uvec b_spots = find(as<arma::mat>(b_r));
uvec omega_spots = arma::find(trimatl(as<arma::mat>(omega_r)));
vec weights = as<arma::vec>(weights_r);
mat omega_weights(v_, v_, fill::zeros);
mat b_weights(v_, v_, fill::zeros);
vec means = vec(v_, fill::zeros); //non-zero means
if(meanEst){
means = as<arma::vec>(weights_r).head(v_);
b_weights.elem(b_spots) = as<arma::vec>(weights_r).subvec(v_, v_ + b_spots.n_elem - 1);
omega_weights.elem(omega_spots) = weights.subvec(v_ + b_spots.n_elem, v_ + b_spots.n_elem + omega_spots.n_elem - 1);
omega_weights = symmatl(omega_weights);
} else {
b_weights.elem(b_spots) = weights.subvec(0, b_spots.n_elem - 1);
omega_weights.elem(omega_spots) = weights.subvec(b_spots.n_elem, b_spots.n_elem + omega_spots.n_elem - 1);
omega_weights = symmatl(omega_weights);
}
sigma_ = solve( (eye(v_, v_) - b_weights), solve( eye(v_, v_) - b_weights, omega_weights).t());
dual_ = as<arma::vec>(dual_r);
//
constraints_ = mat(v_ + (v_ *(v_ + 1) )/ 2, n_);
constraints_.rows(0, v_ - 1) = (eye(v_, v_) - b_weights) * y;
if(meanEst) {
constraints_.rows(0, v_ - 1).each_col() -= means;
}
int i,j,k;
k = v_;
for(i = 0; i < v_; i++) {
for(j = 0; j <= i; j++ ) {
constraints_.row(k) = (y.row(i) % y.row(j)) - sigma_(i,j);
k++;
}
}
d_ = (constraints_.t() * dual_) + 1.0;
}
void DynamicSlidingModeControllerTaskSpace::starting(const ros::Time& time)
{
// get joint positions
for(int i=0; i < joint_handles_.size(); i++)
{
joint_msr_states_.q(i) = joint_handles_[i].getPosition();
joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
joint_des_states_.q(i) = joint_msr_states_.q(i);
sigma_(i) = 0;
// coefficients > 0
Kd_(i) = 10;
gamma_(i) = 1;
alpha_(i) = 12;
lambda_(i) = 1;
k_(i) = 3;
}
//x_des_ = KDL::Frame(KDL::Rotation::RPY(0,0,0),KDL::Vector(-0.4,0.3,1.5));
cmd_flag_ = 0;
step_ = 0;
}
std::tuple<float_type, float_type> operator()(float_type rr, unsigned a, unsigned b) const
{
return lennard_jones_kernel::compute(rr, sigma_(a,b)*sigma_(a,b), epsilon_(a,b));
}
void LDAModel::train()
{
if (descriptor_matrix_.cols() == 0)
{
throw Exception::InconsistentUsage(__FILE__, __LINE__, "Data must be read into the model before training!");
}
readLabels();
// map values of Y to their index
map<int, unsigned int> label_to_pos;
for (unsigned int i = 0; i < labels_.size(); i++)
{
label_to_pos.insert(make_pair(labels_[i], i));
}
// calculate sigma_ = covariance matrix of descriptors
sigma_.resize(descriptor_matrix_.cols(), descriptor_matrix_.cols());
for (int i = 0; i < descriptor_matrix_.cols(); i++)
{
double mi = Statistics::getMean(descriptor_matrix_, i);
for (int j = 0; j < descriptor_matrix_.cols(); j++)
{
sigma_(i, j) = Statistics::getCovariance(descriptor_matrix_, i, j, mi);
}
}
Eigen::MatrixXd I(sigma_.cols(), sigma_.cols()); I.setIdentity();
I *= lambda_;
sigma_ += I;
sigma_ = sigma_.inverse();
mean_vectors_.resize(Y_.cols());
no_substances_.clear();
no_substances_.resize(labels_.size(), 0);
for (int c = 0; c < Y_.cols(); c++)
{
vector < int > no_substances_c(labels_.size(), 0); // no of substances in each class for activitiy c
mean_vectors_[c].resize(labels_.size(), descriptor_matrix_.cols());
mean_vectors_[c].setZero();
for (int i = 0; i < descriptor_matrix_.rows(); i++) // calculate sum vector of each class
{
int yi = static_cast < int > (Y_(i, c)); // Y_ will contains only ints for classifications
int pos = label_to_pos.find(yi)->second;
for (int j = 0; j < descriptor_matrix_.cols(); j++)
{
mean_vectors_[c](pos, j) += descriptor_matrix_(i, j);
}
if (c == 0) no_substances_c[pos]++;
}
for (int i = 0; i < mean_vectors_[c].rows(); i++) // calculate mean vector of each class
{
if (no_substances_c[i] == 0)
{
mean_vectors_[c].row(i).setConstant(std::numeric_limits<double>::infinity()); // set mean of classes NOT occurring for activitiy c to inf
}
for (int j = 0; j < descriptor_matrix_.cols(); j++)
{
mean_vectors_[c](i, j) = mean_vectors_[c](i, j)/no_substances_c[i];
}
}
for (unsigned int i = 0; i < no_substances_.size(); i++) // update overall number of substances per class
{
no_substances_[i] += no_substances_c[i];
}
}
}
Real sigma() const {
return sigma_(0.0);
}
std::tuple<float_type, float_type> operator()(float_type rr, unsigned a, unsigned b) const
{
return modified_lennard_jones_kernel::compute(rr, sigma_(a,b)*sigma_(a,b), epsilon_(a,b)
, index_m_(a,b) / 2, index_n_(a,b) / 2);
}
