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