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;

    }
Exemple #2
0
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 &center) const {
   return logL(center.s_);
 }