math.cpp

Go to the documentation of this file.

#include "math.h"
 
#include <Eigen/Eigenvalues>
 
namespace armarx::control::common
{
 
    MultivariateNormal::MultivariateNormal(const Eigen::VectorXf& mu, float std)
    {
        mean = mu;
        covariance = Eigen::MatrixXf::Identity(mean.size(), mean.size()) * std::pow(std, 2);
    }
 
    float
    MultivariateNormal::pdf(const Eigen::VectorXf& x) const
    {
        float n = x.rows();
        float sqrt2pi = std::sqrt(2 * M_PI);
        float quadform = (x - mean).transpose() * covariance.inverse() * (x - mean);
        float norm = std::pow(sqrt2pi, -n) * std::pow(covariance.determinant(), -0.5);
 
        return norm * exp(-0.5 * quadform);
    }
 
    Eigen::VectorXf
    MultivariateNormal::pdf_gradient(const Eigen::VectorXf& x) const
    {
        float probability = pdf(x);
        return -probability * (x - mean) / covariance.determinant();
    }
 
    Eigen::VectorXf
    MultivariateNormal::sample(unsigned int nr_iterations) const
    {
        int n = mean.rows();
 
        // Generate x from the N(0, I) distribution
        Eigen::VectorXf x(n);
        Eigen::VectorXf sum(n);
        sum.setZero();
        for (unsigned int i = 0; i < nr_iterations; i++)
        {
            x.setRandom();
            x = 0.5 * (x + Eigen::VectorXf::Ones(n));
            sum = sum + x;
        }
        sum = sum - (static_cast<double>(nr_iterations) / 2) * Eigen::VectorXf::Ones(n);
        x = sum / (std::sqrt(static_cast<double>(nr_iterations) / 12));
 
        // Find the eigen vectors of the covariance matrix
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigen_solver(covariance);
        Eigen::MatrixXf eigenvectors = eigen_solver.eigenvectors().real();
 
        // Find the eigenvalues of the covariance matrix
        Eigen::MatrixXf eigenvalues = eigen_solver.eigenvalues().real().asDiagonal();
 
        // Find the transformation matrix
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es(eigenvalues);
        Eigen::MatrixXf sqrt_eigenvalues = es.operatorSqrt();
        Eigen::MatrixXf Q = eigenvectors * sqrt_eigenvalues;
 
        return Q * x + mean;
    }
 
    Eigen::VectorXf
    pdf_gradient(const Eigen::VectorXf& mean, float std, const Eigen::VectorXf& x)
    {
        Eigen::MatrixXf covariance =
            Eigen::MatrixXf::Identity(mean.size(), mean.size()) * std::pow(std, 2);
        float n = x.rows();
        float sqrt2pi = std::sqrt(2 * M_PI);
        float quadform = (x - mean).transpose() * covariance.inverse() * (x - mean);
        float norm = std::pow(sqrt2pi, -n) * std::pow(covariance.determinant(), -0.5);
 
        float probability = norm * exp(-0.5 * quadform);
        return -probability * (x - mean) / covariance.determinant();
    }
 
} // namespace armarx::control::common