PointCoordinateMatrices	ConverterPlaneFromTo3d::projectTo3d(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> image)
	{
		PointCoordinateMatrices reshapedCoordinates;

		//std::cout << "image: " << std::endl << image << std::endl << std::endl;

		//As defined for the example data sets to get the distance in meters Z = depth / 5000 TODO change for kinect data (maybe is the same...)
		Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> z = image.array() / 1;// 5000;
		Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> x = z.cwiseProduct(xAdjustment_);
		Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> y = z.cwiseProduct(yAdjustment_);

		//std::cout << "x: " << std::endl << x << std::endl << std::endl;
		//std::cout << "y: " << std::endl << y << std::endl << std::endl;
		//std::cout << "z: " << std::endl << z << std::endl << std::endl;

		reshapedCoordinates.x = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(x.data(), 1, image.size());
		reshapedCoordinates.y = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(y.data(), 1, image.size());
		reshapedCoordinates.z = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(z.data(), 1, image.size());

		return reshapedCoordinates;
	}
Пример #2
0
template<int N> void calculateGaussPdf(Eigen::Matrix<double,Eigen::Dynamic,N> X, Eigen::Matrix<double,1,N> mu, Eigen::Matrix<double,N,N> C, double* result){
    Eigen::Matrix<double,N,N> L = C.llt().matrixL().transpose(); // cholesky decomposition
    Eigen::Matrix<double,N,N> Linv = L.inverse();
    
    double det = L.diagonal().prod(); //determinant of L is equal to square rooot of determinant of C
	double lognormconst = -log(2 * M_PI)*X.cols()/2 - log(fabs(det));

    Eigen::Matrix<double,1,N> x = mu;
    Eigen::Matrix<double,1,N> tmp = x;
    for (int i=0; i<X.rows(); i++){
        x.noalias() = X.row(i) - mu;
        tmp.noalias() = x*Linv;
        double exponent = -0.5 * (tmp.cwiseProduct(tmp)).sum();
        result[i] = lognormconst+exponent;
    }
    
    /*
	Eigen::Matrix<double,Eigen::Dynamic,N> X0 = (X.rowwise() - mu)*Linv;
	Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,1> > resultMap(result, X.rows());
	resultMap = (X0.rowwise().squaredNorm()).array() * (-0.5) + lognormconst;
    */
    
    fmath::expd_v(result, X.rows());
}
Пример #3
0
int main()
{

	//********************************************************
	// Initialization
	//********************************************************

	double M=0.6;
	double Ix=8*1e-3; // moment of inertia in X- Y- and Z-axis
	double Iy=7.5*1e-3;
	double Iz=13.5*1e-3;
	double d=0.2; // displacement of rotors
	double kt=0.6;
	double km=0.15;

	char file[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_1_small.mat";

	mxArray *mxR;
	mxArray *mxw;
	mxArray *mxxq;
	mxArray *mxvq;
	mxArray *mxU;

	mxR=matGetVariable(matOpen(file,"r"),"state_R");
	mxw=matGetVariable(matOpen(file,"r"),"state_w");
	mxxq=matGetVariable(matOpen(file,"r"),"state_xq");
	mxvq=matGetVariable(matOpen(file,"r"),"state_vq");
#ifdef ROT_INPUT
	mxU=matGetVariable(matOpen(file,"r"),"U");
#else
	mxU=matGetVariable(matOpen(file,"r"),"F");
#endif
	size_t N=mxGetN(mxU);
	size_t sN=5;

	void *pR=mxGetPr(mxR);
	void *pw=mxGetPr(mxw);
	void *pxq=mxGetPr(mxxq);
	void *pvq=mxGetPr(mxvq);
	void *pU=mxGetPr(mxU);

	std::list<typename quadrotor::State> list_par1;
	std::list<typename quadrotor::State> list_par2;

	for(int i=0;i<N;i+=sN)
	{
		Eigen::Matrix<double,3,3> R;
		Eigen::Matrix<double,3,1> w,xq,vq;

		memcpy(R.data(),pR,sizeof(double)*R.rows()*R.cols());
		memcpy(w.data(),pw,sizeof(double)*w.rows()*w.cols());
		memcpy(xq.data(),pxq,sizeof(double)*xq.rows()*xq.cols());
		memcpy(vq.data(),pvq,sizeof(double)*vq.rows()*vq.cols());

		list_par1.emplace_back(R,xq,w,R.transpose()*vq);

		if((i+1)%2)
			list_par2.emplace_back(R,xq,w,R.transpose()*vq);

		pR+=sizeof(double)*R.rows()*R.cols()*sN;
		pw+=sizeof(double)*w.rows()*w.cols()*sN;
		pxq+=sizeof(double)*xq.rows()*xq.cols()*sN;
		pvq+=sizeof(double)*vq.rows()*vq.cols()*sN;
	}

	sN=10;

	std::list<typename quadrotor::U> list_u1;
	std::list<typename quadrotor::U> list_u2;

#ifdef ROT_INPUT
	for(int i=0;i<N;i+=sN)
	{
		Eigen::Matrix<double,4,1> u;
		memcpy(u.data(),pU,sizeof(double)*u.rows()*u.cols());
		list_u1.push_back(u.cwiseProduct(u));
		list_u2.push_back(u);
		pU+=sizeof(double)*u.rows()*u.cols()*sN;
	}
#else
	for(int i=0;i<N;i+=sN)
	{
		Eigen::Matrix<double,4,1> u;
		memcpy(u.data(),pU,sizeof(double)*u.rows()*u.cols());
		list_u.push_back(u);
		pU+=sizeof(double)*u.rows()*u.cols()*sN;
	}
#endif
	mxDestroyArray(mxR);
	mxDestroyArray(mxw);
	mxDestroyArray(mxxq);
	mxDestroyArray(mxvq);
	mxDestroyArray(mxU);

	std::srand((unsigned int) time(0));

	// initialize SAC
	Mat12 PM=2500*Mat12::Identity();
	PM.block(0,0,3,3)*=10;
	PM.block(3,3,3,3)*=40;
	PM.block(6,6,6,6)=PM.block(0,0,6,6)/8;

	Mat12 NM=PM/20;

	quadrotor::Cost cost=std::bind(quad_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, NM);
	quadrotor::DCost dcost=std::bind(quad_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, NM);

	quadrotor::Cost costN=std::bind(quad_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, PM);
	quadrotor::DCost dcostN=std::bind(quad_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, PM);

	SAC<quadrotor>::Functions func(cost, dcost, costN, dcostN);

	SAC<quadrotor>::Params params;

//--------------------------------------------------------------------
// Parameter one
//--------------------------------------------------------------------
	params.alpha=-10;
	params.beta=0.6;

	params.Tc=1.28;
	params.Tp=1.5;
	params.Ts=0.02;
	params.Tcal=0.01;
	params.kmax=8;

	params.R=1*Eigen::Matrix<double,4,4>::Identity();

	
//--------------------------------------------------------------------
// Parameter two 
//--------------------------------------------------------------------
	double dt=0.01;

	quadrotor::System sys(Ix,Iy,Iz,M,d,km,kt);

	SAC<quadrotor> sac(sys,dt);

	double t=0;
	double ts=params.Ts;
	double T=108+params.Tp;

	quadrotor::U umax,umin;
	umax<<6,6,6,6;
	umin<<0.0,0.0,0.0,0.0;

	// initialize LQR
	Mat12 Q=50*Mat12::Identity();
	Q.block(0,0,3,3)*=10;
	Q.block(3,3,3,3)*=5;
	Q.block(6,6,6,6)=50*Mat6::Identity();
	Mat12 Pf=5*Q;
	Mat4 R=2000*Mat4::Identity();

	std::list<Eigen::Matrix<double,4,12> > list_K=quadrotor::lqr(sys, dt, list_par2, list_u2, Q, R, Pf);


	// set up initial configuration
	
	quadrotor::State state0;
#ifdef STATE0
	char file0[]="/media/fantaosha/Documents/JHU/Summer 2015/results/plot_quadrotor/quadrotor_track_12s_important.mat";

	mxArray *mxR0;
	mxArray *mxw0;
	mxArray *mxxq0;
	mxArray *mxvq0;

	mxR0=matGetVariable(matOpen(file0,"r"),"R");
	mxw0=matGetVariable(matOpen(file0,"r"),"omega");
	mxxq0=matGetVariable(matOpen(file0,"r"),"xq");
	mxvq0=matGetVariable(matOpen(file0,"r"),"vq");

	Mat3 R0;
	Vec3 xq0,vq0,w0;

	memcpy(R0.data(),mxGetPr(mxR0),sizeof(double)*9);
	memcpy(w0.data(),mxGetPr(mxw0),sizeof(double)*3);
	memcpy(xq0.data(),mxGetPr(mxxq0),sizeof(double)*3);
	memcpy(vq0.data(),mxGetPr(mxvq0),sizeof(double)*3);

	state0.g.block(0,0,3,3)=R0;
	state0.g.block(0,3,3,1)=xq0;
	state0.v.head(3)=w0;
	state0.v.tail(3)=vq0;

	mxDestroyArray(mxR0);
	mxDestroyArray(mxw0);
	mxDestroyArray(mxxq0);
	mxDestroyArray(mxvq0);
#else
	state0=list_par1.front();

	state0.g.block(0,3,3,1)-=(Vec3::Random()).normalized()*30;
	state0.g.block(0,0,3,3)*=SO3::exp(Vec3::Random().normalized()*PI);
	state0.v.head(3)-=Vec3::Random().normalized()*0;
	state0.v.tail(3)-=Vec3::Random().normalized()*0;
#endif

	// set up simulator
	Sim<quadrotor>::State_Sim sim(sys,dt);
	sim.init(state0);
	
	Vec4 u2=Vec4::Zero();

	int SN=int(params.Ts/dt+0.5);
	
	std::list<Eigen::Matrix<double,4,12> >::const_iterator itr_K=list_K.cbegin();
	std::list<typename quadrotor::State>::const_iterator itr_par2=list_par2.begin();
	std::list<typename quadrotor::U>::const_iterator itr_u2=list_u2.begin();

	bool lqr=0;
	clock_t start,end;

	Vec4 u_curr=list_u1.front();

	double K=10;
	double Aht=exp(-K*dt);

	start=clock();
	for(;T-t>params.Tp;t+=ts)
	{
//		start=clock();

		quadrotor::State state=sim.get_state();	
		quadrotor::State state_ref=list_par1.front();
		Vec12 error=(Vec12()<<SE3::log(SE3::inverse(state_ref.g)*state.g), state.v-state_ref.v).finished();
	
		double err_lqr=6;

		if(lqr)
			err_lqr=8.5;

		if(error.norm()>err_lqr)
		{
			timespec T_start, T_end;
			clock_gettime(CLOCK_MONOTONIC,&T_start);

  		    sac.init(state, params, func,  list_par1.begin(), list_par1.end(), &list_u1);
			sac.sac_ctrl(list_u1,umin,umax,K,u_curr);
			clock_gettime(CLOCK_MONOTONIC,&T_end);
			cout<<"time consumed is "<<(T_end.tv_sec-T_start.tv_sec)+(T_end.tv_nsec-T_start.tv_nsec)/1000000000.0<<"s"<<endl;

			if(lqr)
			{
				lqr=0;
				std::cout<<"SAC: "<<t<<std::endl;
			}

			for(int i=0;i<SN;i++)
			{
				u_curr=list_u1.front();

				sim.update(u_curr);

				list_u1.pop_front();
				list_par1.pop_front();
				list_par1.pop_front();

				itr_par2++;
				itr_u2++;
				itr_K++;
			}
			
		}
		else
		{
			quadrotor::State state_ref=*itr_par2;

			Mat3 R_ref=state_ref.g.block(0,0,3,3);
			Mat3 R=state.g.block(0,0,3,3);

			Vec3 xq_ref=state_ref.g.block(0,3,3,1);
			Vec3 xq=state.g.block(0,3,3,1);
			
			Vec3 omega_ref=state_ref.v.head(3);
			Vec3 omega=state.v.head(3);
			
			Vec3 vq_ref=R_ref*state_ref.v.tail(3);
			Vec3 vq=R*state.v.tail(3);

			Vec12 err=(Vec12()<<SO3::log(R_ref.transpose()*R), 
								xq-xq_ref,
								omega-omega_ref,
								vq-vq_ref).finished();
			u2=*itr_u2-*itr_K*err;
			u2=u2.cwiseProduct(u2);

			Vec4 ut=sim.get_inputs().back();
			u2=u2+Aht*(ut-u2);

			if(u2(0)>umax(0) || u2(1)>umax(1) || u2(2)>umax(2) || u2(3)>umax(3))
			{
				std::cout<<t<<"s "<<": LQR switches to SAC"<<std::endl;

				sac.init(state, params, func,  list_par1.begin(), list_par1.end(), &list_u1);

				sac.sac_ctrl(list_u1,umin,umax,K,u_curr);

				if(lqr)
				{
					lqr=0;
					std::cout<<"SAC: "<<t<<"s"<<std::endl;
				}

				for(int i=0;i<SN;i++)
				{
					u_curr=list_u1.front();

					sim.update(u_curr);

					list_u1.pop_front();
					list_par1.pop_front();
					list_par1.pop_front();


					itr_par2++;
					itr_u2++;
					itr_K++;
				}

			}
			else
			{
				u_curr=u2;

				for(int i=0;i<SN;i++)
				{
					sim.update(u_curr);

					list_u1.pop_front();
					list_par1.pop_front();
					list_par1.pop_front();

					itr_par2++;
					itr_u2++;
					itr_K++;
				}

				if(!lqr)
				{
					lqr=1;
					std::cout<<"LQR: "<<t<<"s: "<<(u2).transpose()<<std::endl;
					end=clock();
					cout<<"time consumed is "<<static_cast<double>(end-start)/CLOCKS_PER_SEC*1000<<" ms"<<endl;
				}
			}
		}

	}
	sim.save("/media/fantaosha/Documents/JHU/Summer 2015/results/quadrotor_track_12s.mat");
	return 0;
}
Пример #4
0
int main()
{

	double M=0.6;
	double d=0.2; // displacement of rotors
	double kt=0.6;
	double km=0.15;
	double K=5;

	double Ix1=8*5e-3; // moment of inertia in X- Y- and Z-axis
	double Iy1=7.5*5e-3;
	double Iz1=13.5*5e-3;
	char file1[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_ud_2.mat";

	double Ix2=8*1e-3; // moment of inertia in X- Y- and Z-axis
	double Iy2=7.5*1e-3;
	double Iz2=13.5*1e-3;
	char file2[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_ud_small_5.mat";

	Eigen::Matrix<double,4,4> B=(Eigen::Matrix<double,4,4>() <<    0, kt*d,    0, -kt*d,
			                                                   -kt*d,    0, kt*d,     0,
															      km,  -km,   km,   -km,
																  kt,   kt,   kt,    kt).finished();

	Eigen::Matrix<double,4,4> Binv=(Eigen::Matrix<double,4,4>() <<          0, -1/(2*d*kt),  1/(4*km),  1/(4*kt),
																   1/(2*d*kt),           0, -1/(4*km),  1/(4*kt),
																            0,  1/(2*d*kt),  1/(4*km),  1/(4*kt),
																  -1/(2*d*kt),           0, -1/(4*km),  1/(4*kt)).finished();

	// load control inputs for SAC
	mxArray *mxR1;
	mxArray *mxw1;
	mxArray *mxxq1;
	mxArray *mxvq1;
	mxArray *mxU1;
	mxArray *mxUd1;

	mxR1=matGetVariable(matOpen(file1,"r"),"state_R");
	mxw1=matGetVariable(matOpen(file1,"r"),"state_w");
	mxxq1=matGetVariable(matOpen(file1,"r"),"state_xq");
	mxvq1=matGetVariable(matOpen(file1,"r"),"state_vq");
	mxU1=matGetVariable(matOpen(file1,"r"),"U");
	mxUd1=matGetVariable(matOpen(file1,"r"),"Ud");

	size_t N=mxGetN(mxU1);
	size_t sN=5;

	void *pR1=mxGetPr(mxR1);
	void *pw1=mxGetPr(mxw1);
	void *pxq1=mxGetPr(mxxq1);
	void *pvq1=mxGetPr(mxvq1);
	void *pU1=mxGetPr(mxU1);

	std::list<typename quadrotor_ext::State> list_ref1;

	for(int i=0;i<N;i+=sN)
	{
		Eigen::Matrix<double,3,3> R;
		Eigen::Matrix<double,3,1> w,xq,vq;
		Eigen::Matrix<double,4,1> u;

		memcpy(R.data(),pR1,sizeof(double)*R.rows()*R.cols());
		memcpy(w.data(),pw1,sizeof(double)*w.rows()*w.cols());
		memcpy(xq.data(),pxq1,sizeof(double)*xq.rows()*xq.cols());
		memcpy(vq.data(),pvq1,sizeof(double)*vq.rows()*vq.cols());
		memcpy(u.data(),pU1,sizeof(double)*u.rows()*u.cols());

		list_ref1.emplace_back(R,xq,w,R.transpose()*vq,u.cwiseProduct(u));

		pR1+=sizeof(double)*R.rows()*R.cols()*sN;
		pw1+=sizeof(double)*w.rows()*w.cols()*sN;
		pxq1+=sizeof(double)*xq.rows()*xq.cols()*sN;
		pvq1+=sizeof(double)*vq.rows()*vq.cols()*sN;
		pU1+=sizeof(double)*u.rows()*u.cols()*sN;
	}

	sN=10;

	std::list<typename quadrotor_ext::U> list_ud1;

	void *pUd1=mxGetPr(mxUd1)+sizeof(double)*5*4;

	for(int i=sN/2;i<N;i+=sN)
	{
		Eigen::Matrix<double,4,1> ud;

		memcpy(ud.data(),pUd1,sizeof(double)*ud.rows()*ud.cols());

		list_ud1.push_back(ud.cwiseProduct(ud));

		pUd1+=sizeof(double)*ud.rows()*ud.cols()*sN;
	}

	mxDestroyArray(mxR1);
	mxDestroyArray(mxw1);
	mxDestroyArray(mxxq1);
	mxDestroyArray(mxvq1);
	mxDestroyArray(mxU1);
	mxDestroyArray(mxUd1);

	// load control inputs for LQR
	mxArray *mxR2;
	mxArray *mxw2;
	mxArray *mxxq2;
	mxArray *mxvq2;
	mxArray *mxU2;
	mxArray *mxUd2;

	mxR2=matGetVariable(matOpen(file2,"r"),"state_R");
	mxw2=matGetVariable(matOpen(file2,"r"),"state_w");
	mxxq2=matGetVariable(matOpen(file2,"r"),"state_xq");
	mxvq2=matGetVariable(matOpen(file2,"r"),"state_vq");
	mxU2=matGetVariable(matOpen(file2,"r"),"U");
	mxUd2=matGetVariable(matOpen(file2,"r"),"Ud");

	N=mxGetN(mxU2);
	sN=10;

	void *pR2=mxGetPr(mxR2);
	void *pw2=mxGetPr(mxw2);
	void *pxq2=mxGetPr(mxxq2);
	void *pvq2=mxGetPr(mxvq2);
	void *pU2=mxGetPr(mxU2);

	std::list<typename quadrotor_ext::State> list_ref2;

	for(int i=0;i<N;i+=sN)
	{
		Eigen::Matrix<double,3,3> R;
		Eigen::Matrix<double,3,1> w,xq,vq;
		Eigen::Matrix<double,4,1> u;

		memcpy(R.data(),pR2,sizeof(double)*R.rows()*R.cols());
		memcpy(w.data(),pw2,sizeof(double)*w.rows()*w.cols());
		memcpy(xq.data(),pxq2,sizeof(double)*xq.rows()*xq.cols());
		memcpy(vq.data(),pvq2,sizeof(double)*vq.rows()*vq.cols());
		memcpy(u.data(),pU2,sizeof(double)*u.rows()*u.cols());

		list_ref2.emplace_back(R,xq,w,R.transpose()*vq,u.cwiseProduct(u));

		pR2+=sizeof(double)*R.rows()*R.cols()*sN;
		pw2+=sizeof(double)*w.rows()*w.cols()*sN;
		pxq2+=sizeof(double)*xq.rows()*xq.cols()*sN;
		pvq2+=sizeof(double)*vq.rows()*vq.cols()*sN;
		pU2+=sizeof(double)*u.rows()*u.cols()*sN;
	}


	std::list<typename quadrotor_ext::U> list_ud2;

	void *pUd2=mxGetPr(mxUd2)+sizeof(double)*4*5;

	for(int i=sN/2;i<N;i+=sN)
	{
		Eigen::Matrix<double,4,1> ud;

		memcpy(ud.data(),pUd2,sizeof(double)*ud.rows()*ud.cols());

		list_ud2.push_back(ud.cwiseProduct(ud));

		pUd2+=sizeof(double)*ud.rows()*ud.cols()*sN;
	}

	mxDestroyArray(mxR2);
	mxDestroyArray(mxw2);
	mxDestroyArray(mxxq2);
	mxDestroyArray(mxvq2);
	mxDestroyArray(mxU2);
	mxDestroyArray(mxUd2);

	std::srand((unsigned int) time(0));

	Mat16 PM=250*Mat16::Identity();
	PM.block(0,0,3,3)*=8;
	PM.block(3,3,3,3)*=40;
	
	PM.block(6,6,6,6)=PM.block(0,0,6,6)/8;
	PM.block(12,12,4,4)*=1e-2;

	Mat16 NM=PM/20;

	quadrotor_ext::Cost cost=std::bind(quad_ext_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, NM);
	quadrotor_ext::DCost dcost=std::bind(quad_ext_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, NM);

	quadrotor_ext::Cost costN=std::bind(quad_ext_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, PM);
	quadrotor_ext::DCost dcostN=std::bind(quad_ext_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, PM);

	SAC<quadrotor_ext>::Functions func(cost, dcost, costN, dcostN);

	SAC<quadrotor_ext>::Params params;

	params.alpha=-10;
	params.beta=0.6;

	params.Tc=1.28;
	params.Tp=1.5;
	params.Ts=0.02;
	params.Tcal=0.01;
	params.kmax=8;
	params.R=0.02*Eigen::Matrix<double,4,4>::Identity();

	quadrotor_ext::System sys1(Ix1,Iy1,Iz1,M,d,km,kt,K);
	double dt=0.01;
	SAC<quadrotor_ext> sac(sys1,dt);

	double t=0;
	double ts=params.Ts;
	double T=108+params.Tp;

	quadrotor_ext::U udmax,udmin;
	udmax<<6,6,6,6;
	udmin<<0.0,0.0,0.0,0.0;

	quadrotor_ext::System sys2(Ix2,Iy2,Iz2,M,d,km,kt,K);
	Mat16 Q=50*Mat16::Identity();
	Q.block(0,0,3,3)*=20;
	Q.block(3,3,3,3)*=5;
	Q.block(6,6,6,6)=50*Mat6::Identity();
	Q.block(12,12,4,4)=10*Mat4::Identity();
	Mat16 Pf=5*Q;
	Mat4 R=4000*Mat4::Identity();
	std::list<Eigen::Matrix<double,4,16> > list_K=quadrotor_ext::lqr(sys2, dt, list_ref2, list_ud2, Q, R, Pf);

	quadrotor_ext::State state0;
	state0=list_ref2.front();
	state0.g.block(0,3,3,1)-=(Vec3::Random()).normalized()*0;
	state0.g.block(0,0,3,3)*=SO3::exp(Vec3::Random().normalized()*PI*0);
	state0.v.head(3)=Vec3::Random().normalized()*0;
	state0.v.tail(3)=Vec3::Random().normalized()*0;
//	state0.u*=0;
	
	Sim<quadrotor_ext> sim(sys2,dt);
	sim.init(state0);

	Vec4 Du((Vec4()<<6,6,6,6).finished());
	Vec4 ud_curr=list_ud1.front();
	
	std::list<Eigen::Matrix<double,4,16> >::const_iterator itr_K=list_K.cbegin();
	std::list<typename quadrotor_ext::U>::const_iterator itr_ud2=list_ud2.begin();

	size_t SN=size_t(params.Ts/sim.dt+0.5);
	
	double err_lqr=6.5;
	double err_sac=8.5;

	bool is_sac=true;

	for(double t=sim.dt*1e-3;t<=96;t+=params.Ts)
	{
		quadrotor_ext::State state=sim.get_state();	
		quadrotor_ext::State state_ref=list_ref2.front();
		Vec12 error=(Vec12()<<SO3::log(SO3::inverse(state_ref.g.block(0,0,3,3))*state.g.block(0,0,3,3)), 
							  state.g.block(0,3,3,1)-state_ref.g.block(0,3,3,1),
							  state.v-state_ref.v).finished();

		double err=error.norm();

		if(is_sac)
		{
			if(err<=err_lqr)
			{
				std::cout<<t<<std::endl;
				is_sac=false;
			}
		}
		else
			if(err>err_sac)
				is_sac=true;

//		is_sac=false;
		if(is_sac)
		{
SAC:		Vec4 F=B*state.u;
			F.head(3)*=5;
			state.u=Binv*F;

		    sac.init(state, params, func,  list_ref1.begin(), list_ref1.end(), &list_ud1);
			sac.sac_ctrl(list_ud1,udmin,udmax,Du,ud_curr);
			
			for(size_t n=0;n<SN;n++)	
			{
				Vec4 ud=list_ud1.front();
				Vec4 Fd=B*ud;
				Fd.head(3)/=5;
				ud_curr=Binv*Fd;

				sim.update(ud_curr);

				list_ud1.pop_front();
				list_ref1.pop_front();
				list_ref1.pop_front();

				list_ref2.pop_front();

				itr_K++;
				itr_ud2++;
			}
		}
		else
		{
			Mat3 R_ref=state_ref.g.block(0,0,3,3);
			Mat3 R=state.g.block(0,0,3,3);

			Vec3 xq_ref=state_ref.g.block(0,3,3,1);
			Vec3 xq=state.g.block(0,3,3,1);
			
			Vec3 omega_ref=state_ref.v.head(3);
			Vec3 omega=state.v.head(3);
			
			Vec3 vq_ref=R_ref*state_ref.v.tail(3);
			Vec3 vq=R*state.v.tail(3);

			Vec16 error((Vec16()<<SO3::log(R_ref.transpose()*R), 
								xq-xq_ref,
								omega-omega_ref,
								vq-vq_ref,
								state.u-state_ref.u).finished());
//			std::cout<<error.transpose()<<std::endl;
			Vec4 ud2=*itr_ud2-*itr_K*error;	

			if(ud2(0)>udmax(0) || ud2(1)> udmax(1) || ud2(2)>udmax(2) || ud2(3)>udmax(3) || ud2(0)<udmin(0) || ud2(1)< udmin(1) || ud2(2)<udmin(2) || ud2(3)<udmin(3))
			{
				is_sac=true;
				goto SAC;
			}
			else
			{
				ud_curr=ud2;
				for(size_t n=0;n<SN;n++)	
				{
					sim.update(ud_curr);

					list_ud1.pop_front();
					list_ref1.pop_front();
					list_ref1.pop_front();

					list_ref2.pop_front();

					itr_K++;
					itr_ud2++;
				}
			}
		}
	}

	sim.save("/media/fantaosha/Documents/JHU/Summer 2015/results/quadrotor_track_12s_ud.mat");
	return 0;
}
Пример #5
0
int main()
{
#ifdef TRACK
	char file[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_1_small.mat";

	mxArray *mxR;
	mxArray *mxw;
	mxArray *mxxq;
	mxArray *mxvq;
	mxArray *mxU;

	mxR=matGetVariable(matOpen(file,"r"),"state_R");
	mxw=matGetVariable(matOpen(file,"r"),"state_w");
	mxxq=matGetVariable(matOpen(file,"r"),"state_xq");
	mxvq=matGetVariable(matOpen(file,"r"),"state_vq");

	mxU=matGetVariable(matOpen(file,"r"),"U");

	size_t N=mxGetN(mxU);
	size_t sN=10;

	void *pR=mxGetPr(mxR);
	void *pw=mxGetPr(mxw);
	void *pxq=mxGetPr(mxxq);
	void *pvq=mxGetPr(mxvq);
	void *pU=mxGetPr(mxU);

	std::vector<typename quadrotor::State> xrefs;
	xrefs.reserve(N/sN+1);

	for(int i=0;i<N;i+=sN)
	{
		Eigen::Matrix<double,3,3> R;
		Eigen::Matrix<double,3,1> w,xq,vq;

		memcpy(R.data(),pR,sizeof(double)*R.rows()*R.cols());
//		R=(Vec3()<<1,-1,-1).finished().asDiagonal();
//		w<<0,0,0;
		memcpy(w.data(),pw,sizeof(double)*w.rows()*w.cols());
		memcpy(xq.data(),pxq,sizeof(double)*xq.rows()*xq.cols());
		memcpy(vq.data(),pvq,sizeof(double)*vq.rows()*vq.cols());

		xrefs.emplace_back(R,xq,w,R.transpose()*vq);

		pR+=sizeof(double)*R.rows()*R.cols()*sN;
		pw+=sizeof(double)*w.rows()*w.cols()*sN;
		pxq+=sizeof(double)*xq.rows()*xq.cols()*sN;
		pvq+=sizeof(double)*vq.rows()*vq.cols()*sN;
	}

	sN=10;

	std::vector<typename quadrotor::U> us;
	us.reserve(N/sN+1);

	for(int i=0;i<N;i+=sN)
	{
		Eigen::Matrix<double,4,1> u;
		memcpy(u.data(),pU,sizeof(double)*u.rows()*u.cols());
		us.push_back(u.cwiseProduct(u));
		pU+=sizeof(double)*u.rows()*u.cols()*sN;
	}
#else
	quadrotor::State xref;
	xref.g.block(0,0,3,3)=SO3::exp((Vec3()<<0,0,0).finished());
	xref.g.block(0,3,3,1)=(Vec3()<<0,0,0).finished();

	std::vector<quadrotor::State> xrefs(4000,xref);
	std::vector<quadrotor::U> us(4000, Vec4::Zero());
#endif

	std::srand((unsigned int) time(0));
	// Set up quadrotor parameters
	double m=0.6;
	double Ix=8*1e-3; // moment of inertia in X- Y- and Z-axis
	double Iy=7.5*1e-3;
	double Iz=13.5*1e-3;
	double d=0.2; // displacement of rotors
	double kt=0.6;
	double km=0.15;

	quadrotor::System sys(Ix,Iy,Iz,m,d,km,kt);
	
	// Set up cost function
	Mat12 Mf=5*Mat12::Identity()/10;
	Mf.block(0,0,3,3)*=1;
	Mf.block(3,3,3,3)*=6;
//	Mf.block(6,6,6,6)=Mf.block(0,0,6,6);
	Mat12 M=Mf/2;
	Mat4 R=Mat4::Identity()/50;
	DDP<quadrotor>::Params params(M,R,Mf);

	// Set up initial state
	quadrotor::State x0=xrefs[0];

	x0.g.block(0,3,3,1)-=(Vec3::Random()).normalized()*3;
	x0.g.block(0,0,3,3)*=SO3::exp(Vec3::Random().normalized()*1);
//	x0.g.block(0,0,3,3)*=SO3::exp((Vec3()<<3.14,0,0).finished());
	x0.v.head(3)-=Vec3::Random().normalized()*0;
	x0.v.tail(3)-=Vec3::Random().normalized()*0;
	// Set up simulator
	double dt=0.01;
	
	size_t num=200;
	Sim<quadrotor> sim(sys,dt);
	sim.init(x0,num);

	Vec4 umin=-Vec4::Ones()*0;
	Vec4 umax=Vec4::Ones()*6;
	
	DDP<quadrotor> ddp(sys,dt);

	double ts=0.02;
	double T=36;
	double Tp=1.5;
	size_t SN=size_t(ts/dt+0.5);
	size_t ND=size_t(Tp/dt+0.5)+1;

	int sn=2;
	int itr_max=20;
	for(int i=0;i<2000;i+=sn)
	{
		timespec T_start, T_end;
		clock_gettime(CLOCK_MONOTONIC,&T_start);
		ddp.init(sim.get_state(), us, xrefs, params, umin, umax, 150);
		ddp.iterate(itr_max,us);
		clock_gettime(CLOCK_MONOTONIC,&T_end);
		std::cout<<"time consumed is "<<(T_end.tv_sec-T_start.tv_sec)+(T_end.tv_nsec-T_start.tv_nsec)/1000000000.0<<"s"<<std::endl;
	
		for(int j=0;j<sn;j++)
		{
			sim.update(us.front());
			xrefs.erase(xrefs.begin());
			us.erase(us.begin());
		}
		Vec12 error=quadrotor::State::diff(sim.get_state(),xrefs[0]);
		std::cout<<dt*i<<": "<<error.head(3).norm()<<" "<<error.head(6).tail(3).norm()<<std::endl;

		if(error.norm()<0.1)
			break;
	}

	sim.save("/media/fantaosha/Documents/JHU/Summer 2015/results/quadrotor_ddp.mat");

}
int main()
{

	//********************************************************
	// Initialization
	//********************************************************

	double M=0.6;
	double Ix=8*5e-3; // moment of inertia in X- Y- and Z-axis
	double Iy=7.5*5e-3;
	double Iz=13.5*5e-3;
	double d=0.2; // displacement of rotors
	double kt=0.6;
	double km=0.15;

	char file[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_7.mat";

//	double M=2; // mass of quadrotor
//	double Ix=1.2; // moment of inertia in X- Y- and Z-axis
//	double Iy=1.2;
//	double Iz=2.3;
//	double d=0.25; // displacement of rotors
//	double kt=1;
//	double km=0.20;
//
//	char file[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_2.mat";

	mxArray *mxR;
	mxArray *mxw;
	mxArray *mxxq;
	mxArray *mxvq;
	mxArray *mxU;

	mxR=matGetVariable(matOpen(file,"r"),"state_R");
	mxw=matGetVariable(matOpen(file,"r"),"state_w");
	mxxq=matGetVariable(matOpen(file,"r"),"state_xq");
	mxvq=matGetVariable(matOpen(file,"r"),"state_vq");
#ifdef ROT_INPUT
	mxU=matGetVariable(matOpen(file,"r"),"U");
#else
	mxU=matGetVariable(matOpen(file,"r"),"F");
#endif
	size_t N=mxGetN(mxU);
	size_t sN=5;

	void *pR=mxGetPr(mxR);
	void *pw=mxGetPr(mxw);
	void *pxq=mxGetPr(mxxq);
	void *pvq=mxGetPr(mxvq);
	void *pU=mxGetPr(mxU);

	std::list<typename quadrotor::State> list_PAR1;
	std::list<typename quadrotor::State> list_PAR2;

	for(int i=0;i<N;i+=sN)
	{
		Eigen::Matrix<double,3,3> R;
		Eigen::Matrix<double,3,1> w,xq,vq;

		memcpy(R.data(),pR,sizeof(double)*R.rows()*R.cols());
		memcpy(w.data(),pw,sizeof(double)*w.rows()*w.cols());
		memcpy(xq.data(),pxq,sizeof(double)*xq.rows()*xq.cols());
		memcpy(vq.data(),pvq,sizeof(double)*vq.rows()*vq.cols());

		list_PAR1.emplace_back(R,xq,w,R.transpose()*vq);

		if((i+1)%2)
			list_PAR2.emplace_back(R,xq,w,R.transpose()*vq);

		pR+=sizeof(double)*R.rows()*R.cols()*sN;
		pw+=sizeof(double)*w.rows()*w.cols()*sN;
		pxq+=sizeof(double)*xq.rows()*xq.cols()*sN;
		pvq+=sizeof(double)*vq.rows()*vq.cols()*sN;
	}

	sN=10;

	std::list<typename quadrotor::U> list_U1;
	std::list<typename quadrotor::U> list_U2;

#ifdef ROT_INPUT
	for(int i=0;i<N;i+=sN)
	{
		Eigen::Matrix<double,4,1> u;
		memcpy(u.data(),pU,sizeof(double)*u.rows()*u.cols());
		list_U1.push_back(u.cwiseProduct(u));
		list_U2.push_back(u);
		pU+=sizeof(double)*u.rows()*u.cols()*sN;
	}
#else
	for(int i=0;i<N;i+=sN)
	{
		Eigen::Matrix<double,4,1> u;
		memcpy(u.data(),pU,sizeof(double)*u.rows()*u.cols());
		list_u.push_back(u);
		pU+=sizeof(double)*u.rows()*u.cols()*sN;
	}
#endif
	mxDestroyArray(mxR);
	mxDestroyArray(mxw);
	mxDestroyArray(mxxq);
	mxDestroyArray(mxvq);
	mxDestroyArray(mxU);

	std::srand((unsigned int) time(0));

	// initialize SAC
	Mat12 PM=2500*Mat12::Identity();
	PM.block(0,0,3,3)*=8;
	PM.block(3,3,3,3)*=4;

	PM.block(6,6,3,3)*=1.2;
	PM.block(9,9,3,3)*=2;

	Mat12 NM=PM/20;

	quadrotor::Cost cost=std::bind(quad_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, NM);
	quadrotor::DCost dcost=std::bind(quad_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, NM);

	quadrotor::Cost costN=std::bind(quad_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, PM);
	quadrotor::DCost dcostN=std::bind(quad_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, PM);

	SAC<quadrotor>::Functions func(cost, dcost, costN, dcostN);

	SAC<quadrotor>::Params params;

	params.alpha=-10;
	params.beta=0.6;

	params.Tc=1.28;
	params.Tp=1.5;
	params.Ts=0.02;
	params.Tcal=0.01;
	params.kmax=8;

//	mat4 k=(mat4()<<0, kt*d,0,-kt*d,
//			        -kt*d, 0, kt*d, 0,
//					km,-km, km, -km,
//					kt, kt, kt, kt).finished();
//
//	vec4 vv=(vec4()<<5,5,10,0.2).finished();
//	params.R=2*K.transpose()*vv.asDiagonal()*K;
	params.R=1*Eigen::Matrix<double,4,4>::Identity();
	double dt=0.01;

	quadrotor::System sys(Ix,Iy,Iz,M,d,km,kt);


	double ts=params.Ts;
	double T=72+params.Tp;

	quadrotor::U umax,umin;
	umax<<6,6,6,6;
	umin<<0,0,0,0;

	// initialize LQR
	Mat12 Q=50*Mat12::Identity();
	Q.block(0,0,3,3)*=10;
	Q.block(3,3,3,3)*=5;
	Q.block(6,6,6,6)=50*Mat6::Identity();
	Mat12 Pf=5*Q;
	Mat4 R=2000*Mat4::Identity();

	std::list<Eigen::Matrix<double,4,12> > list_K=quadrotor::lqr(sys, dt, list_PAR2, list_U2, Q, R, Pf);


	// set up initial configuration
	
    int testN=200;
	int xqN,RN;

	double err_dxq=3,err_dR=PI/10;

	std::ofstream results1,results2;

	results1.open("results1.txt");
	results2.open("results2.txt");

	omp_set_num_threads(6);
	timespec T_start,T_end;
	clock_gettime(CLOCK_MONOTONIC,&T_start);

	std::list<quadrotor::State> list_fail;
	for(xqN=10;xqN<=10;xqN++)
		for(RN=10;RN<=10;RN++)
		{
			double const err_xq=err_dxq*xqN;
			double const err_R=err_dR*RN;

			int succeed=0;
#ifdef PARALLEL
#pragma omp parallel for reduction(+:succeed) schedule(dynamic)  
#endif
			for(int test=1;test<=testN;test++)
			{
				SAC<quadrotor> sac(sys,dt);

				quadrotor::State state0=list_PAR1.front();

				state0.g.block(0,3,3,1)-=(Vec3::Random()).normalized()*err_xq;
				state0.g.block(0,0,3,3)*=SO3::exp(Vec3::Random().normalized()*err_R);
//				state0.v.head(3)-=Vec3::Random().normalized()*0;
//				state0.v.tail(3)-=Vec3::Random().normalized()*0;
				state0.v.head(3)*=0;
				state0.v.tail(3)*=0;

				std::list<typename quadrotor::State> list_par1=list_PAR1;
				std::list<typename quadrotor::State> list_par2=list_PAR2;

				std::list<typename quadrotor::U> list_u1=list_U1;

				// set up simulator
				Sim<quadrotor>::State_Sim sim(sys,dt);
				sim.init(state0);
				
				Vec4 u2=Vec4::Zero();

				int SN=int(params.Ts/dt+0.5);
				
				std::list<Eigen::Matrix<double,4,12> >::const_iterator itr_K=list_K.cbegin();
				std::list<typename quadrotor::State>::const_iterator itr_par2=list_par2.begin();
				std::list<typename quadrotor::U>::const_iterator itr_u2=list_U2.begin();

				bool lqr=0;
				clock_t start,end;

#ifdef LOW_PASS
				std::list<quadrotor::U> list_U;
				size_t UN=5;
				double weights[]={6,5,4,3,2,1};
				double sum_weights[]={6,11,15,18,20,21};
#endif

				Vec4 u_curr=list_u1.front();

#ifdef SMOOTH_INPUTS
				Vec4 Du=1*(Vec4()<<0.5,0.5,0.5,0.5).finished();
#endif
			
				start=clock();
#ifdef PARALLEL
#pragma omp critical
				{
				std::cout<<"================================================================="<<std::endl;
				std::cout<<omp_get_thread_num()<<" "<<omp_get_num_threads()<<std::endl;
				std::cout<<state0.g<<std::endl;
				std::cout<<"================================================================="<<std::endl;
				}
#else
				std::cout<<"==============================================================================="<<std::endl;
				std::cout<<state0.g<<std::endl;
				std::cout<<"================================================================="<<std::endl;
#endif
				

				double TTS=T; // Time to drive the system to the basin of attraction for LQR

				Vec12 error=Vec12::Zero();
				for(double t=0;T-t>params.Tp;t+=ts)
				{
					quadrotor::State state=sim.get_state();	
					quadrotor::State state_ref=list_par1.front();
//					Vec12 error=(Vec12()<<SE3::log(SE3::inverse(state_ref.g)*state.g), state.v-state_ref.v).finished();
					error=(Vec12()<<SE3::log(SE3::inverse(state_ref.g)*state.g), state.v-state_ref.v).finished();

					double err_lqr=6;
					if(lqr)
						err_lqr=8.5;

					if(error.norm()>err_lqr)
					{
						sac.init(state, params, func,  list_par1.begin(), list_par1.end(), &list_u1);
#ifdef SMOOTH_INPUTS
						sac.sac_ctrl(list_u1,umin,umax,Du,u_curr);
#else
						sac.sac_ctrl(list_u1,umin,umax);
#endif

						if(lqr)
						{
							TTS=T;
							lqr=0;
						}

						for(int i=0;i<SN;i++)
						{
#ifdef LOW_PASS
							list_U.push_back(list_u1.front());
							
							Vec4 u1=Vec4::Zero();
							std::list<quadrotor::U>::const_reverse_iterator rit_u=list_U.rbegin();
							size_t index=0;

							while(rit_u!=list_U.rend())
							{
								u1+=*rit_u*weights[index];

								rit_u++;
								index++;
							}

							index--;
							u1/=sum_weights[index];

							if(index>=UN)
								list_U.pop_front();

							u_curr=u1;
#else
							u_curr=list_u1.front();
#endif
		//					std::cout<<t<<": "<<u_curr.transpose()<<std::endl;
							
							sim.update(u_curr);

							list_u1.pop_front();
							list_par1.pop_front();
							list_par1.pop_front();


							itr_par2++;
							itr_u2++;
							itr_K++;
						}
						
					}
					else
					{
						quadrotor::State state_ref=*itr_par2;

						Mat3 R_ref=state_ref.g.block(0,0,3,3);
						Mat3 R=state.g.block(0,0,3,3);

						Vec3 xq_ref=state_ref.g.block(0,3,3,1);
						Vec3 xq=state.g.block(0,3,3,1);
						
						Vec3 omega_ref=state_ref.v.head(3);
						Vec3 omega=state.v.head(3);
						
						Vec3 vq_ref=R_ref*state_ref.v.tail(3);
						Vec3 vq=R*state.v.tail(3);

						Vec12 err=(Vec12()<<SO3::log(R_ref.transpose()*R), 
											xq-xq_ref,
											omega-omega_ref,
											vq-vq_ref).finished();
						u2=*itr_u2-*itr_K*err;
						u2=u2.cwiseProduct(u2);

						if(u2(0)>umax(0) || u2(1)>umax(1) || u2(2)>umax(2) || u2(3)>umax(3))
						{
							sac.init(state, params, func,  list_par1.begin(), list_par1.end(), &list_u1);
#ifdef SMOOTH_INPUTS
							sac.sac_ctrl(list_u1,umin,umax,Du,u_curr);
#else
							sac.sac_ctrl(list_u1,umin,umax);
#endif
							if(lqr)
							{
								lqr=0;
								TTS=T;
#ifndef PARALLEL
								std::cout<<"SAC: "<<t<<"s"<<std::endl;
#endif
							}

							for(int i=0;i<SN;i++)
							{
#ifdef LOW_PASS
								list_U.push_back(list_u1.front());
								
								Vec4 u1=Vec4::Zero();
								std::list<quadrotor::U>::const_reverse_iterator rit_u=list_U.rbegin();
								size_t index=0;

								while(rit_u!=list_U.rend())
								{
									u1+=*rit_u*weights[index];

									rit_u++;
									index++;
								}

								index--;

								u1/=sum_weights[index];

								if(index>=UN)
									list_U.pop_front();

								u_curr=u1;
#else
								u_curr=list_u1.front();
#endif
								sim.update(u_curr);

								list_u1.pop_front();
								list_par1.pop_front();
								list_par1.pop_front();


								itr_par2++;
								itr_u2++;
								itr_K++;
							}

						}
						else
						{
							u_curr=u2;

							for(int i=0;i<SN;i++)
							{
								sim.update(u_curr);

								list_u1.pop_front();
								list_par1.pop_front();
								list_par1.pop_front();
#ifdef LOW_PASS
								if(!list_U.empty())
									list_U.pop_front();
#endif
								itr_par2++;
								itr_u2++;
								itr_K++;
							}

							if(!lqr)
							{
								lqr=1;
#ifndef PARALLEL
								std::cout<<"LQR: "<<t<<"s: "<<(u2).transpose()<<std::endl;
#endif
								TTS=t;
								end=clock();
#ifndef PARALLEL
								std::cout<<"-----------------------------------------------------------------"<<std::endl;
								cout<<"time consumed is "<<static_cast<double>(end-start)/CLOCKS_PER_SEC*1000<<" ms"<<endl;
								std::cout<<"================================================================="<<std::endl;
#endif
							}
						}
					}

				}
#ifndef PARALLEL
				std::cout<<"==============================================================================="<<std::endl<<std::endl;
#endif

				if(lqr)
				{
#ifdef PARALLEL
					succeed+=1;
#else
					succeed++;
#endif
				}
				else
				{
#ifdef PARALLEL
#pragma omp critical
					{
						list_fail.push_back(state0);
					}
#else
						list_fail.push_back(state0);
#endif
				}

#ifdef PARALLEL
#pragma omp critical
				{
//					std::cout<<omp_get_thread_num()<<std::endl;
					std::cout<<"=================================="<<std::endl;
					std::cout<<(error.head(3)).norm()<<std::endl;
					std::cout<<"=================================="<<std::endl;
					results2<<TTS<<" ";
				}
#else
				results2<<TTS<<" ";
#endif
			}
			results2<<std::endl;
			std::cout<<"err_xq="<<err_dxq*xqN<<", err_R="<<0.1*RN<<"Pi"<<" : "<<testN<<" "<<succeed<<std::endl;
			results1<<"err_xq="<<err_dxq*xqN<<", err_R="<<0.1*RN<<"Pi"<<" : "<<testN<<" "<<succeed<<std::endl;

		}

	std::string path_fail="./failture.mat";
	if(!list_fail.empty())
	{
		std::cout<<list_fail.size()<<std::endl;
		quadrotor::State::save(list_fail,path_fail);
	}
	clock_gettime(CLOCK_MONOTONIC,&T_end);
	cout<<"time consumed is "<<(T_end.tv_sec-T_start.tv_sec)+(T_end.tv_nsec-T_start.tv_nsec)/1000000000.0<<"s"<<endl;
//	sim.save("/media/fantaosha/Documents/JHU/Summer 2015/results/quadrotor_track_12s.mat");
	return 0;
}