clear all; clc;
global m S CD g rou0 Hre RE
%% Parameter Declaration
v_ini = 300 ; % m/s
the_ini = 0 / 180 * pi ; % rad
x_ini = 0 ; % m
y_ini = 2500 ; % m
x_fin = 5000 ; % m
y_fin = 0 ; % m
the_upp = - 80 / 180 * pi ; % rad
the_low = - 90 / 180 * pi ; % rad
col_upp = 60 ; % m/s2
col_low = - 60 ; % m/s2
m = 200 ; % kg
S = 0.04 ; % m2
CD = 1 ;
g = 9.8 ; % m /s2
rou0 = 1.255 ; % kg/m3
Hre = 7.6 * 1e3 ; % m
RE = 6378 * 1e3 ; % m
t0 = 0 ; % s
tf = 30 ; % s
%% Numerical Settings
X_ini = [ v_ini ; the_ini ; x_ini ; y_ini ] ;
N = 100 ; % number of phases
lmax = 15 ;
eps = 1e-3 ;
h = ( tf - t0 ) / N ;
[ ta , Xa_0 ] = RULER( @Dynamics,X_ini,t0,tf,N ) ;
X = Xa_0 ;
a = zeros(1,N+1) ;
%% Sequential CVX Optimization
for l = 1:lmax
XX = X ;
aa = a ;
cvx_solver mosek
cvx_begin
variables X(4,N+1) a(1,N+1) yita
minimize ( sum(abs(a)) + yita )
subject to
X(:,1) == X_ini ;
X(2,end) <= the_upp ;
the_low <= X(2,end) ;
X(3,end) == x_fin ;
X(4,end) == y_fin ;
for i = 1 : N+1
a(:,i) <= col_upp ;
col_low <= a(:,i) ;
norm(a(:,i)-aa(:,i)) <= yita ;
norm(X(:,i)-XX(:,i)) <= yita ;
end
for i = 2 : N+1
[ Ak , Bk , Fk ] = Coeff( XX(:,i-1) , aa(:,i-1) ) ;
dy = Fk + Ak * ( X(:,i-1) - XX(:,i-1) ) +...
Bk * ( a(:,i-1) - aa(:,i-1) ) ;
X(:,i) == X(:,i-1) + h * dy ;
end
cvx_end
if yita < eps
break;
end
[ l , yita ]
end
%% Data Extraction and Plot Figures
ll = 1 ;
figure(ll)
hold on
grid on
box on
xlabel('T/s');
ylabel('V/ m/s');
plot(ta,X(ll,:),'LineWidth',1.5);
title('速度曲线');
ll = ll +1 ;
axis([0,30,180,300]);
figure(ll)
hold on
grid on
box on
xlabel('T/s');
ylabel('theta/ rad');
plot(ta,X(ll,:),'LineWidth',1.5);
title('弹道倾角曲线')
ll = ll +1 ;
axis([0,30,-1.6,0.4]);
figure(ll)
hold on
grid on
box on
xlabel('T/s');
ylabel('x/ m');
plot(ta,X(ll,:),'LineWidth',1.5);
ll = ll +1 ;
figure(ll)
hold on
grid on
box on
xlabel('T/s');
ylabel('y/ m');
plot(ta,X(ll,:),'LineWidth',1.5);
ll = ll +1 ;
figure(ll)
hold on
grid on
box on
xlabel('T/s');
ylabel('control/ m/s2');
plot(ta,a(:,:),'LineWidth',1.5);
title('控制力加速度曲线');
axis([0,30,-60,60]);
figure;
hold on
grid on
box on
xlabel('x/ m');
ylabel('y/ m');
plot(X(3,:),X(4,:),'LineWidth',1.5);
title('弹道');