void mouseDemo() {
mouseinitial();
_DBG_("Intialised mouse");
int sta = 0;
while(sta != 4) {
switch(sta) {
case 0:
while (coord_x<200) {
forwards(20);
}
sta = 1;
break;
case 1:
while (convertToDeg(theta) > -90) {
spinLeft();
}
sta = 2;
break;
case 2:
while (coord_y<100) {
forwards(20);
}
sta = 3;
break;
case 3:
while (convertToDeg(theta) < 90 ) {
spinRight();
}
sta = 4;
break;
}
}
}
void fullDemo() {
int robotState = 0;
uint16_t sensorPattern[5] = {0};
while(robotState != 6) {
switch(robotState) {
case 0:
while(get_coord_x() < 200) {
forwards(20);
}
robotState = 1;
break;
case 1:
setSensorSide(1);
while(get_coord_x() < 400) {
correctForwardMotion();
}
robotState = 2;
break;
case 2:
while(convertToDeg(get_theta()) > -90) {
spinLeft();
}
while(get_coord_y() < 120) {
forwards(20);
}
while(convertToDeg(get_theta()) < 0) {
spinRight();
}
while(get_coord_x() < 600) {
forwards(20);
}
robotState = 3;
break;
case 3:
setSensorSide(2);
while(get_coord_x() < 800) {
correctForwardMotion();
}
robotState = 4;
break;
case 4:
forwards(20);
while(sensorPattern[3]<2000){
getRawSensors(sensorPattern);
}
robotState = 5;
break;
case 5:
followLine();
robotState = 6;
break;
}
}
}
int stFindIterImp::next()
{
if ( !state_.reverse() )
return forwards();
else
return backwards();
}
void doShortCourse(int state) {
int nextState = 0;
switch (state) {
case 0: { //move 2 metres to the wall
forwards(20);
while (!wallFound(1)) {
}
nextState = 1;
break;
}
case 1: { //found a wall follow it
setSensorSide(1);//follow left wall
while (wallFound(1)) {
correctForwardMotion();
}
nextState = 2;
break;
}
case 2: { //wall ended find a line
forwards(20);
while (!checkForLine()) {
}
nextState = 3;
break;
}
case 3: { //line found follow it
lineFollow();
nextState = 4;
break;
}
case 4: { // docked please stop.
while(wallFound(3)) {
}
stopLineFollow();
nextState = 7;
break;
}
}
if (nextState != 7){
doShortCourse(nextState);
}
}
void keypress(unsigned char key, int x, int y){
switch (key)
{
case 'q':
exit(0);
break;
case 'w':
forwards();
break;
case 's':
backwards();
break;
}
}
void doIdle()
{
/* do some stuff... */
if (recordingAndAnimating) //If the menu item to record and animate has been called
{
//Record current Frame
char name[10];
_snprintf_s(name, 10, "%03d.jpg", currentRecordingFrame);
saveScreenshot(name);
currentRecordingFrame++;
//animate
forwards();
}
/* make the screen update */
glutPostRedisplay();
}
void controller()
{
if(x > 0)
{
right();
//digitalWrite(ledpin, HIGH); // turn ON the LED
}else if (x < 0)
{
left();
//digitalWrite(ledpin, LOW); // turn ON the LED
}
if(y > 0)
{
forwards();
//digitalWrite(ledpin, HIGH); // turn ON the LED
}else if(y < 0)
{
backwards();
//digitalWrite(ledpin, LOW); // turn ON the LED
}
if(z > 0)
{
up();
//digitalWrite(ledpin, HIGH); // turn ON the LED
}else if(z < 0){
down();
//digitalWrite(ledpin, LOW); // turn ON the LED
}
if(rotation > 0)
{
rotateLeft();
//digitalWrite(ledpin, HIGH); // turn ON the LED
}else if(rotation < 0)
{
rotateRight();
//digitalWrite(ledpin, LOW); // turn ON the LED
}
}
void Gaussian1dCapFloorEngine::calculate() const {
for (Size i = 0; i < arguments_.spreads.size(); i++)
QL_REQUIRE(arguments_.spreads[i] == 0.0,
"Non zero spreads (" << arguments_.spreads[i]
<< ") are not allowed.");
Size optionlets = arguments_.startDates.size();
std::vector<Real> values(optionlets, 0.0);
std::vector<Real> forwards(optionlets, 0.0);
Real value = 0.0;
Date settlement = model_->termStructure()->referenceDate();
CapFloor::Type type = arguments_.type;
Array z = model_->yGrid(stddevs_, integrationPoints_);
Array p(z.size());
for (Size i = 0; i < optionlets; ++i) {
Date valueDate = arguments_.startDates[i];
Date paymentDate = arguments_.endDates[i];
boost::shared_ptr<IborIndex> iborIndex =
boost::dynamic_pointer_cast<IborIndex>(arguments_.indexes[i]);
// if we do not find an ibor index with associated forwarding curve
// we fall back on the model curve
if (paymentDate > settlement) {
Real f = arguments_.nominals[i] * arguments_.gearings[i];
Date fixingDate = arguments_.fixingDates[i];
Time fixingTime =
model_->termStructure()->timeFromReference(fixingDate);
Real strike;
if (type == CapFloor::Cap || type == CapFloor::Collar) {
strike = arguments_.capRates[i];
if (fixingDate <= settlement) {
values[i] =
std::max(arguments_.forwards[i] - strike, 0.0) * f *
arguments_.accrualTimes[i];
} else {
// todo add openmp support later on (as in gaussian1dswaptionengine)
for (Size j = 0; j < z.size(); j++) {
Real floatingLegNpv;
if (iborIndex != NULL)
floatingLegNpv =
arguments_.accrualTimes[i] *
model_->forwardRate(fixingDate, fixingDate,
z[j], iborIndex) *
model_->zerobond(paymentDate, fixingDate,
z[j], discountCurve_);
else
floatingLegNpv =
(model_->zerobond(valueDate, fixingDate,
z[j]) -
model_->zerobond(paymentDate, fixingDate,
z[j]));
Real fixedLegNpv =
arguments_.capRates[i] *
arguments_.accrualTimes[i] *
model_->zerobond(paymentDate, fixingDate, z[j]);
p[j] =
std::max((floatingLegNpv - fixedLegNpv), 0.0) /
model_->numeraire(fixingTime, z[j],
discountCurve_);
}
CubicInterpolation payoff(
z.begin(), z.end(), p.begin(),
CubicInterpolation::Spline, true,
CubicInterpolation::Lagrange, 0.0,
CubicInterpolation::Lagrange, 0.0);
Real price = 0.0;
for (Size j = 0; j < z.size() - 1; j++) {
price += model_->gaussianShiftedPolynomialIntegral(
0.0, payoff.cCoefficients()[j],
payoff.bCoefficients()[j],
payoff.aCoefficients()[j], p[j], z[j], z[j],
z[j + 1]);
}
if (extrapolatePayoff_) {
if (flatPayoffExtrapolation_) {
price +=
model_->gaussianShiftedPolynomialIntegral(
0.0, 0.0, 0.0, 0.0, p[z.size() - 2],
z[z.size() - 2], z[z.size() - 1],
100.0);
price +=
model_->gaussianShiftedPolynomialIntegral(
0.0, 0.0, 0.0, 0.0, p[0], z[0], -100.0,
z[0]);
} else {
price +=
model_->gaussianShiftedPolynomialIntegral(
0.0,
payoff.cCoefficients()[z.size() - 2],
payoff.bCoefficients()[z.size() - 2],
payoff.aCoefficients()[z.size() - 2],
p[z.size() - 2], z[z.size() - 2],
z[z.size() - 1], 100.0);
}
}
values[i] =
price *
model_->numeraire(0.0, 0.0, discountCurve_) * f;
}
}
if (type == CapFloor::Floor || type == CapFloor::Collar) {
strike = arguments_.floorRates[i];
Real floorlet;
if (fixingDate <= settlement) {
floorlet =
std::max(-(arguments_.forwards[i] - strike), 0.0) *
f * arguments_.accrualTimes[i];
} else {
for (Size j = 0; j < z.size(); j++) {
Real floatingLegNpv;
if (iborIndex != NULL)
floatingLegNpv =
arguments_.accrualTimes[i] *
model_->forwardRate(fixingDate, fixingDate,
z[j], iborIndex) *
model_->zerobond(paymentDate, fixingDate,
z[j], discountCurve_);
else
floatingLegNpv =
(model_->zerobond(valueDate, fixingDate,
z[j]) -
model_->zerobond(paymentDate, fixingDate,
z[j]));
Real fixedLegNpv =
arguments_.floorRates[i] *
arguments_.accrualTimes[i] *
model_->zerobond(paymentDate, fixingDate, z[j]);
p[j] =
std::max(-(floatingLegNpv - fixedLegNpv), 0.0) /
model_->numeraire(fixingTime, z[j],
discountCurve_);
}
CubicInterpolation payoff(
z.begin(), z.end(), p.begin(),
CubicInterpolation::Spline, true,
CubicInterpolation::Lagrange, 0.0,
CubicInterpolation::Lagrange, 0.0);
Real price = 0.0;
for (Size j = 0; j < z.size() - 1; j++) {
price += model_->gaussianShiftedPolynomialIntegral(
0.0, payoff.cCoefficients()[j],
payoff.bCoefficients()[j],
payoff.aCoefficients()[j], p[j], z[j], z[j],
z[j + 1]);
}
if (extrapolatePayoff_) {
if (flatPayoffExtrapolation_) {
price +=
model_->gaussianShiftedPolynomialIntegral(
0.0, 0.0, 0.0, 0.0, p[z.size() - 2],
z[z.size() - 2], z[z.size() - 1],
100.0);
price +=
model_->gaussianShiftedPolynomialIntegral(
0.0, 0.0, 0.0, 0.0, p[0], z[0], -100.0,
z[0]);
} else {
price +=
model_->gaussianShiftedPolynomialIntegral(
0.0, payoff.cCoefficients()[0],
payoff.bCoefficients()[0],
payoff.aCoefficients()[0], p[0], z[0],
-100.0, z[0]);
}
}
floorlet = price *
model_->numeraire(0.0, 0.0, discountCurve_) *
f;
}
if (type == CapFloor::Floor) {
values[i] = floorlet;
} else {
// a collar is long a cap and short a floor
values[i] -= floorlet;
}
}
value += values[i];
}
}
results_.value = value;
results_.additionalResults["optionletsPrice"] = values;
results_.additionalResults["optionletsAtmForward"] = forwards;
}
void wallFollow(SensorPair sensor) {
if ((sensor.FrontSensor > WALL_DISTANCE) && (sensor.RearSensor > WALL_DISTANCE)) { // both sensors are over the desired distance
if(sensor.RearSensor > sensor.FrontSensor) {
forwards(20); //keep on going straight until one of the sensors is at the desired distance.
// debug_output(sensor);
}
else { //if we are either parallel or facing away from the wall
if (sensorSide == 1) {
setRightMotorFw(20 + (sensor.FrontSensor - WALL_DISTANCE));
setLeftMotorFw(20);
}
else if (sensorSide == 2) {
setLeftMotorFw(20 + (sensor.FrontSensor - WALL_DISTANCE));
setRightMotorFw(20);
}
}
}
else if ((sensor.FrontSensor < WALL_DISTANCE) && (sensor.RearSensor < WALL_DISTANCE)) { // both sensors less closer to the wall than required
if (sensor.RearSensor > sensor.FrontSensor) { //Front sensor is closest to the wall and under the desired distance
if (sensorSide == 1) {
motorPair motorInfo = getSpeedLeft();
setLeftMotorFw(motorInfo.motor_speed + (WALL_DISTANCE - sensor.FrontSensor));
}
else if (sensorSide == 2) {
motorPair motorInfo = getSpeedRight();
setRightMotorFw(motorInfo.motor_speed + (WALL_DISTANCE - sensor.FrontSensor));
}
// debug_output(sensor);
}
else if(sensor.RearSensor < sensor.FrontSensor) {//Rear sensor is closest to the wall and under the desired distance
forwards(20);
// debug_output(sensor);
}
}
else if ((sensor.FrontSensor <= WALL_DISTANCE) && (sensor.RearSensor > WALL_DISTANCE)) { // both sensors less closer to the wall than required
if (sensorSide == 1) {
motorPair motorInfo = getSpeedLeft();
setLeftMotorFw(motorInfo.motor_speed + (WALL_DISTANCE - sensor.FrontSensor));
}
else if (sensorSide == 2) {
motorPair motorInfo = getSpeedRight();
setRightMotorFw(motorInfo.motor_speed + (WALL_DISTANCE - sensor.FrontSensor));
}
// debug_output(sensor);
}
else if ((sensor.FrontSensor > WALL_DISTANCE) && (sensor.RearSensor <= WALL_DISTANCE)) { // both sensors less closer to the wall than required
if (sensorSide == 1) {
motorPair motorInfo = getSpeedRight();
setRightMotorFw(motorInfo.motor_speed + (sensor.FrontSensor - WALL_DISTANCE));
}
else if (sensorSide == 2) {
motorPair motorInfo = getSpeedLeft();
setLeftMotorFw(motorInfo.motor_speed + (sensor.FrontSensor - WALL_DISTANCE ));
}
// debug_output(sensor);
}
else if(sensor.RearSensor < sensor.FrontSensor) {//Rear sensor is closest to the wall and under the desired distance
forwards(20);
// debug_output(sensor);
}
else if ((sensor.FrontSensor == WALL_DISTANCE) && (sensor.RearSensor == WALL_DISTANCE)) {//if at the required distance carry on all is well
forwards(20);
}
else {
cmdDoPlay(">>>a"); //error beep
forwards(20);
debug_output(sensor);
}
}
SwaptionVolCube1::Cube
SwaptionVolCube1::sabrCalibration(const Cube& marketVolCube) 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();
Matrix alphas(optionTimes.size(), swapLengths.size(),0.);
Matrix betas(alphas);
Matrix nus(alphas);
Matrix rhos(alphas);
Matrix forwards(alphas);
Matrix errors(alphas);
Matrix maxErrors(alphas);
Matrix endCriteria(alphas);
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++) {
for (Size k=0; k<swapLengths.size(); k++) {
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 rmsError = sabrInterpolation->rmsError();
Real maxError = sabrInterpolation->maxError();
alphas [j][k] = sabrInterpolation->alpha();
betas [j][k] = sabrInterpolation->beta();
nus [j][k] = sabrInterpolation->nu();
rhos [j][k] = sabrInterpolation->rho();
forwards [j][k] = atmForward;
errors [j][k] = rmsError;
maxErrors [j][k] = maxError;
endCriteria[j][k] = sabrInterpolation->endCriteria();
QL_ENSURE(endCriteria[j][k]!=EndCriteria::MaxIterations,
"global swaptions calibration failed: "
"MaxIterations reached: " << "\n" <<
"option maturity = " << optionDates[j] << ", \n" <<
"swap tenor = " << swapTenors[k] << ", \n" <<
"error = " << io::rate(errors[j][k]) << ", \n" <<
"max error = " << io::rate(maxErrors[j][k]) << ", \n" <<
" alpha = " << alphas[j][k] << "n" <<
" beta = " << betas[j][k] << "\n" <<
" nu = " << nus[j][k] << "\n" <<
" rho = " << rhos[j][k] << "\n"
);
QL_ENSURE(useMaxError_ ? maxError : rmsError < maxErrorTolerance_,
"global swaptions calibration failed: "
"option tenor " << optionDates[j] <<
", swap tenor " << swapTenors[k] <<
(useMaxError_ ? ": max error " : ": error") <<
(useMaxError_ ? maxError : rmsError) <<
" alpha = " << alphas[j][k] << "n" <<
" beta = " << betas[j][k] << "\n" <<
" nu = " << nus[j][k] << "\n" <<
" rho = " << rhos[j][k] << "\n" <<
(useMaxError_ ? ": error" : ": max error ") <<
(useMaxError_ ? rmsError :maxError)
);
}
}
Cube sabrParametersCube(optionDates, swapTenors,
optionTimes, swapLengths, 8);
sabrParametersCube.setLayer(0, alphas);
sabrParametersCube.setLayer(1, betas);
sabrParametersCube.setLayer(2, nus);
sabrParametersCube.setLayer(3, rhos);
sabrParametersCube.setLayer(4, forwards);
sabrParametersCube.setLayer(5, errors);
sabrParametersCube.setLayer(6, maxErrors);
sabrParametersCube.setLayer(7, endCriteria);
return sabrParametersCube;
}
void doLongCourse(int state) {
int nextState = 0;
switch (state) {
case 0: { //move 2 metres to the wall
forwards(20);
while (!wallFound(1)) {
}
nextState = 1;
break;
}
case 1: { //found a wall follow it
setSensorSide(1);//follow left wall
while (wallFound(1)) {
correctForwardMotion();
}
nextState = 2;
break;
}
case 2: { //turn away from the wall and run to the right hand side
right();
delay(150);
nextState = 3;
break;
}
case 3:{
forwards(20);
while(!wallFound(2)) {
}
nextState = 4;
break;
}
case 4 :{
setSensorSide(2);
while(wallFound(2)){
correctForwardMotion();
}
nextState = 5;
break;
}
case 5: { //wall ended find a line
forwards(20);
while (!checkForLine()) {
}
nextState = 6;
break;
}
case 6: { //line found follow it
lineFollow();
nextState = 7;
break;
}
case 7: { // docked please stop.
while(wallFound(3)) {
}
stopLineFollow();
nextState = 8;
break;
}
}
if (nextState != 8){
doLongCourse(nextState);
}
}
void doTheDemo() {
cmdDoPlay(">cc");
_DBG_("State 0");
forwards(20);
cmdDoPlay(">dd");
_DBG_("State 1");
while (sensorSide != 1) {
//Forwards until we find a left wall
checkForWall();
_DBD(sensorSide);_DBG_(" sens");
}
cmdDoPlay(">ee");
_DBG_("State 2");
//Follow wall until it's ended
int wallState = -1;
while (wallState != 3 && wallState != 0) {
wallState = checkForWall();
correctForwardMotion();
}
if (courseType == 0) {
cmdDoPlay(">aa");
_DBG_("State 5");
while(!checkForLine()) {
}
followLine();
while(!checkForNoLine()) {
}
brake();
}
else {
//Bear right to head for other wall
cmdDoPlay(">ff");
_DBG_("State 3");
right();
delay(1000);
_DBG_("State 3.1");
forwards(20);
//Wait until right wall is trackable
while (sensorSide != 2) {
checkForWall();
_DBD(sensorSide);_DBG_(" sens");
}
//Track right wall
cmdDoPlay(">gg");
_DBG_("State 4");
wallState = -1;
while (wallState != 4 && wallState != 0) {
wallState = checkForWall();
correctForwardMotion();
}
//Find the line
cmdDoPlay(">aa");
_DBG_("State 5");
forwards(20);
while(!checkForLine()) {
}
followLine();
while(!checkForNoLine()) {
}
brake();
}
}
void YoYInflationCapFloorEngine::calculate() const {
// copy black version then adapt to others
Real value = 0.0;
Size optionlets = arguments_.startDates.size();
std::vector<Real> values(optionlets, 0.0);
std::vector<Real> stdDevs(optionlets, 0.0);
std::vector<Real> forwards(optionlets, 0.0);
YoYInflationCapFloor::Type type = arguments_.type;
Handle<YoYInflationTermStructure> yoyTS
= index()->yoyInflationTermStructure();
Handle<YieldTermStructure> nominalTS =
!nominalTermStructure_.empty() ?
nominalTermStructure_ :
yoyTS->nominalTermStructure();
Date settlement = nominalTS->referenceDate();
for (Size i=0; i<optionlets; ++i) {
Date paymentDate = arguments_.payDates[i];
if (paymentDate > settlement) { // discard expired caplets
DiscountFactor d = arguments_.nominals[i] *
arguments_.gearings[i] *
nominalTS->discount(paymentDate) *
arguments_.accrualTimes[i];
// We explicitly have the index and assume that
// the fixing is natural, i.e. no convexity adjustment.
// If that was required then we would also need
// nominal vols in the pricing engine, i.e. a different engine.
// This also means that we do not need the coupon to have
// a pricing engine to return the swaplet rate and then
// the adjusted fixing in the instrument.
forwards[i] = yoyTS->yoyRate(arguments_.fixingDates[i],Period(0,Days));
Rate forward = forwards[i];
Date fixingDate = arguments_.fixingDates[i];
Time sqrtTime = 0.0;
if (fixingDate > volatility_->baseDate()){
sqrtTime = std::sqrt(
volatility_->timeFromBase(fixingDate));
}
if (type == YoYInflationCapFloor::Cap || type == YoYInflationCapFloor::Collar) {
Rate strike = arguments_.capRates[i];
if (sqrtTime>0.0) {
stdDevs[i] = std::sqrt(
volatility_->totalVariance(fixingDate, strike, Period(0,Days)));
}
// sttDev=0 for already-fixed dates so everything on forward
values[i] = optionletImpl(Option::Call, strike,
forward, stdDevs[i], d);
}
if (type == YoYInflationCapFloor::Floor || type == YoYInflationCapFloor::Collar) {
Rate strike = arguments_.floorRates[i];
if (sqrtTime>0.0) {
stdDevs[i] = std::sqrt(
volatility_->totalVariance(fixingDate, strike, Period(0,Days)));
}
Real floorlet = optionletImpl(Option::Put, strike,
forward, stdDevs[i], d);
if (type == YoYInflationCapFloor::Floor) {
values[i] = floorlet;
} else {
// a collar is long a cap and short a floor
values[i] -= floorlet;
}
}
value += values[i];
}
}
results_.value = value;
results_.additionalResults["optionletsPrice"] = values;
results_.additionalResults["optionletsAtmForward"] = forwards;
if (type != YoYInflationCapFloor::Collar)
results_.additionalResults["optionletsStdDev"] = stdDevs;
}
template<typename Robot> void DDP<Robot>::iterate(int const & itr_max, std::vector<U> & us0)
{
std::vector<MatNM> Ks;
std::vector<VecN> kus;
Ks.reserve(num);
Ks.resize(num, MatNM::Zero());
kus.reserve(num);
kus.resize(num, VecN::Zero());
double lambda=params.lambda;
double dlambda=params.dlambda;
for(int i=0;i<itr_max;i++)
{
// std::cout<<"========================================================================"<<std::endl;
// std::cout<<"Iteration # "<<i<<std::endl;
// std::cout<<"------------------------------------------------------------------------"<<std::endl;
// backward pass
bool backPassDone=false;
while(!backPassDone)
{
int result=backwards(Ks,kus,lambda);
if(result>=0)
{
dlambda=std::max(dlambda*params.lambdaFactor, params.lambdaFactor);
lambda=std::max(lambda*dlambda, params.lambdaMin);
if(lambda>params.lambdaMax)
break;
continue;
}
backPassDone=true;
}
double gnorm=getGnorm(kus,us);
if(gnorm<params.tolGrad && lambda<1e-5)
{
dlambda=std::min(dlambda/params.lambdaFactor, 1.0/params.lambdaFactor);
lambda=lambda*dlambda*(lambda>params.lambdaMin);
#ifdef PRINT
std::cout<<"SUCCESS: gradient norm = "<<gnorm" < tolGrad"<<std::endl
#endif
break;
}
// forward pass
bool fwdPassDone=false;
std::vector<State> xns;
std::vector<U> uns;
double Jn;
double actual;
double expected;
xns.reserve(num+1);
uns.reserve(num);
xns.resize(num+1,x0);
uns.resize(num,VecN::Zero());
if(backPassDone)
{
double alpha=params.alpha;
while(alpha>params.alphaMin)
{
forwards(Ks, kus, alpha, xns, uns, Jn);
actual=J0-Jn;
expected=-alpha*dJ(0)-alpha*alpha*dJ(1);
double reductionRatio=-1;
if(expected>0)
reductionRatio=actual/expected;
// else
// std::cout<<"WARNING: non-positive expected reduction: should not occur"<<std::endl;
if(reductionRatio>params.reductionRatioMin)
fwdPassDone=true;
break;
alpha*=params.dalphaFactor;
}
}
// std::cout<<"--------------------------------------------"<<std::endl;
// std::cout<<"Results"<<std::endl;
// std::cout<<"--------------------------------------------"<<std::endl;
if(fwdPassDone)
{
dlambda=std::min(dlambda/params.lambdaFactor, 1.0/params.lambdaFactor);
lambda=lambda*dlambda*(lambda>params.lambdaMin);
// std::cout<<"Improved"<<std::endl;
// std::cout<<"lambda: "<<lambda<<std::endl;
// std::cout<<"dlambda: "<<dlambda<<std::endl;
// std::cout<<"Jn: "<<Jn<<std::endl;
// std::cout<<"Jn: "<<Jn<<std::endl;
// std::cout<<Robot::State::diff(xns[num],xrefs[num]).transpose()<<std::endl;
xs=xns;
us=uns;
J0=Jn;
if(actual<params.tolFun)
{
#ifdef PRINT
std::cout<<"SUCCESS: cost change = "<<actual<<" < tolFun"<<std::endl;
#endif
break;
}
}
else
{
dlambda=std::max(dlambda*params.lambdaFactor, params.lambdaFactor);
lambda=std::max(lambda*dlambda, params.lambdaMin);
// std::cout<<"No step found"<<std::endl;
// std::cout<<"lambda: "<<lambda<<std::endl;
// std::cout<<"dlambda: "<<dlambda<<std::endl;
// std::cout<<"Jn: "<<Jn<<std::endl;
if (lambda>params.lambdaMax)
break;
}
// std::cout<<"========================================================================"<<std::endl<<std::endl;
}
QString next( int k )
{
return ( k == Qt::Key_Up ) ? backwards() : forwards();
}
