sai kumar - 2020-05-16

Dear all,

by following the periodic reference tracking examples in the provided acado kit, i started implementing a similar code for my reference tracking of x,y position and a specific velocity profile around a track. I was successfull in one step where i had only x,y as reference .

However, i am struggling right now when i have to track three variables. My acado code in matlab looks like below. I really appreciate any help on this and also correcting my mistakes which helps in my understanding.

BEGIN_ACADO;

acadoSet('problemname','TrajectoryTracking');
acadoSet('results_to_file',false);


DifferentialState x y v psi;

Control delta a;
%v = 2.5;
lf = 0.175;
lr = 0.175;
l = lf+lr;

%Differential equations
f = acado.DifferentialEquation(0,4);

f.add(dot(x) == v*cos(psi+atan(lr*tan(delta)/l)));
f.add(dot(y) == v*sin(psi+atan(lr*tan(delta)/l)));
f.add(dot(v) == a);
f.add(dot(psi) == v*sin(atan(lr*tan(delta)/l))/lr);

%optimal control problem

h = {x,y,v};
r = zeros(1,4);
Q = eye(3);
Q(1,1) = 1e5;
Q(2,2) = 1e5; 
Q(3,3) = 1e5;
%Q(4,4) = 1e5;

ocp = acado.OCP(0,4,10); % OCP horizon should be longer than the sampling time
ocp.minimizeLSQ(Q,h,r);

%adding constraints

ocp.subjectTo( f );
ocp.subjectTo( -0.95 <= delta <= 0.95 );
ocp.subjectTo(-0.5 <= a <= 3);
ocp.subjectTo(-22.82 <= x <= 11.05);
ocp.subjectTo(-0.308 <= y <= 19.26);
ocp.subjectTo(2.0 <= v <= 5.0);
identity = acado.OutputFcn();
dynamicSystem = acado.DynamicSystem(f, identity);
process = acado.Process(dynamicSystem, 'INT_RK45');



algo = acado.RealTimeAlgorithm(ocp,0.1); % the last element is the sampling time
algo.set('DISCRETIZATION_TYPE','MULTIPLE_SHOOTING');
algo.set( 'HESSIAN_APPROXIMATION', 'GAUSS_NEWTON' );
algo.set('MAX_NUM_ITERATIONS',1);
algo.set('KKT_TOLERANCE', 1e-10);
%algo.set('INTEGRATOR_TOLERANCE', 1e-1);



%setting up NMPC controller
ref = [Time X Y Z];

reference = acado.PeriodicReferenceTrajectory(ref);
%zeroReference = acado.StaticReferenceTrajectory();
controller = acado.Controller(algo,reference);

sim = acado.SimulationEnvironment( 0.0,50,process,controller);


r(1,1) = -7.959;
r(1,2) = -0.119323;
r(1,3) = 4.6960;
r(1,4) = 0;
sim.init(r);

END_ACADO;

out = TrajectoryTracking_RUN();