DiscountFactor discount(Size i, Size index) const {
Size modulo = tree1_->size(i);
Size index1 = index % modulo;
Size index2 = index / modulo;
Real x = tree1_->underlying(i, index1);
Real y = tree2_->underlying(i, index2);
Rate r = dynamics_->shortRate(timeGrid()[i], x, y);
return std::exp(-r*timeGrid().dt(i));
}
boost::shared_ptr<path_generator_type> pathGenerator() const {
Size numAssets = processes_->size();
TimeGrid grid = timeGrid();
Calendar calendar = this->arguments_.sp_payoff->calendar();
typename RNG::rsg_type gen =
RNG::make_sequence_generator(numAssets*(grid.size()-1),seed_);
boost::shared_ptr<path_generator_type> pgPtr
= boost::shared_ptr<path_generator_type>(
new path_generator_type(processes_,
grid,
gen));
PathInformation::instance().initialize(pgPtr->multiPath(),calendar);
//history setting here..
//index의 Calendar와 product의 Calendar(calculation Date Calendar)는 따로 가야함
//과거 index fixing은 각각 클래스에서 fixing하는데 그 방법은 pathmanager가 indexHistory를 가지고 있어서 거기에 문의해서,
//따로 클래스 내에 가져와서 fixing 시켜놈. 결과만 저장해서 나중에 함침.
//그러므로 path에 따로 무언가를 붙일 필요는 없음.
//MultiPath Grid는 product calendar로 grid숫자 계산함.
return pgPtr;
}
//! Return shared pointer to path generator who generates sample pathes in simulation
virtual boost::shared_ptr<path_generator_type> pathGenerator() const {
Size factors = this->process()->factors();
TimeGrid grid = timeGrid();
Size steps = grid.size() - 1;
typename RNG::rsg_type gen = RNG::make_sequence_generator(factors * steps, seed_);
return boost::make_shared<path_generator_type>(this->process(), grid, gen, false);
}
Size MCIRDLSLSEngine::randNum() const {
boost::shared_ptr<PayoffLeg> payoffLeg = arguments_.payoffLeg;
const std::vector<Date> & fixings = this->arguments_.fixingDates;
Size numAssets = process_->size();
Size numberOfFixings = fixings.size();
QL_REQUIRE(payoffLeg, "IRStructuredDLS payoffLeg given");
QL_REQUIRE(arguments_.refIndexTenor.size()==process_->size(),
"Process Size (" << numAssets << ") must be Same to IndexSize (" << arguments_.refIndexTenor.size() << ")");
for(Size i=0 ; i<numAssets ; ++i){
QL_REQUIRE(arguments_.refIndexTenor[i]==process_->process(i)->tenor(),
"Process Tenor must be Same to refIndex Tenor");
}
TimeGrid grid = timeGrid();
for(Size i=0 ; i<numAssets ; ++i){
process_->process(i)->setPreCalculation(grid); // 속도를 위해 수정되었음.
}
//return boost::shared_ptr<PseudoRandom::rsg_type>(&PseudoRandom::make_sequence_generator(maxSamples_* numAssets * numberOfFixings, seed_));
return maxSamples_* numAssets * numberOfFixings;
}
void TreeSwaptionEngine::calculate() const {
QL_REQUIRE(arguments_.settlementType==Settlement::Physical,
"cash-settled swaptions not priced with tree engine");
QL_REQUIRE(!model_.empty(), "no model specified");
Date referenceDate;
DayCounter dayCounter;
boost::shared_ptr<TermStructureConsistentModel> tsmodel =
boost::dynamic_pointer_cast<TermStructureConsistentModel>(*model_);
if (tsmodel) {
referenceDate = tsmodel->termStructure()->referenceDate();
dayCounter = tsmodel->termStructure()->dayCounter();
} else {
referenceDate = termStructure_->referenceDate();
dayCounter = termStructure_->dayCounter();
}
boost::shared_ptr<DiscretizedSwaption> swaption(new DiscretizedSwaption(arguments_, referenceDate, dayCounter));
if (additionalResultCalculator_) {
additionalResultCalculator_->setupDiscretizedAsset(swaption);
}
boost::shared_ptr<Lattice> lattice;
if (lattice_) {
lattice = lattice_;
} else {
std::vector<Time> times = swaption->mandatoryTimes();
TimeGrid timeGrid(times.begin(), times.end(), timeSteps_);
lattice = model_->tree(timeGrid, additionalResultCalculator_);
}
std::vector<Time> stoppingTimes(arguments_.exercise->dates().size());
for (Size i=0; i<stoppingTimes.size(); ++i)
stoppingTimes[i] =
dayCounter.yearFraction(referenceDate,
arguments_.exercise->date(i));
swaption->initialize(lattice, stoppingTimes.back());
Time nextExercise =
*std::find_if(stoppingTimes.begin(),
stoppingTimes.end(),
std::bind2nd(std::greater_equal<Time>(), 0.0));
swaption->rollback(nextExercise);
results_.value = swaption->presentValue();
if (additionalResultCalculator_) {
additionalResultCalculator_->calculateAdditionalResults();
results_.additionalResults = additionalResultCalculator_->additionalResults();
}
}
ext::shared_ptr<path_generator_type> pathGenerator() const {
Size numAssets = processes_->size();
TimeGrid grid = timeGrid();
typename RNG::rsg_type gen =
RNG::make_sequence_generator(numAssets*(grid.size()-1),seed_);
return ext::shared_ptr<path_generator_type>(
new path_generator_type(processes_,
grid, gen, brownianBridge_));
}
boost::shared_ptr<path_generator_type> pathGenerator() const {
Size dimensions = process_->factors();
TimeGrid grid = timeGrid();
typename RNG::rsg_type gen =
RNG::make_sequence_generator(dimensions*(grid.size()-1),seed_);
return boost::shared_ptr<path_generator_type>(
new path_generator_type(process_, grid, gen,
brownianBridge_));
}
boost::shared_ptr<path_generator_type> pathGenerator() const {
boost::shared_ptr<BasketPayoff> payoff =
boost::dynamic_pointer_cast<BasketPayoff>(
arguments_.payoff);
QL_REQUIRE(payoff, "non-basket payoff given");
Size numAssets = processes_->size();
TimeGrid grid = timeGrid();
typename RNG::rsg_type gen =
RNG::make_sequence_generator(numAssets*(grid.size()-1),seed_);
return boost::shared_ptr<path_generator_type>(
new path_generator_type(processes_,
grid, gen, brownianBridge_));
}
inline
boost::shared_ptr<typename MCPathBasketEngine<RNG,S>::path_generator_type>
MCPathBasketEngine<RNG,S>::pathGenerator() const {
boost::shared_ptr<PathPayoff> payoff = arguments_.payoff;
QL_REQUIRE(payoff, "non-basket payoff given");
Size numAssets = process_->size();
TimeGrid grid = timeGrid();
typename RNG::rsg_type gen =
RNG::make_sequence_generator(numAssets * (grid.size() - 1), seed_);
return boost::shared_ptr<path_generator_type>(
new path_generator_type(process_,
grid, gen, brownianBridge_));
}
inline
boost::shared_ptr<typename MCPathBasketEngine<RNG,S>::path_pricer_type>
MCPathBasketEngine<RNG,S>::pathPricer() const {
boost::shared_ptr<PathPayoff> payoff = arguments_.payoff;
QL_REQUIRE(payoff, "non-basket payoff given");
boost::shared_ptr<GeneralizedBlackScholesProcess> process =
boost::dynamic_pointer_cast<GeneralizedBlackScholesProcess>(
process_->process(0));
QL_REQUIRE(process, "Black-Scholes process required");
const TimeGrid theTimeGrid = timeGrid();
const std::vector<Time> & times = theTimeGrid.mandatoryTimes();
const Size numberOfTimes = times.size();
const std::vector<Date> & fixings = this->arguments_.fixingDates;
QL_REQUIRE(fixings.size() == numberOfTimes, "Invalid dates/times");
std::vector<Size> timePositions(numberOfTimes);
Array discountFactors(numberOfTimes);
std::vector<Handle<YieldTermStructure> > forwardTermStructures(numberOfTimes);
const Handle<YieldTermStructure> & riskFreeRate = process->riskFreeRate();
for (Size i = 0; i < numberOfTimes; ++i) {
timePositions[i] = theTimeGrid.index(times[i]);
discountFactors[i] = riskFreeRate->discount(times[i]);
forwardTermStructures[i] = Handle<YieldTermStructure>(
new ImpliedTermStructure(riskFreeRate, fixings[i]));
}
return boost::shared_ptr<
typename MCPathBasketEngine<RNG,S>::path_pricer_type>(
new EuropeanPathMultiPathPricer(payoff, timePositions,
forwardTermStructures,
discountFactors));
}
DiscountFactor discount(Size i, Size index) const {
Real x = tree_->underlying(i, index);
Rate r = dynamics_->shortRate(timeGrid()[i], x);
return std::exp(-r*timeGrid().dt(i));
}
boost::shared_ptr<ELSNotionalProtectedPricer> MCELSNotionalProtectedEngine::pathPricer() const{
/*boost::shared_ptr<GeneralizedBlackScholesProcess> process =
boost::dynamic_pointer_cast<GeneralizedBlackScholesProcess>(
process_->process(0));
QL_REQUIRE(process, "Black-Scholes process required");*/
TimeGrid theTimeGrid = timeGrid();
const std::vector<Time> & times = theTimeGrid.mandatoryTimes();
Size numberOfTimes = times.size();
Size numAssets = process_->size();
Size pastFixingNum=this->arguments_.pastFixingDateNum;
const std::vector<Date> & fixings = this->arguments_.fixingDates;
const std::vector<boost::shared_ptr<StockIndex>>& refIndex=arguments_.refIndex;
DayCounter dayCounter = this->arguments_.dayCounter;
std::vector<Real> initialValues = this->arguments_.initialValues;
std::vector<Real> dividends = this->arguments_.dividends; // 후에 TS로 수정
Real redemCoupon= this->arguments_.redemCoupon;
std::vector<Date> remainfixings;
//오늘일자를 TS에서 가져옴.
Date today = Settings::instance().evaluationDate();
Size numberOfFixings = fixings.size();
for(Size i=0;i<numberOfFixings;++i){
if(fixings[i]>today){
remainfixings.push_back(fixings[i]);
}
}
Size numberOfRemainFixings = remainfixings.size();
additionalStats_.resize(numberOfTimes+pastFixingNum);
underlyingValue_.resize(numAssets);
QL_REQUIRE(numberOfRemainFixings == numberOfTimes, "Invalid dates/times");
std::vector<Size> timePositions(numberOfTimes);
Array discountFactors(numberOfTimes);
for (Size i = 0; i < numberOfTimes; ++i) {
//timePositions[i] = theTimeGrid.index(times[i]);
discountFactors[i] = discountTS_->discount(times[i]);
}
Matrix earlyExValues_Chg(numAssets,numberOfTimes,Null<Real>());
Array x0(numAssets);
Array drift(numAssets);
Array sigma(numAssets);
const std::vector<Real>& earlyExValues = this->arguments_.earlyExTriggers;
for(Size i=0;i<numAssets;++i){
if(process_->process(i)->x0()<0.00001){
x0[i]=std::log((refIndex[i]->fixing(Settings::instance().evaluationDate()))/initialValues[i]);
underlyingValue_[i]=refIndex[i]->fixing(Settings::instance().evaluationDate());
}else{
x0[i]=std::log((process_->process(i)->x0())/initialValues[i]);
underlyingValue_[i]=process_->process(i)->x0();
}
sigma[i]=process_->process(i)->diffusion(times[0],1.0) ; // 변동성 조정 필요
if(process_->process(i)->drift(times[0],1.0)<0.00001){
drift[i]= discountTS_->zeroRate(times.back(),Compounded,Annual,false) - dividends[i]; //discountTS_->zeroRate(times.back(),Compounded,Annual,false);
}else{
drift[i]= process_->process(i)->drift(times[0],1.0) - dividends[i]; //discountTS_->zeroRate(times.back(),Compounded,Annual,false);
}
//drift 이거 dividend 추가해야함.
for(Size j=0;j<numberOfTimes;++j){
earlyExValues_Chg[i][j]=(std::log(earlyExValues[j])-(drift[i]-0.5*sigma[i]*sigma[i])*times[j]);
}
}
return boost::shared_ptr<ELSNotionalProtectedPricer>(
new ELSNotionalProtectedPricer(times,process_,x0,discountFactors,earlyExValues_Chg,
redemCoupon,pastFixingNum,additionalStats_));
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4,arm1x,arm1v,arm2x,arm2v;
Control vu1,vu2,vu3,vu4;
//Parameter T;
DifferentialEquation f(0.0,15);
const double c = 0.00001;
const double Cf = 0.00065;
const double d = 0.250;
const double Jx = 0.018;
const double Jy = 0.018;
const double Jz = 0.026;
const double m = 0.9;
const double g = 9.81;
const double Cx = 0.1;
const double Iarm1 = 0 ;
const double xf = 20.;
const double yf = 0.;
const double zf = 0.;
double coeffU = 0.001;
double coeffX = .01;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
OCP ocp( 0.0, 15, 60 );
//ocp.minimizeMayerTerm( 1000*coeffX*((x-xf)*(x-xf) + (y-yf)*(y-yf) + (z-zf)*(z-zf)) );
ocp.minimizeLagrangeTerm(coeffX*(sqrt(1+(x-xf)*(x-xf)) + sqrt(1+(y-yf)*(y-yf)) + sqrt(1+(z-zf)*(z-zf))) + coeffU*( sqrt(1+(u1-58.27)*(u1-58.27))+sqrt(1+(u2-58.27)*(u2-58.27))+sqrt(1+(u3-58.27)*(u3-58.27))+sqrt(1+(u4-58.27)*(u4-58.27))));
f << dot(x) == vx;
f << dot(y) == vy;
f << dot(z) == vz;
f << dot(vx) == -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m;
f << dot(vy) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(phi)*cos(theta)/m;
f << dot(vz) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(phi)*cos(theta)/m - g;
f << dot(phi) == p + sin(theta)*r;
f << dot(theta) == cos(phi)*q-sin(phi)*cos(theta)*r;
f << dot(psi) == sin(phi)*q+cos(phi)*cos(theta)*r;
f << dot(p) == (d*Cf*(u1*u1-u2*u2)+(Jy-Jz)*q*r)/Jx;
f << dot(q) == (d*Cf*(u4*u4-u3*u3)+(Jz-Jx)*p*r)/Jy;
f << dot(r) == (c*(u1*u1+u2*u2-u3*u3-u4*u4)+(Jx-Jy)*p*q)/Jz;
f << dot(u1) == vu1;
f << dot(u2) == vu2;
f << dot(u3) == vu3;
f << dot(u4) == vu4;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
//Dynamic
ocp.subjectTo( f );
//Start conditions
ocp.subjectTo( AT_START, x == 0 );
ocp.subjectTo( AT_START, y == 0 );
ocp.subjectTo( AT_START, z == 0 );
ocp.subjectTo( AT_START, vx == 0 );
ocp.subjectTo( AT_START, vy == 0 );
ocp.subjectTo( AT_START, vz == 0 );
ocp.subjectTo( AT_START, phi == 0.0 );
ocp.subjectTo( AT_START, theta == -0. );
ocp.subjectTo( AT_START, psi == -0.);
ocp.subjectTo( AT_START, p == 0.0 );
ocp.subjectTo( AT_START, q == 0.0 );
ocp.subjectTo( AT_START, r == 0.0 );
ocp.subjectTo( AT_START, (d*Cf*(u1*u1-u2*u2)+(Jy-Jz)*q*r)/Jx == 0.0 );
ocp.subjectTo( AT_START, (d*Cf*(u4*u4-u3*u3)+(Jz-Jx)*p*r)/Jy == 0.0 );
ocp.subjectTo( AT_START, (c*(u1*u1+u2*u2-u3*u3-u4*u4)+(Jx-Jy)*p*q)/Jz == 0.0 );
ocp.subjectTo( AT_START, -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m == 0 );
ocp.subjectTo( AT_START, Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(phi)*cos(theta)/m == 0 );
ocp.subjectTo( AT_START, Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(phi)*cos(theta)/m - g == 0 );
//Command constraints
//on each motor
//ocp.subjectTo( (u1*u1+u2*u2-u3*u3-u4*u4) == 0);
ocp.subjectTo( 16 <= u1 <= 95 );
ocp.subjectTo( 16 <= u2 <= 95 );
ocp.subjectTo( 16 <= u3 <= 95 );
ocp.subjectTo( 16 <= u4 <= 95 );
ocp.subjectTo( -100 <= vu1 <= 100 );
ocp.subjectTo( -100 <= vu2 <= 100 );
ocp.subjectTo( -100 <= vu3 <= 100 );
ocp.subjectTo( -100 <= vu4 <= 100 );
//ocp.subjectTo( -15 <= vx <= 15);
//ocp.subjectTo( -0.1 <= z <= 0.1);
//ocp.subjectTo( -0.1 <= y <= 0.1);
//on total power
//ocp.subjectTo( u1*u1+u2*u2+u3*u3+u4*u4 <= 800 );
// DEFINE A PLOT WINDOW:
// ---------------------
GnuplotWindow window2(PLOT_AT_EACH_ITERATION);
window2.addSubplot( z,"DifferentialState z" );
window2.addSubplot( x,"DifferentialState x" );
//window2.addSubplot( y,"DifferentialState y" );
window2.addSubplot( vx,"DifferentialState vx" );
window2.addSubplot( vz,"DifferentialState vz" );
window2.addSubplot( Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(phi)*cos(theta)/m - g,"acc z" );
//window2.addSubplot( vx,"DifferentialState vx" );
window2.addSubplot( -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m,"acc x" );
//window2.addSubplot( vy,"DifferentialState vy" );
//window2.addSubplot( Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(phi)*cos(theta)/m,"acc y" );
window2.addSubplot( u1,"u1" );
window2.addSubplot( u2,"u2" );
window2.addSubplot( u3,"u3" );
window2.addSubplot( u4,"u4" );
//window2.addSubplot( u4+u1+u2+u3,"f tot" );
//window2.addSubplot( vu1,"vu1" );
//window2.addSubplot( vu2,"vu2" );
//window2.addSubplot( vu3,"vu3" );
//window2.addSubplot( vu4,"vu4" );
//window2.addSubplot( phi,"phi" );
//window2.addSubplot( theta,"theta" );
//window2.addSubplot( z,"z" );
//window2.addSubplot( y,"y" );
//window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" );
// window.addSubplot( 0.5 * m * v*v,"Kinetic Energy" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
//algorithm.set(DISCRETIZATION_TYPE, SINGLE_SHOOTING);
//VariableGrid T_init(1
// algorithm.initializeParameters(4.0);
// algorithm.set( INTEGRATOR_TYPE, INT_BDF );
// algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 );
// algorithm.set( KKT_TOLERANCE, 1e-3 );
//algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY );
Grid timeGrid(0.0,1.0,41);
VariablesGrid u_init(16, timeGrid);
for (int i = 0 ; i<41 ; i++ ) {
if(i>24 && i<33) {
//srand (time(NULL));
u_init(i,12) = 50;
u_init(i,15) = 35;
u_init(i,14) = 50;
u_init(i,13) = 50;
}/*
if(i>15 && i<25) {u_init(i,12) = 15.0;
u_init(i,15) = 90.0;
u_init(i,14) = 15.0;
u_init(i,13) = 15.0;}
if(i>24 && i<34) {u_init(i,12) = 90.0;
u_init(i,15) = 90.0;
u_init(i,14) = 90.0;
u_init(i,13) = 90.0;}
if(i>34 && i<41) {u_init(i,12) = 90.0;
u_init(i,15) = 15.0;
u_init(i,14) = 15.0;
u_init(i,13) = 15.0;}*/
//u_init(i,8) = i*1.6/40;
}
//u_init.print();
//algorithm.initializeDifferentialStates(u_init);
//algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
algorithm.set( MAX_NUM_ITERATIONS, 2000 );
algorithm.set( KKT_TOLERANCE, 1e-18 );
// algorithm.set( MAX_NUM_INTEGRATOR_STEPS, 4 );
//algorithm << window3;
algorithm << window2;
algorithm.solve();
VariablesGrid states, controls;
algorithm.getDifferentialStates("/tmp/states.txt");
algorithm.getDifferentialStates(states);
algorithm.getControls("/tmp/controls.txt");
algorithm.getControls(controls);
/*VariablesGrid output(5,timeGrid);
for ( int i =0 ; i<41 ; i++ ) {
output(i,0) = states(i,0);
output(i,1) = states(i,3);
if(i>0) output(i,2) = (states(i,3)-states(i-1,3))/0.025;
if(i>1) output(i,3) = (output(i,2)-output(i-1,2));///0.025;
if(i>2) output(i,4) = (output(i,3)-output(i-1,3));///0.025;
}*/
//output.print();
//states.print();
// BlockMatrix sens;
// algorithm.getSensitivitiesX( sens );
// sens.print();
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4;
// x, y, z : position
// vx, vy, vz : linear velocity
// phi, theta, psi : orientation (Yaw-Pitch-Roll = Euler(3,2,1))
// p, q, r : angular velocity
// u1, u2, u3, u4 : velocity of the propellers
Control vu1,vu2,vu3,vu4;
// vu1, vu2, vu3, vu4 : derivative of u1, u2, u3, u4
DifferentialEquation f;
// Quad constants
const double c = 0.00001;
const double Cf = 0.00065;
const double d = 0.250;
const double Jx = 0.018;
const double Jy = 0.018;
const double Jz = 0.026;
const double Im = 0.0001;
const double m = 0.9;
const double g = 9.81;
const double Cx = 0.1;
// Minimization Weights
double coeffX = .00001;
double coeffU = coeffX*0.0001;//0.000000000000001;
double coeffX2 = coeffX * 100.;
double coeffX3 = coeffX * 0.00001;
double coeffO = -coeffX * 0.1;
// final position (used in the cost function)
double xf = 0., yf = 0., zf = 20.;
//
double T = 8.; //length (in second) of the trajectory predicted in the MPC
int nb_nodes = 20.; //number of nodes used in the Optimal Control Problem
// 20 nodes means that the algorithm will discretized the trajectory equally into 20 pieces
// If you increase the number of node, the solution will be more precise but calculation will take longer (~nb_nodes^2)
// In ACADO, the commands are piecewise constant functions, constant between each node.
double tmpc = 0.2; //time (in second) between two activation of the MPC algorithm
// DEFINE A OPTIMAL CONTROL PROBLEM
// -------------------------------
OCP ocp( 0.0, T, nb_nodes );
// DEFINE THE COST FUNCTION
// -------------------------------
Function h, hf;
h << x;
h << y;
h << z;
h << vu1;
h << vu2;
h << vu3;
h << vu4;
h << p;
h << q;
h << r;
hf << x;
hf << y;
hf << z;
DMatrix Q(10,10), Qf(3,3);
Q(0,0) = coeffX;
Q(1,1) = coeffX;
Q(2,2) = coeffX;
Q(3,3) = coeffU;
Q(4,4) = coeffU;
Q(5,5) = coeffU;
Q(6,6) = coeffU;
Q(7,7) = coeffX2;
Q(8,8) = coeffX2;
Q(9,9) = coeffX2;
Qf(0,0) = 1.;
Qf(1,1) = 1.;
Qf(2,2) = 1.;
DVector reff(3), ref(10);
ref(0) = xf;
ref(1) = yf;
ref(2) = zf;
ref(3) = 58.;
ref(4) = 58.;
ref(5) = 58.;
ref(6) = 58.;
ref(7) = 0.;
ref(8) = 0.;
ref(9) = 0.;
reff(0) = xf;
reff(1) = yf;
reff(2) = zf;
// The cost function is define as : integrale from 0 to T { transpose(h(x,u,t)-ref)*Q*(h(x,u,t)-ref) } + transpose(hf(x,u,T))*Qf*hf(x,u,T)
// == integrale cost (also called running cost or Lagrange Term) + final cost (Mayer Term)
ocp.minimizeLSQ ( Q, h, ref);
// ocp.minimizeLSQEndTerm(Qf, hf, reff);
// When doing MPC, you need terms in the cost function to stabilised the system => p, q, r and vu1, vu2, vu3, vu4. You can check that if you reduce their weights "coeffX2" or "coeffU" too low, the optimization will crashed.
// DEFINE THE DYNAMIC EQUATION OF THE SYSTEM:
// ----------------------------------
f << dot(x) == vx;
f << dot(y) == vy;
f << dot(z) == vz;
f << dot(vx) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m;
f << dot(vy) == -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(psi)*cos(theta)/m;
f << dot(vz) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(psi)*cos(theta)/m - g;
f << dot(phi) == -cos(phi)*tan(theta)*p+sin(phi)*tan(theta)*q+r;
f << dot(theta) == sin(phi)*p+cos(phi)*q;
f << dot(psi) == cos(phi)/cos(theta)*p-sin(phi)/cos(theta)*q;
f << dot(p) == (d*Cf*(u1*u1-u2*u2)+(Jy-Jz)*q*r)/Jx;
f << dot(q) == (d*Cf*(u4*u4-u3*u3)+(Jz-Jx)*p*r)/Jy;
f << dot(r) == (c*(u1*u1+u2*u2-u3*u3-u4*u4)+(Jx-Jy)*p*q)/Jz;
f << dot(u1) == vu1;
f << dot(u2) == vu2;
f << dot(u3) == vu3;
f << dot(u4) == vu4;
// ---------------------------- DEFINE CONTRAINTES --------------------------------- //
//Dynamic
ocp.subjectTo( f );
//State constraints
//Constraints on the velocity of each propeller
ocp.subjectTo( 16 <= u1 <= 95 );
ocp.subjectTo( 16 <= u2 <= 95 );
ocp.subjectTo( 16 <= u3 <= 95 );
ocp.subjectTo( 16 <= u4 <= 95 );
//Command constraints
//Constraints on the acceleration of each propeller
ocp.subjectTo( -100 <= vu1 <= 100 );
ocp.subjectTo( -100 <= vu2 <= 100 );
ocp.subjectTo( -100 <= vu3 <= 100 );
ocp.subjectTo( -100 <= vu4 <= 100 );
#if AVOID_SINGULARITIES == 1
//Constraint to avoid singularity
// In this example I used Yaw-Pitch-Roll convention to describe orientation of the quadrotor
// when using Euler Angles representation, you always have a singularity, and we need to
// avoid it otherwise the algorithm will crashed.
ocp.subjectTo( -1. <= theta <= 1.);
#endif
//Example of Eliptic obstacle constraints (here, cylinders with eliptic basis)
ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-5)*(z-5)) );
ocp.subjectTo( 16 <= ((x-3)*(x-3)+2*(z-9)*(z-9)) );
ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-15)*(z-15)) );
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
// The next line define the equation used to simulate the system :
// here "f" is used (=the same as the one used for the ocp) so the trajectory simulated
// from the commands will be exactly the same as the one calculated by the MPC.
// If for instance you want to test robusness, you can define an other dynamic equation "f2"
// with changed parameters and put it here.
DynamicSystem dynamicSystem( f,identity );
Process process( dynamicSystem,INT_RK45 );
// SETTING UP THE MPC CONTROLLER:
// ------------------------------
RealTimeAlgorithm alg( ocp, tmpc );
//Usually, you do only one step of the optimisation algorithm (~Gauss-Newton here)
//at each activation of the MPC, that way the delay between getting the state and
//sending a command is as quick as possible.
alg.set( MAX_NUM_ITERATIONS, 1 );
StaticReferenceTrajectory zeroReference;
Controller controller( alg,zeroReference );
// SET AN INITIAL GUESS FOR THE FIRST MPC LOOP (NEXT LOOPS WILL USE AS INITIAL GUESS THE SOLUTION FOUND AT THE PREVIOUS MPC LOOP)
Grid timeGrid(0.0,T,nb_nodes+1);
VariablesGrid x_init(16, timeGrid);
// init with static
for (int i = 0 ; i<nb_nodes+1 ; i++ ) {
x_init(i,0) = 0.;
x_init(i,1) = 0.;
x_init(i,2) = 0.;
x_init(i,3) = 0.;
x_init(i,4) = 0.;
x_init(i,5) = 0.;
x_init(i,6) = 0.;
x_init(i,7) = 0.;
x_init(i,8) = 0.;
x_init(i,9) = 0.;
x_init(i,10) = 0.;
x_init(i,11) = 0.;
x_init(i,12) = 58.; //58. is the propeller rotation speed so the total thrust balance the weight of the quad
x_init(i,13) = 58.;
x_init(i,14) = 58.;
x_init(i,15) = 58.;
}
alg.initializeDifferentialStates(x_init);
// SET OPTION AND PLOTS WINDOW
// ---------------------------
// Linesearch is an algorithm which will try several points along the descent direction to choose a better step length.
// It looks like activating this option produice more stable trajectories.198
alg.set( GLOBALIZATION_STRATEGY, GS_LINESEARCH );
alg.set(INTEGRATOR_TYPE, INT_RK45);
// You can uncomment those lines to see how the predicted trajectory involve along time
// (but be carefull because you will have 1 ploting window per MPC loop)
// GnuplotWindow window1(PLOT_AT_EACH_ITERATION);
// window1.addSubplot( z,"DifferentialState z" );
// window1.addSubplot( x,"DifferentialState x" );
// window1.addSubplot( theta,"DifferentialState theta" );
// window1.addSubplot( 16./((x+3)*(x+3)+4*(z-5)*(z-5)),"Dist obs1" );
// window1.addSubplot( 16./((x-3)*(x-3)+4*(z-9)*(z-9)),"Dist obs2" );
// window1.addSubplot( 16./((x+2)*(x+2)+4*(z-15)*(z-15)),"Dist obs3" );
// alg<<window1;
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
// The first argument is the starting time, the second the end time.
SimulationEnvironment sim( 0.0,10.,process,controller );
//Setting the state of the sytem at the beginning of the simulation.
DVector x0(16);
x0.setZero();
x0(0) = 0.;
x0(12) = 58.;
x0(13) = 58.;
x0(14) = 58.;
x0(15) = 58.;
t = clock();
if (sim.init( x0 ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
if (sim.run( ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
t = clock() - t;
std::cout << "total time : " << (((float)t)/CLOCKS_PER_SEC)<<std::endl;
// ...SAVE THE RESULTS IN FILES
// ----------------------------------------------------------
std::ofstream file;
file.open("/tmp/log_state.txt",std::ios::out);
std::ofstream file2;
file2.open("/tmp/log_control.txt",std::ios::out);
VariablesGrid sampledProcessOutput;
sim.getSampledProcessOutput( sampledProcessOutput );
sampledProcessOutput.print(file);
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
feedbackControl.print(file2);
// ...AND PLOT THE RESULTS
// ----------------------------------------------------------
GnuplotWindow window;
window.addSubplot( sampledProcessOutput(0), "x " );
window.addSubplot( sampledProcessOutput(1), "y " );
window.addSubplot( sampledProcessOutput(2), "z " );
window.addSubplot( sampledProcessOutput(6),"phi" );
window.addSubplot( sampledProcessOutput(7),"theta" );
window.addSubplot( sampledProcessOutput(8),"psi" );
window.plot( );
graphics::corbaServer::ClientCpp client = graphics::corbaServer::ClientCpp();
client.createWindow("window");
return 0;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// Define a Right-Hand-Side:
// -------------------------
DifferentialState x;
DifferentialEquation f;
TIME t;
f << dot(x) == -x + sin(0.01*t);
// Define an initial value:
// ------------------------
Vector xStart( 1 );
xStart(0) = 1.0;
double tStart = 0.0;
double tEnd = 1000.0;
Grid timeHorizon( tStart,tEnd,2 );
Grid timeGrid( tStart,tEnd,20 );
// Define an integration algorithm:
// --------------------------------
IntegrationAlgorithm intAlg;
intAlg.addStage( f, timeHorizon );
intAlg.set( INTEGRATOR_TYPE, INT_BDF );
intAlg.set( INTEGRATOR_PRINTLEVEL, MEDIUM );
intAlg.set( INTEGRATOR_TOLERANCE, 1.0e-3 );
intAlg.set( PRINT_INTEGRATOR_PROFILE, YES );
intAlg.set( PLOT_RESOLUTION, HIGH );
GnuplotWindow window;
window.addSubplot( x,"x" );
intAlg << window;
// START THE INTEGRATION
// ----------------------
intAlg.integrate( timeHorizon, xStart );
// GET THE RESULTS
// ---------------
VariablesGrid differentialStates;
intAlg.getX( differentialStates );
differentialStates.print( "x" );
Vector xEnd;
intAlg.getX( xEnd );
xEnd.print( "xEnd" );
return 0;
}
