addpath('C:\Users\Siddarth\Documents\MATLAB')
import casadi.*

Missile Longitudinal Dynamics

A = [-1.064 1.000; 290.26 0.00];
B = [-0.25; -331.40];
C = [-123.34 0.00; 0.00 1.00];
D = [0;0];
plant = ss(A,B,C,D);
 
% Check system properties
if rank(ctrb(A,B)) == size(A,1), disp("System is controllable"), end
System is controllable
if rank(obsv(A,C)) == size(A,1), disp("System is observable"), end
System is observable

Discretization

Ts = 0.1;
sysd = c2d(plant,Ts);
Ad = sysd.A; Bd = sysd.B; Cd = sysd.C; Dd = sysd.D;
 
nx = size(Ad,1); nu = size(Bd,2); ny = size(Cd,1);
Qy = diag([2, 0.5]); R = 1;

CasADi symbolic model

x = MX.sym('x',nx); u = MX.sym('u',nu);
x_next = Ad*x + Bd*u;
f = Function('f',{x,u},{x_next});

MPC setup

N = 10;
x0_val = [0; 0];
U = MX.sym('U',nu,N);
X = MX.sym('X',nx,N+1);
x0 = MX.sym('x0',nx);

Reference tracking

Az_step = linspace(0, 15, N); % target normal acceleration (g)
q_ref = zeros(1, N); % target pitch rate
y_ref = [Az_step; q_ref];

3D estimation parameters

Vel = 1021.08; % m/s constant forward velocity
n_steps = 50;
x_sim = zeros(nx, n_steps+1); u_sim = zeros(nu, n_steps);
x_sim(:,1) = x0_val;
pitch_angle = zeros(1,n_steps+1);
x_pos = zeros(1,n_steps+1);
z_pos = zeros(1,n_steps+1);
 
x_pos(1)=1590;
z_pos(1) = 10000; % initial height

MPC loop

for t = 1:n_steps
cost = 0; g = {};
g{end+1} = X(:,1) - x0;
 
% Predict pitch angle symbolically
pitch_pred = MX.zeros(1,N+1);
pitch_pred(1) = pitch_angle(t);
 
% Predict 3D trajectory symbolically
x_pos_pred = MX.zeros(1,N+1);
z_pos_pred = MX.zeros(1,N+1);
x_pos_pred(1) = x_pos(t);
z_pos_pred(1) = z_pos(t);
 
for k = 1:N
pitch_pred(k+1) = pitch_pred(k) + X(2,k)*Ts;
x_k = X(:,k); u_k = U(:,k);
x_next = f(x_k, u_k);
g{end+1} = X(:,k+1) - x_next;
 
% Tracking cost
y_k = C * x_k;
y_ref_k = y_ref(:,k);
cost = cost+(y_k - y_ref_k)' * Qy * (y_k - y_ref_k) + u_k'*R*u_k;
 
% Position integration
x_pos_pred(k+1) = x_pos_pred(k) + Vel*cos(pitch_pred(k+1))*Ts;
z_pos_pred(k+1) = z_pos_pred(k) + Vel*sin(pitch_pred(k+1))*Ts;
end
 
% Terminal soft constraint to target position
target = [10500; 300];
final_pos = [x_pos_pred(end); z_pos_pred(end)];
Qterm = 0.01 * eye(2);
cost = cost + (final_pos - target)' * Qterm * (final_pos - target);
 
% Solve
opt_vars = [reshape(X, nx*(N+1), 1); reshape(U, nu*N, 1)];
nlp = struct('x',opt_vars,'f',cost,'g',vertcat(g{:}),'p',x0);
solver = nlpsol('solver','ipopt',nlp,struct('ipopt',struct('print_level',0)));
 
% Initial guesses
U0 = zeros(nu,N); X0 = repmat(x_sim(:,t),1,N+1); w0 = [X0(:);U0(:)];
sol = solver('x0',w0,'p',x_sim(:,t),'lbg',0,'ubg',0);
w_opt = full(sol.x);
U_opt = reshape(w_opt(nx*(N+1)+1:end),nu,N);
 
% Apply control and update states
u0 = U_opt(:,1); u_sim(:,t) = u0;
x_sim(:,t+1) = Ad * x_sim(:,t) + Bd * u0;
 
