void compute(const QUESO::FullEnvironment& env) {
// Step 1 of 6: Instantiate the parameter space
QUESO::VectorSpace<QUESO::GslVector,QUESO::GslMatrix>
paramSpace(env, "param_", 2, NULL);
// Step 2 of 6: Instantiate the parameter domain
QUESO::GslVector paramMins(paramSpace.zeroVector());
paramMins.cwSet(-INFINITY);
QUESO::GslVector paramMaxs(paramSpace.zeroVector());
paramMaxs.cwSet( INFINITY);
QUESO::BoxSubset<QUESO::GslVector,QUESO::GslMatrix>
paramDomain("param_",paramSpace,paramMins,paramMaxs);
// Step 3 of 6: Instantiate the qoi space
QUESO::VectorSpace<QUESO::GslVector,QUESO::GslMatrix>
qoiSpace(env, "qoi_", 1, NULL);
// Step 4 of 6: Instantiate the qoi function object
qoiRoutine_DataType qoiRoutine_Data;
qoiRoutine_Data.coef1 = 1.;
qoiRoutine_Data.coef2 = 1.;
QUESO::GenericVectorFunction<QUESO::GslVector,QUESO::GslMatrix,
QUESO::GslVector,QUESO::GslMatrix>
qoiFunctionObj("qoi_",
paramDomain,
qoiSpace,
qoiRoutine,
(void *) &qoiRoutine_Data);
// Step 5 of 6: Instantiate the forward problem
// Parameters are Gaussian RV
QUESO::GslVector meanVector( paramSpace.zeroVector() );
meanVector[0] = -1;
meanVector[1] = 2;
QUESO::GslMatrix covMatrix = QUESO::GslMatrix(paramSpace.zeroVector());
covMatrix(0,0) = 4.;
covMatrix(0,1) = 0.;
covMatrix(1,0) = 0.;
covMatrix(1,1) = 1.;
QUESO::GaussianVectorRV<QUESO::GslVector,QUESO::GslMatrix>
paramRv("param_",paramDomain,meanVector,covMatrix);
QUESO::GenericVectorRV<QUESO::GslVector,QUESO::GslMatrix>
qoiRv("qoi_", qoiSpace);
QUESO::StatisticalForwardProblem<QUESO::GslVector,QUESO::GslMatrix,
QUESO::GslVector,QUESO::GslMatrix>
fp("", NULL, paramRv, qoiFunctionObj, qoiRv);
// Step 6 of 6: Solve the forward problem
fp.solveWithMonteCarlo(NULL);
return;
}
void compute(const QUESO::FullEnvironment& env) {
// Step 1 of 5: Instantiate the parameter space
QUESO::VectorSpace<QUESO::GslVector,QUESO::GslMatrix>
paramSpace(env, "param_", 2, NULL);
// Step 2 of 5: Instantiate the parameter domain
QUESO::GslVector paramMins(paramSpace.zeroVector());
paramMins.cwSet(-INFINITY);
QUESO::GslVector paramMaxs(paramSpace.zeroVector());
paramMaxs.cwSet( INFINITY);
QUESO::BoxSubset<QUESO::GslVector,QUESO::GslMatrix>
paramDomain("param_",paramSpace,paramMins,paramMaxs);
// Step 3 of 5: Instantiate the likelihood function object
QUESO::GslVector meanVector(paramSpace.zeroVector());
meanVector[0] = -1;
meanVector[1] = 2;
QUESO::GslMatrix covMatrix(paramSpace.zeroVector());
covMatrix(0,0) = 4.; covMatrix(0,1) = 0.;
covMatrix(1,0) = 0.; covMatrix(1,1) = 1.;
likelihoodRoutine_DataType likelihoodRoutine_Data;
likelihoodRoutine_Data.meanVector = &meanVector;
likelihoodRoutine_Data.covMatrix = &covMatrix;
QUESO::GenericScalarFunction<QUESO::GslVector,QUESO::GslMatrix>
likelihoodFunctionObj("like_",
paramDomain,
likelihoodRoutine,
(void *) &likelihoodRoutine_Data,
true); // routine computes [ln(function)]
// Step 4 of 5: Instantiate the inverse problem
QUESO::UniformVectorRV<QUESO::GslVector,QUESO::GslMatrix>
priorRv("prior_", paramDomain);
QUESO::GenericVectorRV<QUESO::GslVector,QUESO::GslMatrix>
postRv("post_", paramSpace);
QUESO::StatisticalInverseProblem<QUESO::GslVector,QUESO::GslMatrix>
ip("", NULL, priorRv, likelihoodFunctionObj, postRv);
// Step 5 of 5: Solve the inverse problem
QUESO::GslVector paramInitials(paramSpace.zeroVector());
paramInitials[0] = 0.1;
paramInitials[1] = -1.4;
QUESO::GslMatrix proposalCovMatrix(paramSpace.zeroVector());
proposalCovMatrix(0,0) = 8.; proposalCovMatrix(0,1) = 4.;
proposalCovMatrix(1,0) = 4.; proposalCovMatrix(1,1) = 16.;
ip.solveWithBayesMetropolisHastings(NULL,paramInitials, &proposalCovMatrix);
return;
}
geometry_msgs::TwistWithCovariance CalibrateAction::calcTwistWithCov(std::vector<geometry_msgs::Twist> twists)
{
geometry_msgs::TwistWithCovariance resultTwist;
int n = twists.size();
MatrixXd mat(n,6);
// creating value matrix here
for (int i = 0; i<n; i++)
{
mat(i,0) = twists[i].linear.x;
mat(i,1) = twists[i].linear.y;
mat(i,2) = twists[i].linear.z;
mat(i,3) = twists[i].angular.x;
mat(i,4) = twists[i].angular.y;
mat(i,5) = twists[i].angular.z;
}
MatrixXd centered = mat.rowwise() - mat.colwise().mean();
MatrixXd covMat = MatrixXd::Ones(6,6);
covMat = (centered.adjoint() * centered) / double(mat.rows());
VectorXd meanVector(6);
meanVector = mat.colwise().mean();
// only need linear.x and angular.z but other values could be useful in the future
resultTwist.twist.linear.x = double(meanVector(0));
resultTwist.twist.linear.y = double(meanVector(1));
resultTwist.twist.linear.z = double(meanVector(2));
resultTwist.twist.angular.x = double(meanVector(3));
resultTwist.twist.angular.y = double(meanVector(4));
resultTwist.twist.angular.z = double(meanVector(5));
double *resultC = &(resultTwist.covariance[0]);
// arrange resultMatrix as RowMajor as required by TwistWithCovariance type
Map<Matrix<double,6,6,RowMajor> >(resultC, 6,6) = covMat;
// print for test only
//std::cout << "CovMat:\n" << covMat <<"\n";
return resultTwist;
}
void computeGravityAndTraveledDistance(const QUESO::FullEnvironment& env) {
struct timeval timevalNow;
gettimeofday(&timevalNow, NULL);
if (env.fullRank() == 0) {
std::cout << "\nBeginning run of 'Gravity + Projectile motion' example at "
<< ctime(&timevalNow.tv_sec)
<< "\n my fullRank = " << env.fullRank()
<< "\n my subEnvironmentId = " << env.subId()
<< "\n my subRank = " << env.subRank()
<< "\n my interRank = " << env.inter0Rank()
<< std::endl << std::endl;
}
// Just examples of possible calls
if ((env.subDisplayFile() ) &&
(env.displayVerbosity() >= 2)) {
*env.subDisplayFile() << "Beginning run of 'Gravity + Projectile motion' example at "
<< ctime(&timevalNow.tv_sec)
<< std::endl;
}
env.fullComm().Barrier();
env.subComm().Barrier(); // Just an example of a possible call
//================================================================
// Statistical inverse problem (SIP): find posterior PDF for 'g'
//================================================================
gettimeofday(&timevalNow, NULL);
if (env.fullRank() == 0) {
std::cout << "Beginning 'SIP -> Gravity estimation' at "
<< ctime(&timevalNow.tv_sec)
<< std::endl;
}
//------------------------------------------------------
// SIP Step 1 of 6: Instantiate the parameter space
//------------------------------------------------------
QUESO::VectorSpace<QUESO::GslVector,QUESO::GslMatrix> paramSpace(env, "param_", 1, NULL);
//------------------------------------------------------
// SIP Step 2 of 6: Instantiate the parameter domain
//------------------------------------------------------
QUESO::GslVector paramMinValues(paramSpace.zeroVector());
QUESO::GslVector paramMaxValues(paramSpace.zeroVector());
paramMinValues[0] = 8.;
paramMaxValues[0] = 11.;
QUESO::BoxSubset<QUESO::GslVector,QUESO::GslMatrix>
paramDomain("param_",
paramSpace,
paramMinValues,
paramMaxValues);
//------------------------------------------------------
// SIP Step 3 of 6: Instantiate the likelihood function
// object to be used by QUESO.
//------------------------------------------------------
likelihoodRoutine_Data likelihoodRoutine_Data(env);
QUESO::GenericScalarFunction<QUESO::GslVector,QUESO::GslMatrix>
likelihoodFunctionObj("like_",
paramDomain,
likelihoodRoutine,
(void *) &likelihoodRoutine_Data,
true); // the routine computes [ln(function)]
//------------------------------------------------------
// SIP Step 4 of 6: Define the prior RV
//------------------------------------------------------
#ifdef PRIOR_IS_GAUSSIAN
QUESO::GslVector meanVector( paramSpace.zeroVector() );
meanVector[0] = 9;
QUESO::GslMatrix covMatrix = QUESO::GslMatrix(paramSpace.zeroVector());
covMatrix(0,0) = 1.;
// Create a Gaussian prior RV
QUESO::GaussianVectorRV<QUESO::GslVector,QUESO::GslMatrix> priorRv("prior_",paramDomain,meanVector,covMatrix);
#else
// Create an uniform prior RV
QUESO::UniformVectorRV<QUESO::GslVector,QUESO::GslMatrix> priorRv("prior_",paramDomain);
#endif
//------------------------------------------------------
// SIP Step 5 of 6: Instantiate the inverse problem
//------------------------------------------------------
QUESO::GenericVectorRV<QUESO::GslVector,QUESO::GslMatrix>
postRv("post_", // Extra prefix before the default "rv_" prefix
paramSpace);
QUESO::StatisticalInverseProblem<QUESO::GslVector,QUESO::GslMatrix>
ip("", // No extra prefix before the default "ip_" prefix
NULL,
priorRv,
likelihoodFunctionObj,
postRv);
//------------------------------------------------------
// SIP Step 6 of 6: Solve the inverse problem, that is,
// set the 'pdf' and the 'realizer' of the posterior RV
//------------------------------------------------------
std::cout << "Solving the SIP with Metropolis Hastings"
<< std::endl << std::endl;
QUESO::GslVector paramInitials(paramSpace.zeroVector());
priorRv.realizer().realization(paramInitials);
QUESO::GslMatrix proposalCovMatrix(paramSpace.zeroVector());
proposalCovMatrix(0,0) = std::pow( fabs(paramInitials[0])/20. , 2. );
ip.solveWithBayesMetropolisHastings(NULL, paramInitials, &proposalCovMatrix);
//================================================================
// Statistical forward problem (SFP): find the max distance
// traveled by an object in projectile motion; input pdf for 'g'
// is the solution of the SIP above.
//================================================================
gettimeofday(&timevalNow, NULL);
std::cout << "Beginning 'SFP -> Projectile motion' at "
<< ctime(&timevalNow.tv_sec)
<< std::endl;
//------------------------------------------------------
// SFP Step 1 of 6: Instantiate the parameter *and* qoi spaces.
// SFP input RV = FIP posterior RV, so SFP parameter space
// has been already defined.
//------------------------------------------------------
QUESO::VectorSpace<QUESO::GslVector,QUESO::GslMatrix> qoiSpace(env, "qoi_", 1, NULL);
//------------------------------------------------------
// SFP Step 2 of 6: Instantiate the parameter domain
//------------------------------------------------------
// Not necessary because input RV of the SFP = output RV of SIP.
// Thus, the parameter domain has been already defined.
//------------------------------------------------------
// SFP Step 3 of 6: Instantiate the qoi function object
// to be used by QUESO.
//------------------------------------------------------
qoiRoutine_Data qoiRoutine_Data;
qoiRoutine_Data.m_angle = M_PI/4.0; //45 degrees (radians)
qoiRoutine_Data.m_initialVelocity= 5.; //initial speed (m/s)
qoiRoutine_Data.m_initialHeight = 0.; //initial height (m)
QUESO::GenericVectorFunction<QUESO::GslVector,QUESO::GslMatrix,QUESO::GslVector,QUESO::GslMatrix>
qoiFunctionObj("qoi_",
paramDomain,
qoiSpace,
qoiRoutine,
(void *) &qoiRoutine_Data);
//------------------------------------------------------
// SFP Step 4 of 6: Define the input RV
//------------------------------------------------------
// Not necessary because input RV of SFP = output RV of SIP
// (postRv).
//------------------------------------------------------
// SFP Step 5 of 6: Instantiate the forward problem
//------------------------------------------------------
QUESO::GenericVectorRV<QUESO::GslVector,QUESO::GslMatrix> qoiRv("qoi_", qoiSpace);
QUESO::StatisticalForwardProblem<QUESO::GslVector,QUESO::GslMatrix,QUESO::GslVector,QUESO::GslMatrix>
fp("",
NULL,
postRv,
qoiFunctionObj,
qoiRv);
//------------------------------------------------------
// SFP Step 6 of 6: Solve the forward problem
//------------------------------------------------------
std::cout << "Solving the SFP with Monte Carlo"
<< std::endl << std::endl;
fp.solveWithMonteCarlo(NULL);
//------------------------------------------------------
gettimeofday(&timevalNow, NULL);
if ((env.subDisplayFile() ) &&
(env.displayVerbosity() >= 2)) {
*env.subDisplayFile() << "Ending run of 'Gravity + Projectile motion' example at "
<< ctime(&timevalNow.tv_sec)
<< std::endl;
}
if (env.fullRank() == 0) {
std::cout << "Ending run of 'Gravity + Projectile motion' example at "
<< ctime(&timevalNow.tv_sec)
<< std::endl;
}
return;
}
