void SwaptionVolCube1::sabrCalibrationSection(
const Cube& marketVolCube,
Cube& parametersCube,
const Period& swapTenor) const {
const std::vector<Time>& optionTimes = marketVolCube.optionTimes();
const std::vector<Time>& swapLengths = marketVolCube.swapLengths();
const std::vector<Date>& optionDates = marketVolCube.optionDates();
const std::vector<Period>& swapTenors = marketVolCube.swapTenors();
Size k = std::find(swapTenors.begin(), swapTenors.end(),
swapTenor) - swapTenors.begin();
QL_REQUIRE(k != swapTenors.size(), "swap tenor not found");
std::vector<Real> calibrationResult(8,0.);
const std::vector<Matrix>& tmpMarketVolCube = marketVolCube.points();
std::vector<Real> strikes(strikeSpreads_.size());
std::vector<Real> volatilities(strikeSpreads_.size());
for (Size j=0; j<optionTimes.size(); j++) {
Rate atmForward = atmStrike(optionDates[j], swapTenors[k]);
strikes.clear();
volatilities.clear();
for (Size i=0; i<nStrikes_; i++){
Real strike = atmForward+strikeSpreads_[i];
if(strike>=MINSTRIKE) {
strikes.push_back(strike);
volatilities.push_back(tmpMarketVolCube[i][j][k]);
}
}
const std::vector<Real>& guess = parametersGuess_.operator()(
optionTimes[j], swapLengths[k]);
const boost::shared_ptr<SABRInterpolation> sabrInterpolation =
boost::shared_ptr<SABRInterpolation>(new
SABRInterpolation(strikes.begin(), strikes.end(),
volatilities.begin(),
optionTimes[j], atmForward,
guess[0], guess[1],
guess[2], guess[3],
isParameterFixed_[0],
isParameterFixed_[1],
isParameterFixed_[2],
isParameterFixed_[3],
vegaWeightedSmileFit_,
endCriteria_,
optMethod_,
errorAccept_,
useMaxError_,
maxGuesses_));
sabrInterpolation->update();
Real interpolationError = sabrInterpolation->rmsError();
calibrationResult[0]=sabrInterpolation->alpha();
calibrationResult[1]=sabrInterpolation->beta();
calibrationResult[2]=sabrInterpolation->nu();
calibrationResult[3]=sabrInterpolation->rho();
calibrationResult[4]=atmForward;
calibrationResult[5]=interpolationError;
calibrationResult[6]=sabrInterpolation->maxError();
calibrationResult[7]=sabrInterpolation->endCriteria();
QL_ENSURE(calibrationResult[7]!=EndCriteria::MaxIterations,
"section calibration failed: "
"option tenor " << optionDates[j] <<
", swap tenor " << swapTenors[k] <<
": max iteration (" <<
endCriteria_->maxIterations() << ")" <<
", alpha " << calibrationResult[0]<<
", beta " << calibrationResult[1] <<
", nu " << calibrationResult[2] <<
", rho " << calibrationResult[3] <<
", max error " << calibrationResult[6] <<
", error " << calibrationResult[5]
);
QL_ENSURE(useMaxError_ ? calibrationResult[6] : calibrationResult[5] < maxErrorTolerance_,
"section calibration failed: "
"option tenor " << optionDates[j] <<
", swap tenor " << swapTenors[k] <<
(useMaxError_ ? ": max error " : ": error ") <<
(useMaxError_ ? calibrationResult[6] : calibrationResult[5]) <<
", alpha " << calibrationResult[0] <<
", beta " << calibrationResult[1] <<
", nu " << calibrationResult[2] <<
", rho " << calibrationResult[3] <<
(useMaxError_ ? ": error" : ": max error ") <<
(useMaxError_ ? calibrationResult[5] : calibrationResult[6])
);
parametersCube.setPoint(optionDates[j], swapTenors[k],
optionTimes[j], swapLengths[k],
calibrationResult);
parametersCube.updateInterpolators();
}
}
bool ClosedFormCalibration::calibrate(const MotionInformationVector& measurements, SE2& laserOffset, Eigen::Vector3d& odomParams)
{
std::vector<VelocityMeasurement, Eigen::aligned_allocator<VelocityMeasurement> > velMeasurements;
for (size_t i = 0; i < measurements.size(); ++i) {
const SE2& odomMotion = measurements[i].odomMotion;
const double& timeInterval = measurements[i].timeInterval;
MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm);
velMeasurements.push_back(velMeas);
}
double J_21, J_22;
{
Eigen::MatrixXd A(measurements.size(), 2);
Eigen::VectorXd x(measurements.size());
for (size_t i = 0; i < measurements.size(); ++i) {
const SE2& laserMotion = measurements[i].laserMotion;
const double& timeInterval = measurements[i].timeInterval;
const VelocityMeasurement& velMeas = velMeasurements[i];
A(i, 0) = velMeas.vl() * timeInterval;
A(i, 1) = velMeas.vr() * timeInterval;
x(i) = laserMotion.rotation().angle();
}
// (J_21, J_22) = (-r_l / b, r_r / b)
Eigen::Vector2d linearSolution = (A.transpose() * A).inverse() * A.transpose() * x;
//std::cout << linearSolution.transpose() << std::endl;
J_21 = linearSolution(0);
J_22 = linearSolution(1);
}
// construct M
Eigen::Matrix<double, 5, 5> M;
M.setZero();
for (size_t i = 0; i < measurements.size(); ++i) {
const SE2& laserMotion = measurements[i].laserMotion;
const double& timeInterval = measurements[i].timeInterval;
const VelocityMeasurement& velMeas = velMeasurements[i];
Eigen::Matrix<double, 2, 5> L;
double omega_o_k = J_21 * velMeas.vl() + J_22 * velMeas.vr();
double o_theta_k = omega_o_k * timeInterval;
double sx = 1.;
double sy = 0.;
if (fabs(o_theta_k) > std::numeric_limits<double>::epsilon()) {
sx = sin(o_theta_k) / o_theta_k;
sy = (1 - cos(o_theta_k)) / o_theta_k;
}
double c_x = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sx;
double c_y = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sy;
L(0, 0) = -c_x;
L(0, 1) = 1 - cos(o_theta_k);
L(0, 2) = sin(o_theta_k);
L(0, 3) = laserMotion.translation().x();
L(0, 4) = -laserMotion.translation().y();
L(1, 0) = -c_y;
L(1, 1) = - sin(o_theta_k);
L(1, 2) = 1 - cos(o_theta_k);
L(1, 3) = laserMotion.translation().y();
L(1, 4) = laserMotion.translation().x();
M.noalias() += L.transpose() * L;
}
//std::cout << M << std::endl;
// compute lagrange multiplier
// and solve the constrained least squares problem
double m11 = M(0,0);
double m13 = M(0,2);
double m14 = M(0,3);
double m15 = M(0,4);
double m22 = M(1,1);
double m34 = M(2,3);
double m35 = M(2,4);
double m44 = M(3,3);
double a = m11 * SQR(m22) - m22 * SQR(m13);
double b = 2*m11 * SQR(m22) * m44 - SQR(m22) * SQR(m14) - 2*m22 * SQR(m13) * m44 - 2*m11 * m22 * SQR(m34)
- 2*m11 * m22 * SQR(m35) - SQR(m22) * SQR(m15) + 2*m13 * m22 * m34 * m14 + SQR(m13) * SQR(m34)
+ 2*m13 * m22 * m35 * m15 + SQR(m13) * SQR(m35);
double c = - 2*m13 * CUBE(m35) * m15 - m22 * SQR(m13) * SQR(m44) + m11 * SQR(m22) * SQR(m44) + SQR(m13) * SQR(m35) * m44
+ 2*m13 * m22 * m34 * m14 * m44 + SQR(m13) * SQR(m34) * m44 - 2*m11 * m22 * SQR(m34) * m44 - 2 * m13 * CUBE(m34) * m14
- 2*m11 * m22 * SQR(m35) * m44 + 2*m11 * SQR(m35) * SQR(m34) + m22 * SQR(m14) * SQR(m35) - 2*m13 * SQR(m35) * m34 * m14
- 2*m13 * SQR(m34) * m35 * m15 + m11 * std::pow(m34, 4) + m22 * SQR(m15) * SQR(m34) + m22 * SQR(m35) * SQR(m15)
+ m11 * std::pow(m35, 4) - SQR(m22) * SQR(m14) * m44 + 2*m13 * m22 * m35 * m15 * m44 + m22 * SQR(m34) * SQR(m14)
- SQR(m22) * SQR(m15) * m44;
// solve the quadratic equation
double lambda1, lambda2;
if(a < std::numeric_limits<double>::epsilon()) {
if(b <= std::numeric_limits<double>::epsilon())
return false;
lambda1 = lambda2 = -c/b;
} else {
double delta = b*b - 4*a*c;
if (delta < 0)
return false;
lambda1 = 0.5 * (-b-sqrt(delta)) / a;
lambda2 = 0.5 * (-b+sqrt(delta)) / a;
}
Eigen::VectorXd x1 = solveLagrange(M, lambda1);
Eigen::VectorXd x2 = solveLagrange(M, lambda2);
double err1 = x1.dot(M * x1);
double err2 = x2.dot(M * x2);
const Eigen::VectorXd& calibrationResult = err1 < err2 ? x1 : x2;
odomParams(0) = - calibrationResult(0) * J_21;
odomParams(1) = calibrationResult(0) * J_22;
odomParams(2) = calibrationResult(0);
laserOffset = SE2(calibrationResult(1), calibrationResult(2), atan2(calibrationResult(4), calibrationResult(3)));
return true;
}