% Update pitch and trajectory
pitch_angle(t+1) = pitch_angle(t) + x_sim(2,t)*Ts;
x_pos(t+1) = x_pos(t) + Vel*cos(pitch_angle(t+1))*Ts;
z_pos(t+1) = z_pos(t) + Vel*sin(pitch_angle(t+1))*Ts;
end
solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 90.00us ( 18.00us) 5 nlp_g | 0 ( 0) 51.00us ( 10.20us) 5 nlp_grad_f | 2.00ms (333.33us) 202.00us ( 33.67us) 6 nlp_hess_l | 1.00ms (250.00us) 883.00us (220.75us) 4 nlp_jac_g | 0 ( 0) 164.00us ( 27.33us) 6 total | 13.00ms ( 13.00ms) 12.64ms ( 12.64ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 1.00ms ( 38.46us) 455.00us ( 17.50us) 26 nlp_g | 0 ( 0) 271.00us ( 10.42us) 26 nlp_grad_f | 0 ( 0) 613.00us ( 27.86us) 22 nlp_hess_l | 3.00ms (150.00us) 4.58ms (229.00us) 20 nlp_jac_g | 1.00ms ( 45.45us) 585.00us ( 26.59us) 22 total | 35.00ms ( 35.00ms) 34.62ms ( 34.62ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 150.00us ( 18.75us) 8 nlp_g | 0 ( 0) 95.00us ( 11.87us) 8 nlp_grad_f | 0 ( 0) 283.00us ( 31.44us) 9 nlp_hess_l | 1.00ms (142.86us) 1.47ms (210.57us) 7 nlp_jac_g | 0 ( 0) 231.00us ( 25.67us) 9 total | 13.00ms ( 13.00ms) 12.95ms ( 12.95ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 80.00us ( 16.00us) 5 nlp_g | 0 ( 0) 50.00us ( 10.00us) 5 nlp_grad_f | 0 ( 0) 178.00us ( 29.67us) 6 nlp_hess_l | 1.00ms (250.00us) 757.00us (189.25us) 4 nlp_jac_g | 0 ( 0) 150.00us ( 25.00us) 6 total | 7.00ms ( 7.00ms) 7.26ms ( 7.26ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 1.00ms (250.00us) 58.00us ( 14.50us) 4 nlp_g | 0 ( 0) 35.00us ( 8.75us) 4 nlp_grad_f | 0 ( 0) 144.00us ( 28.80us) 5 nlp_hess_l | 1.00ms (333.33us) 604.00us (201.33us) 3 nlp_jac_g | 0 ( 0) 121.00us ( 24.20us) 5 total | 6.00ms ( 6.00ms) 5.98ms ( 5.98ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 72.00us ( 18.00us) 4 nlp_g | 0 ( 0) 42.00us ( 10.50us) 4 nlp_grad_f | 0 ( 0) 155.00us ( 31.00us) 5 nlp_hess_l | 0 ( 0) 752.00us (250.67us) 3 nlp_jac_g | 0 ( 0) 128.00us ( 25.60us) 5 total | 6.00ms ( 6.00ms) 6.56ms ( 6.56ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 73.00us ( 18.25us) 4 nlp_g | 0 ( 0) 46.00us ( 11.50us) 4 nlp_grad_f | 0 ( 0) 154.00us ( 30.80us) 5 nlp_hess_l | 1.00ms (333.33us) 723.00us (241.00us) 3 nlp_jac_g | 1.00ms (200.00us) 132.00us ( 26.40us) 5 total | 8.00ms ( 8.00ms) 7.35ms ( 7.35ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 62.00us ( 15.50us) 4 nlp_g | 0 ( 0) 34.00us ( 8.50us) 4 nlp_grad_f | 0 ( 0) 148.00us ( 29.60us) 5 nlp_hess_l | 1.00ms (333.33us) 607.00us (202.33us) 3 nlp_jac_g | 0 ( 0) 120.00us ( 24.00us) 5 total | 6.00ms ( 6.00ms) 5.99ms ( 5.99ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 57.00us ( 14.25us) 4 nlp_g | 0 ( 0) 34.00us ( 8.50us) 4 nlp_grad_f | 0 ( 0) 138.00us ( 27.60us) 5 nlp_hess_l | 1.00ms (333.33us) 572.00us (190.67us) 3 nlp_jac_g | 0 ( 0) 122.00us ( 24.40us) 5 total | 7.00ms ( 7.00ms) 6.11ms ( 6.11ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 75.00us ( 18.75us) 4 nlp_g | 0 ( 0) 46.00us ( 11.50us) 4 nlp_grad_f | 0 ( 0) 149.00us ( 29.80us) 5 nlp_hess_l | 2.00ms (666.67us) 722.00us (240.67us) 3 nlp_jac_g | 0 ( 0) 128.00us ( 25.60us) 5 total | 7.00ms ( 7.00ms) 6.58ms ( 6.58ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 62.00us ( 15.50us) 4 nlp_g | 0 ( 0) 35.00us ( 8.75us) 4 nlp_grad_f | 0 ( 0) 155.00us ( 31.00us) 5 nlp_hess_l | 1.00ms (333.33us) 711.00us (237.00us) 3 nlp_jac_g | 0 ( 0) 129.00us ( 25.80us) 5 total | 6.00ms ( 6.00ms) 6.36ms ( 6.36ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 106.00us ( 26.50us) 4 nlp_g | 0 ( 0) 56.00us ( 14.00us) 4 nlp_grad_f | 0 ( 0) 156.00us ( 31.20us) 5 nlp_hess_l | 1.00ms (333.33us) 584.00us (194.67us) 3 nlp_jac_g | 0 ( 0) 134.00us ( 26.80us) 5 total | 8.00ms ( 8.00ms) 7.02ms ( 7.02ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 60.00us ( 15.00us) 4 nlp_g | 0 ( 0) 38.00us ( 9.50us) 4 nlp_grad_f | 0 ( 0) 153.00us ( 30.60us) 5 nlp_hess_l | 0 ( 0) 576.00us (192.00us) 3 nlp_jac_g | 0 ( 0) 129.00us ( 25.80us) 5 total | 7.00ms ( 7.00ms) 6.42ms ( 6.42ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 1.00ms (250.00us) 99.00us ( 24.75us) 4 nlp_g | 0 ( 0) 60.00us ( 15.00us) 4 nlp_grad_f | 0 ( 0) 184.00us ( 36.80us) 5 nlp_hess_l | 0 ( 0) 868.00us (289.33us) 3 nlp_jac_g | 0 ( 0) 165.00us ( 33.00us) 5 total | 7.00ms ( 7.00ms) 7.21ms ( 7.21ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 57.00us ( 14.25us) 4 nlp_g | 0 ( 0) 33.00us ( 8.25us) 4 nlp_grad_f | 0 ( 0) 139.00us ( 27.80us) 5 nlp_hess_l | 0 ( 0) 580.00us (193.33us) 3 nlp_jac_g | 0 ( 0) 119.00us ( 23.80us) 5 total | 6.00ms ( 6.00ms) 6.16ms ( 6.16ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 56.00us ( 14.00us) 4 nlp_g | 0 ( 0) 32.00us ( 8.00us) 4 nlp_grad_f | 0 ( 0) 126.00us ( 25.20us) 5 nlp_hess_l | 1.00ms (333.33us) 535.00us (178.33us) 3 nlp_jac_g | 0 ( 0) 110.00us ( 22.00us) 5 total | 6.00ms ( 6.00ms) 5.72ms ( 5.72ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 60.00us ( 15.00us) 4 nlp_g | 0 ( 0) 32.00us ( 8.00us) 4 nlp_grad_f | 0 ( 0) 129.00us ( 25.80us) 5 nlp_hess_l | 1.00ms (333.33us) 518.00us (172.67us) 3 nlp_jac_g | 0 ( 0) 113.00us ( 22.60us) 5 total | 7.00ms ( 7.00ms) 6.28ms ( 6.28ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 91.00us ( 22.75us) 4 nlp_g | 0 ( 0) 56.00us ( 14.00us) 4 nlp_grad_f | 0 ( 0) 181.00us ( 36.20us) 5 nlp_hess_l | 1.00ms (333.33us) 1.06ms (353.33us) 3 nlp_jac_g | 0 ( 0) 157.00us ( 31.40us) 5 total | 7.00ms ( 7.00ms) 7.51ms ( 7.51ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 81.00us ( 20.25us) 4 nlp_g | 0 ( 0) 41.00us ( 10.25us) 4 nlp_grad_f | 0 ( 0) 194.00us ( 38.80us) 5 nlp_hess_l | 0 ( 0) 692.00us (230.67us) 3 nlp_jac_g | 0 ( 0) 169.00us ( 33.80us) 5 total | 7.00ms ( 7.00ms) 7.15ms ( 7.15ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 103.00us ( 25.75us) 4 nlp_g | 0 ( 0) 74.00us ( 18.50us) 4 nlp_grad_f | 0 ( 0) 214.00us ( 42.80us) 5 nlp_hess_l | 3.00ms ( 1.00ms) 1.54ms (514.33us) 3 nlp_jac_g | 0 ( 0) 164.00us ( 32.80us) 5 total | 9.00ms ( 9.00ms) 8.92ms ( 8.92ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 86.00us ( 21.50us) 4 nlp_g | 0 ( 0) 54.00us ( 13.50us) 4 nlp_grad_f | 0 ( 0) 176.00us ( 35.20us) 5 nlp_hess_l | 2.00ms (666.67us) 892.00us (297.33us) 3 nlp_jac_g | 0 ( 0) 144.00us ( 28.80us) 5 total | 7.00ms ( 7.00ms) 7.42ms ( 7.42ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 92.00us ( 23.00us) 4 nlp_g | 0 ( 0) 60.00us ( 15.00us) 4 nlp_grad_f | 2.00ms (400.00us) 208.00us ( 41.60us) 5 nlp_hess_l | 1.00ms (333.33us) 865.00us (288.33us) 3 nlp_jac_g | 0 ( 0) 163.00us ( 32.60us) 5 total | 8.00ms ( 8.00ms) 7.61ms ( 7.61ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 65.00us ( 16.25us) 4 nlp_g | 0 ( 0) 36.00us ( 9.00us) 4 nlp_grad_f | 0 ( 0) 161.00us ( 32.20us) 5 nlp_hess_l | 1.00ms (333.33us) 633.00us (211.00us) 3 nlp_jac_g | 0 ( 0) 127.00us ( 25.40us) 5 total | 6.00ms ( 6.00ms) 6.36ms ( 6.36ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 85.00us ( 21.25us) 4 nlp_g | 0 ( 0) 68.00us ( 17.00us) 4 nlp_grad_f | 0 ( 0) 164.00us ( 32.80us) 5 nlp_hess_l | 2.00ms (666.67us) 838.00us (279.33us) 3 nlp_jac_g | 0 ( 0) 150.00us ( 30.00us) 5 total | 7.00ms ( 7.00ms) 7.56ms ( 7.56ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 43.00us ( 14.33us) 3 nlp_g | 0 ( 0) 26.00us ( 8.67us) 3 nlp_grad_f | 0 ( 0) 129.00us ( 32.25us) 4 nlp_hess_l | 1.00ms (500.00us) 447.00us (223.50us) 2 nlp_jac_g | 0 ( 0) 95.00us ( 23.75us) 4 total | 5.00ms ( 5.00ms) 4.86ms ( 4.86ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 44.00us ( 14.67us) 3 nlp_g | 0 ( 0) 27.00us ( 9.00us) 3 nlp_grad_f | 0 ( 0) 118.00us ( 29.50us) 4 nlp_hess_l | 0 ( 0) 411.00us (205.50us) 2 nlp_jac_g | 1.00ms (250.00us) 102.00us ( 25.50us) 4 total | 5.00ms ( 5.00ms) 5.03ms ( 5.03ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 41.00us ( 13.67us) 3 nlp_g | 0 ( 0) 26.00us ( 8.67us) 3 nlp_grad_f | 0 ( 0) 129.00us ( 32.25us) 4 nlp_hess_l | 0 ( 0) 435.00us (217.50us) 2 nlp_jac_g | 0 ( 0) 100.00us ( 25.00us) 4 total | 4.00ms ( 4.00ms) 4.83ms ( 4.83ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 46.00us ( 15.33us) 3 nlp_g | 0 ( 0) 27.00us ( 9.00us) 3 nlp_grad_f | 0 ( 0) 120.00us ( 30.00us) 4 nlp_hess_l | 1.00ms (500.00us) 438.00us (219.00us) 2 nlp_jac_g | 0 ( 0) 97.00us ( 24.25us) 4 total | 5.00ms ( 5.00ms) 4.99ms ( 4.99ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 45.00us ( 15.00us) 3 nlp_g | 0 ( 0) 26.00us ( 8.67us) 3 nlp_grad_f | 0 ( 0) 130.00us ( 32.50us) 4 nlp_hess_l | 0 ( 0) 444.00us (222.00us) 2 nlp_jac_g | 0 ( 0) 105.00us ( 26.25us) 4 total | 6.00ms ( 6.00ms) 5.27ms ( 5.27ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 40.00us ( 13.33us) 3 nlp_g | 0 ( 0) 24.00us ( 8.00us) 3 nlp_grad_f | 0 ( 0) 110.00us ( 27.50us) 4 nlp_hess_l | 1.00ms (500.00us) 419.00us (209.50us) 2 nlp_jac_g | 0 ( 0) 93.00us ( 23.25us) 4 total | 5.00ms ( 5.00ms) 4.71ms ( 4.71ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 52.00us ( 17.33us) 3 nlp_g | 0 ( 0) 29.00us ( 9.67us) 3 nlp_grad_f | 0 ( 0) 127.00us ( 31.75us) 4 nlp_hess_l | 1.00ms (500.00us) 473.00us (236.50us) 2 nlp_jac_g | 0 ( 0) 107.00us ( 26.75us) 4 total | 5.00ms ( 5.00ms) 5.15ms ( 5.15ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 43.00us ( 14.33us) 3 nlp_g | 0 ( 0) 27.00us ( 9.00us) 3 nlp_grad_f | 0 ( 0) 123.00us ( 30.75us) 4 nlp_hess_l | 0 ( 0) 451.00us (225.50us) 2 nlp_jac_g | 0 ( 0) 99.00us ( 24.75us) 4 total | 4.00ms ( 4.00ms) 4.58ms ( 4.58ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 1.00ms (333.33us) 41.00us ( 13.67us) 3 nlp_g | 0 ( 0) 23.00us ( 7.67us) 3 nlp_grad_f | 0 ( 0) 112.00us ( 28.00us) 4 nlp_hess_l | 0 ( 0) 412.00us (206.00us) 2 nlp_jac_g | 0 ( 0) 94.00us ( 23.50us) 4 total | 4.00ms ( 4.00ms) 4.80ms ( 4.80ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 63.00us ( 21.00us) 3 nlp_g | 0 ( 0) 43.00us ( 14.33us) 3 nlp_grad_f | 0 ( 0) 147.00us ( 36.75us) 4 nlp_hess_l | 1.00ms (500.00us) 535.00us (267.50us) 2 nlp_jac_g | 2.00ms (500.00us) 129.00us ( 32.25us) 4 total | 7.00ms ( 7.00ms) 6.57ms ( 6.57ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 69.00us ( 23.00us) 3 nlp_g | 0 ( 0) 32.00us ( 10.67us) 3 nlp_grad_f | 0 ( 0) 123.00us ( 30.75us) 4 nlp_hess_l | 1.00ms (500.00us) 513.00us (256.50us) 2 nlp_jac_g | 0 ( 0) 97.00us ( 24.25us) 4 total | 5.00ms ( 5.00ms) 5.24ms ( 5.24ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 41.00us ( 13.67us) 3 nlp_g | 0 ( 0) 25.00us ( 8.33us) 3 nlp_grad_f | 0 ( 0) 116.00us ( 29.00us) 4 nlp_hess_l | 0 ( 0) 415.00us (207.50us) 2 nlp_jac_g | 0 ( 0) 98.00us ( 24.50us) 4 total | 4.00ms ( 4.00ms) 4.87ms ( 4.87ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 54.00us ( 18.00us) 3 nlp_g | 0 ( 0) 34.00us ( 11.33us) 3 nlp_grad_f | 0 ( 0) 128.00us ( 32.00us) 4 nlp_hess_l | 1.00ms (500.00us) 484.00us (242.00us) 2 nlp_jac_g | 0 ( 0) 106.00us ( 26.50us) 4 total | 6.00ms ( 6.00ms) 6.06ms ( 6.06ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 43.00us ( 14.33us) 3 nlp_g | 0 ( 0) 25.00us ( 8.33us) 3 nlp_grad_f | 0 ( 0) 116.00us ( 29.00us) 4 nlp_hess_l | 1.00ms (500.00us) 411.00us (205.50us) 2 nlp_jac_g | 0 ( 0) 98.00us ( 24.50us) 4 total | 5.00ms ( 5.00ms) 4.92ms ( 4.92ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 71.00us ( 23.67us) 3 nlp_g | 0 ( 0) 45.00us ( 15.00us) 3 nlp_grad_f | 0 ( 0) 134.00us ( 33.50us) 4 nlp_hess_l | 2.00ms ( 1.00ms) 604.00us (302.00us) 2 nlp_jac_g | 0 ( 0) 113.00us ( 28.25us) 4 total | 6.00ms ( 6.00ms) 6.16ms ( 6.16ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 57.00us ( 19.00us) 3 nlp_g | 0 ( 0) 32.00us ( 10.67us) 3 nlp_grad_f | 0 ( 0) 141.00us ( 35.25us) 4 nlp_hess_l | 1.00ms (500.00us) 472.00us (236.00us) 2 nlp_jac_g | 0 ( 0) 118.00us ( 29.50us) 4 total | 5.00ms ( 5.00ms) 5.46ms ( 5.46ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 42.00us ( 14.00us) 3 nlp_g | 0 ( 0) 26.00us ( 8.67us) 3 nlp_grad_f | 0 ( 0) 119.00us ( 29.75us) 4 nlp_hess_l | 0 ( 0) 437.00us (218.50us) 2 nlp_jac_g | 0 ( 0) 100.00us ( 25.00us) 4 total | 5.00ms ( 5.00ms) 4.96ms ( 4.96ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 53.00us ( 17.67us) 3 nlp_g | 0 ( 0) 33.00us ( 11.00us) 3 nlp_grad_f | 0 ( 0) 135.00us ( 33.75us) 4 nlp_hess_l | 0 ( 0) 455.00us (227.50us) 2 nlp_jac_g | 0 ( 0) 121.00us ( 30.25us) 4 total | 5.00ms ( 5.00ms) 5.14ms ( 5.14ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 63.00us ( 21.00us) 3 nlp_g | 0 ( 0) 33.00us ( 11.00us) 3 nlp_grad_f | 1.00ms (250.00us) 139.00us ( 34.75us) 4 nlp_hess_l | 1.00ms (500.00us) 552.00us (276.00us) 2 nlp_jac_g | 0 ( 0) 127.00us ( 31.75us) 4 total | 6.00ms ( 6.00ms) 5.43ms ( 5.43ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 46.00us ( 15.33us) 3 nlp_g | 0 ( 0) 24.00us ( 8.00us) 3 nlp_grad_f | 0 ( 0) 118.00us ( 29.50us) 4 nlp_hess_l | 1.00ms (500.00us) 397.00us (198.50us) 2 nlp_jac_g | 0 ( 0) 102.00us ( 25.50us) 4 total | 6.00ms ( 6.00ms) 5.00ms ( 5.00ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 54.00us ( 18.00us) 3 nlp_g | 0 ( 0) 34.00us ( 11.33us) 3 nlp_grad_f | 0 ( 0) 120.00us ( 30.00us) 4 nlp_hess_l | 0 ( 0) 512.00us (256.00us) 2 nlp_jac_g | 1.00ms (250.00us) 105.00us ( 26.25us) 4 total | 5.00ms ( 5.00ms) 5.21ms ( 5.21ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 78.00us ( 26.00us) 3 nlp_g | 2.00ms (666.67us) 49.00us ( 16.33us) 3 nlp_grad_f | 0 ( 0) 162.00us ( 40.50us) 4 nlp_hess_l | 1.00ms (500.00us) 609.00us (304.50us) 2 nlp_jac_g | 0 ( 0) 115.00us ( 28.75us) 4 total | 6.00ms ( 6.00ms) 5.78ms ( 5.78ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 69.00us ( 23.00us) 3 nlp_g | 0 ( 0) 43.00us ( 14.33us) 3 nlp_grad_f | 0 ( 0) 141.00us ( 35.25us) 4 nlp_hess_l | 1.00ms (500.00us) 569.00us (284.50us) 2 nlp_jac_g | 0 ( 0) 118.00us ( 29.50us) 4 total | 6.00ms ( 6.00ms) 6.26ms ( 6.26ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 37.00us ( 12.33us) 3 nlp_g | 0 ( 0) 22.00us ( 7.33us) 3 nlp_grad_f | 0 ( 0) 125.00us ( 31.25us) 4 nlp_hess_l | 0 ( 0) 420.00us (210.00us) 2 nlp_jac_g | 0 ( 0) 96.00us ( 24.00us) 4 total | 5.00ms ( 5.00ms) 4.48ms ( 4.48ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 58.00us ( 19.33us) 3 nlp_g | 0 ( 0) 34.00us ( 11.33us) 3 nlp_grad_f | 1.00ms (250.00us) 134.00us ( 33.50us) 4 nlp_hess_l | 1.00ms (500.00us) 532.00us (266.00us) 2 nlp_jac_g | 0 ( 0) 115.00us ( 28.75us) 4 total | 6.00ms ( 6.00ms) 5.82ms ( 5.82ms) 1 solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 0 ( 0) 42.00us ( 14.00us) 3 nlp_g | 0 ( 0) 24.00us ( 8.00us) 3 nlp_grad_f | 0 ( 0) 113.00us ( 28.25us) 4 nlp_hess_l | 1.00ms (500.00us) 380.00us (190.00us) 2 nlp_jac_g | 0 ( 0) 94.00us ( 23.50us) 4 total | 5.00ms ( 5.00ms) 4.74ms ( 4.74ms) 1

Plot results

time = 0:Ts:n_steps*Ts;
 
figure;
subplot(2,1,1);
plot(time, x_sim(1,:), 'r', 'LineWidth', 1.5); hold on;
plot(time, x_sim(2,:), 'b', 'LineWidth', 1.5);
legend('Alpha', 'q'); title('State Trajectories'); grid on;
 
subplot(2,1,2);
stairs(time(1:end-1), u_sim, 'k', 'LineWidth', 1.5);
title('Control Input'); xlabel('Time (s)'); ylabel('Fin Deflection'); grid on;

3D Trajectory Visualization

y_pos = zeros(size(x_pos)); % 2D path
figure;
plot3(x_pos/1000, y_pos/1000, z_pos/1000, 'b', 'LineWidth', 1.5); hold on;
plot3(x_pos(1)/1000, 0, z_pos(1)/1000, 'go', 'MarkerSize', 10, 'DisplayName','Start');
plot3(x_pos(end)/1000, 0, z_pos(end)/1000, 'ro', 'MarkerSize', 10, 'DisplayName','End');
xlabel('x (km)'); ylabel('y (km)'); zlabel('z (km)');
title('Estimated 3D Missile Trajectory to Target'); legend; grid on;

3D Missile Animation

% Missile body dimensions (in km)
missile_length = 0.1; % 100 meters
missile_radius = 0.02; % 20 meters
 
% Create cone-shaped missile geometry
[cone_x, cone_y, cone_z] = cylinder([0 missile_radius], 20);
cone_z = missile_length * cone_z;
 
% Create figure
figure;
hold on;
axis equal;
grid on;
xlabel('x (km)'); ylabel('y (km)'); zlabel('z (km)');
title('Missile Trajectory Animation');
view(3);
 
% Plot trail
h = plot3(x_pos(1)/1000, y_pos(1)/1000, z_pos(1)/1000, 'b', 'LineWidth', 1.5);
 
% Plot start and end
plot3(x_pos(1)/1000, y_pos(1)/1000, z_pos(1)/1000, 'go', 'MarkerSize', 8, 'DisplayName','Start');
plot3(x_pos(end)/1000, y_pos(end)/1000, z_pos(end)/1000, 'ro', 'MarkerSize', 8, 'DisplayName','End');
 
% Set up missile surface (initially hidden)
missile_surf = surf(nan(size(cone_x)), nan(size(cone_y)), nan(size(cone_z)), ...
'FaceColor', 'r', 'EdgeColor', 'none');
 
% Loop through trajectory
for k = 2:length(x_pos)
% Missile center position (in km)
center = [x_pos(k), y_pos(k), z_pos(k)] / 1000;
 
% Get current pitch angle
theta = pitch_angle(k); % already in radians
 
% Rotation matrix (around y-axis)
R = [cos(theta) 0 sin(theta);
0 1 0;
-sin(theta) 0 cos(theta)];
 
% Rotate and translate cone
rotated = R * [cone_x(:)'; cone_y(:)'; cone_z(:)'];
Xr = reshape(rotated(1,:) + center(1), size(cone_x));
Yr = reshape(rotated(2,:) + center(2), size(cone_y));
Zr = reshape(rotated(3,:) + center(3), size(cone_z));
 
% Update missile surface
set(missile_surf, 'XData', Xr, 'YData', Yr, 'ZData', Zr);
 
% Update trail
set(h, 'XData', x_pos(1:k)/1000, ...
'YData', y_pos(1:k)/1000, ...
'ZData', z_pos(1:k)/1000);
 
drawnow;
pause(0.5);
end