# Diagnose Infeasibility of Simple 3-DoF Trajectory Optimization

7 views (last 30 days)
Ayden Clay on 13 Jun 2022
Commented: Ayden Clay on 14 Jun 2022
I am trying to write a simple optimization program which finds the best initial launch angle and velocity to arrive at a target location. I have written - using this mathworks tutorial as a guide - a program which I believe ought to do this.
First, I define and simulate for a randomly chosen initial guess the trajectory of a 3-DoF aircraft model (and define some constants).
% Constants.
g = 9.81; % gravity
dt = 1e-1;
tspan = 0:dt:5;
e = [50,0]; % target loc.
degs = pi/180; % conversion.
a = 30*degs; % a is angle of launch in rads.
s = 50; % velocity of vehicle at launch.
x = 0;
h = 0;
y0 = [s,a,x,h]';
[~,Y] = ode45(@(t,y) dfu(t,y),tspan,y0);
figure
hold on
plot(Y(:,3),Y(:,4),'k')
plot(e(1),e(2),'bx')
Next, I set up the optimization program and determine the solution.
% Setup Optimization Problem
r = optimvar('r',1,'LowerBound',0,'UpperBound',90*degs);
v = optimvar('v',1,'LowerBound',0,'UpperBound',100);
f = fcn2optimexpr(@RtoODE,r,v,tspan,[v,r,x,h]);
obj = 0;
prob = optimproblem("Objective",obj);
prob.Constraints.terminalx = f(end,3) == e(1);
prob.Constraints.terminaly = f(end,4) == e(2);
r0.r = a;
r0.v = s;
options = optimoptions(@fmincon,'Display','iter');
[rsol,~] = solve(prob,r0,'Options',options);
Solving problem using fmincon. First-order Norm of Iter F-count f(x) Feasibility optimality step 0 3 0.000000e+00 4.520e+01 0.000e+00 1 6 0.000000e+00 4.176e+01 0.000e+00 4.799e-01 2 9 0.000000e+00 4.060e+01 0.000e+00 2.824e-01 3 12 0.000000e+00 4.031e+01 0.000e+00 1.569e-01 4 15 0.000000e+00 4.023e+01 0.000e+00 7.196e-02 5 18 0.000000e+00 4.021e+01 0.000e+00 3.717e-02 6 21 0.000000e+00 4.020e+01 0.000e+00 2.057e-02 7 24 0.000000e+00 4.020e+01 0.000e+00 1.345e-02 8 27 0.000000e+00 4.020e+01 0.000e+00 1.094e-02 9 30 0.000000e+00 4.019e+01 0.000e+00 1.021e-02 10 33 0.000000e+00 4.019e+01 0.000e+00 1.001e-02 11 36 0.000000e+00 4.019e+01 0.000e+00 9.964e-03 12 39 0.000000e+00 4.019e+01 0.000e+00 9.949e-03 13 42 0.000000e+00 4.019e+01 0.000e+00 9.946e-03 14 45 0.000000e+00 4.018e+01 0.000e+00 9.941e-03 15 48 0.000000e+00 4.018e+01 0.000e+00 9.933e-03 16 51 0.000000e+00 4.018e+01 0.000e+00 9.928e-03 17 54 0.000000e+00 4.018e+01 0.000e+00 9.938e-03 18 57 0.000000e+00 4.018e+01 0.000e+00 9.832e-03 19 60 0.000000e+00 4.017e+01 0.000e+00 9.925e-03 20 63 0.000000e+00 4.017e+01 0.000e+00 1.016e-02 21 66 0.000000e+00 4.017e+01 0.000e+00 9.898e-03 22 69 0.000000e+00 4.017e+01 0.000e+00 9.422e-03 23 72 0.000000e+00 4.017e+01 0.000e+00 1.026e-02 24 75 0.000000e+00 4.017e+01 0.000e+00 9.164e-03 25 78 0.000000e+00 4.016e+01 0.000e+00 2.072e-02 26 81 0.000000e+00 4.011e+01 0.000e+00 2.642e-01 27 84 0.000000e+00 4.011e+01 0.000e+00 1.351e-02 28 87 0.000000e+00 4.010e+01 0.000e+00 1.351e-02 29 90 0.000000e+00 4.010e+01 0.000e+00 1.287e-02 30 93 0.000000e+00 4.010e+01 0.000e+00 7.211e-03 First-order Norm of Iter F-count f(x) Feasibility optimality step 31 96 0.000000e+00 4.010e+01 0.000e+00 3.624e-03 32 99 0.000000e+00 4.006e+01 0.000e+00 1.711e-01 33 102 0.000000e+00 4.006e+01 0.000e+00 9.214e-03 34 105 0.000000e+00 3.973e+01 0.000e+00 1.683e+00 35 108 0.000000e+00 3.942e+01 0.000e+00 1.581e+00 36 111 0.000000e+00 3.878e+01 0.000e+00 6.448e+00 37 114 0.000000e+00 3.465e+01 0.000e+00 1.971e+01 38 118 0.000000e+00 3.458e+01 0.000e+00 7.287e-01 39 123 0.000000e+00 3.456e+01 0.000e+00 6.677e-01 40 126 0.000000e+00 3.455e+01 0.000e+00 2.639e-01 41 131 0.000000e+00 3.455e+01 0.000e+00 2.384e-01 42 135 0.000000e+00 3.455e+01 0.000e+00 1.527e-01 43 140 0.000000e+00 3.455e+01 0.000e+00 3.766e-02 44 145 0.000000e+00 3.455e+01 0.000e+00 3.395e-02 45 149 0.000000e+00 3.455e+01 0.000e+00 2.173e-02 46 154 0.000000e+00 3.455e+01 0.000e+00 3.291e-03 47 161 0.000000e+00 3.455e+01 0.000e+00 2.113e-04 48 175 0.000000e+00 3.455e+01 0.000e+00 8.856e-09 49 179 0.000000e+00 3.455e+01 0.000e+00 3.542e-09 Converged to an infeasible point. fmincon stopped because the size of the current step is less than the value of the step size tolerance but constraints are not satisfied to within the value of the constraint tolerance. Consider enabling the interior point method feasibility mode.
y0 = [rsol.v,rsol.r,x,h]';
% Model Production.
[~,Y] = ode45(@(t,y) dfu(t,y),tspan,y0);
plot(Y(:,3),Y(:,4),'r')
% Output check.
angle = rsol.r/degs
angle = 78.6686
velocity = rsol.v
velocity = 79.7992
downrange = Y(end,3)
downrange = 78.3958
altitude = Y(end,4)
altitude = 268.5932
targetx = e(1)
targetx = 50
targety = e(2)
targety = 0
Obviously, this is determining the problem to be infeasible. But I do not suspect this is the case. Just manually tuning some values returns successful solutions to within the required parameters.
Here are the additional functions required to run this program:
function dydt = dfu(~,y)
g = 9.81;
dydt = zeros(4,1);
dydt(1) = -g*sin(y(2));
dydt(2) = -(g*cos(y(2)))/y(1);
dydt(3) = y(1)*cos(y(2));
dydt(4) = y(1)*sin(y(2));
end
function solpts = RtoODE(r,v,tspan,y0)
sol = ode45(@(t,y)dfu(t,y),tspan,y0);
solpts = deval(sol,tspan);
solpts = solpts(3:4,:);
end
I was wondering if perhaps anyone can find my mistake, or offer general advice on simulating and solving optimization problems for this purpose.
Thank you.
Ayden Clay on 14 Jun 2022
Thank you both for the help. I'll follow through the more relevant documentation.

Bjorn Gustavsson on 13 Jun 2022
Why not write the equation of motion component-by-component instead:
function dydtd2ydt2 = dfu(~,y)
g = 9.81;
dydtd2ydt2 = zeros(4,1);
dydy = y(3:4);
d2ydt2 = [0;-g];
dydtd2ydt2 = [dydt(:);d2ydt2];
end
Then you look at the ballode-example for how to catch the ground-impact with event-detection. That will definitely give you the postition of the particle as y(2) (presumably vertical) is equal to zero. From that you can determine how far from your target you landed and that is your function to fit. From there fmincon should be able to do the job. Have look at the help, documentation and code for/of ballode.
HTH
Ayden Clay on 14 Jun 2022
Thank you for correcting my syntax, the change to cosd and sind isn't necessary as I convert everything to radians by multiplying by "degs". Sorry thats my way of converting because then I can say "Oh it's 40 degrees" or "40*degs". I've converted it back and it's exceeding function evaluation limits, probably just a difficult problem for it to solve, but useful to see a working program. Thank you very much!

### Categories

Find more on Systems of Nonlinear Equations in Help Center and File Exchange

R2022a

### Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!