arma::mat CartesianDMPLearner::computeFitY(arma::vec& time, arma::mat &y, arma::mat &dy, arma::mat &ddy, arma::vec& vec_g) {
// position
mat retMat(y.n_cols - 1, y.n_rows);
for(int i = 0; i < y.n_rows; ++i) {
for(int j = 0; j < 3; ++j) {
double yVal = y(i, j);
double dyVal = dy(i, j);
double ddyVal = ddy(i, j);
retMat(j, i) = /*1 / (vec_g(j) - y(0, j)) */ tau * tau * ddyVal - az * (bz * (vec_g(j) - yVal) - tau * dyVal);
}
}
// orientation
arma::mat omega(y.n_rows, 3);
arma::mat domega;
arma::mat eta;
arma::mat deta;
for (int j = 0; j < y.n_rows - 1; ++j) {
vec logL= log(tf::Quaternion(y(j + 1, 3), y(j + 1, 4), y(j + 1, 5), y(j + 1, 6)) * tf::Quaternion(y(j, 3), y(j, 4), y(j, 5), y(j, 6)).inverse());
for (int i = 0; i < 3; i++)
omega(j, i) = 2 * logL(i) / (time(1)-time(0));
if (j == y.n_rows - 2)
for (int i = 0; i < 3; i++)
omega(y.n_rows - 1, i) = 2 * logL(i) / (time(1)-time(0));
}
for(int i = 0; i < 3 ; ++i) {
vec trajectory = omega.col(i);
vec domegaV = computeDiscreteDerivatives(time, trajectory);
domega = join_rows(domega, domegaV);
}
eta = tau * omega;
deta = tau * domega;
for (int i = 0; i < y.n_rows; ++i) {
vec logL = log(tf::Quaternion(vec_g(3), vec_g(4), vec_g(5), vec_g(6)) * tf::Quaternion(y(i, 3), y(i, 4), y(i, 5), y(i, 6)).inverse());
for (int j = 3; j < retMat.n_rows; ++j)
retMat(j, i) = tau * deta (i, j - 3) - az * (bz * 2 * logL(j - 3) - eta(i, j - 3));
}
return retMat;
}
double twoLogLambda(double k1, double k2, double n1, double n2) {
double p = (k1 + k2) / (n1 + n2);
return 2.0 * (logL(k1 / n1, k1, n1) + logL(k2 / n2, k2, n2) - logL(p, k1, n1) - logL(p, k2, n2));
}
double logL(const ExpandedKMer ¢er) const {
return logL(center.s_);
}
