C++ options::Multipliers方法代码示例

void BackwardSweep(options & nominal){

	double x, y, z, xdot, ydot, zdot;
	double xf, yf, zf, xdotf, ydotf, zdotf;
	double l0, l1, l2, l3, l4, l5;

	// Extract some information
	x = nominal.traj[nominal.nstages][1];
	y = nominal.traj[nominal.nstages][2];
	z = nominal.traj[nominal.nstages][3];
	xdot = nominal.traj[nominal.nstages][4];
	ydot = nominal.traj[nominal.nstages][5];
	zdot = nominal.traj[nominal.nstages][6];

	xf = nominal.r_target[0];
	yf = nominal.r_target[1];
	zf = nominal.r_target[2];
	xdotf = nominal.v_target[0];
	ydotf = nominal.v_target[1];
	zdotf = nominal.v_target[2];

	l0 = nominal.Multipliers(0);
	l1 = nominal.Multipliers(1);
	l2 = nominal.Multipliers(2);
	l3 = nominal.Multipliers(3);
	l4 = nominal.Multipliers(4);
	l5 = nominal.Multipliers(5);

	// Compute Terminal Cost Derivatives
	Matrix<double> Vl(6, 1), Vll(6, 6, 0.0), Vx(7, 1), Vxx(7, 7, 0.0), Vxl(7, 6);

	Vl(0,0) = pow(x - xf, 2.0);
	Vl(1,0) = pow(y - yf, 2.0);
	Vl(2,0) = pow(z - zf, 2.0);
	Vl(3,0) = pow(xdot - xdotf, 2.0);
	Vl(4,0) = pow(ydot - ydotf, 2.0);
	Vl(5,0) = pow(zdot - zdotf, 2.0);

	Vx(0,0) = l0*(x*2.0 - xf*2.0);
	Vx(1,0) = l1*(y*2.0 - yf*2.0);
	Vx(2,0) = l2*(z*2.0 - zf*2.0);
	Vx(3,0) = l3*(xdot*2.0 - xdotf*2.0);
	Vx(4,0) = l4*(ydot*2.0 - ydotf*2.0);
	Vx(5,0) = l5*(zdot*2.0 - zdotf*2.0);
	Vx(6,0) = -1.0*nominal.s;

	Vxx(0,0) = l0*2.0;
	Vxx(1,1) = l1*2.0;
	Vxx(2,2) = l2*2.0;
	Vxx(3,3) = l3*2.0;
	Vxx(4,4) = l4*2.0;
	Vxx(5,5) = l5*2.0;

	Vxl(0,0) = x*2.0 - xf*2.0;
	Vxl(1,1) = y*2.0 - yf*2.0;
	Vxl(2,2) = z*2.0 - zf*2.0;
	Vxl(3,3) = xdot*2.0 - xdotf*2.0;
	Vxl(4,4) = ydot*2.0 - ydotf*2.0;
	Vxl(5,5) = zdot*2.0 - zdotf*2.0;
	
	// Start Sweeping
	Matrix<double> lx(7, 1), lxx(7,7), lu(3, 1), luu(3,3), lux(3, 7);
	nominal.ER = 0.0;
	for (int stage = nominal.nstages - 1; stage >= 0; stage--){
		Matrix<double> identity(3, 3), G, K, A, B, C;

		// Stage Cost Derivatives
		lx = 0.0;
		lxx = 0.0;
		lux = 0.0;
		lu = 0.0;
		luu = 0.0;

		Matrix<double> u_stage(3, 1);
		u_stage(0, 0) = nominal.traj[stage][8];
		u_stage(1, 0) = nominal.traj[stage][9];
		u_stage(2, 0) = nominal.traj[stage][10];
		/*
		for (int i = 0; i < 3; i++){
			lu(0, i) = .01*nominal.delta_u[stage](i,0);
			luu(i, i) = .01*1.0;
		}
		*/
		// Partition the STM
		Matrix<double> STM = nominal.STMs[stage];
		Matrix<double> STMx(7,7), STMu(7,3);

		for (int i = 0; i < 7; i++)
			for (int j = 0; j < 7; j++)
				STMx(i, j) = STM(i, j);

		for (int i = 0; i < 7; i++)
			for (int j = 0; j < 3; j++)
				STMu(i, j) = STM(i, 7 + j);

		// Partition the STT
		vector< Matrix<double> > STT, STTxx(7, Matrix<double>(7, 7)), STTuu(3, Matrix<double>(7, 3)), STTux(7, Matrix<double>(7, 3));
		
		for (int i = 0; i < 10; i++)
			STT.push_back(nominal.STTs[stage + i]);
//.........这里部分代码省略.........