Here are my code.
clc; clear all;
close all;
load(‘gamma0.mat’);
m(:,1)=m_0(:,1);
gam(:,1)=gam_0(:,1);
deltat(1)=tau;
r(:,1:3)=r_0(:,1:3);
v(:,1:3)=v_0(:,1:3);
a(:,1:3)=a_0(:,1:3);
T(:,1:3)=T_0(:,1:3);
kar(:,1:3)=kar_0(:,1:3);
KAR(:,1)=KAR_0(:,1);
%%
w_t=0.1;w_T=0.00001;
n_sc=10;
for k=1:N-1
fm(k,:)=-(alpha*0.5*(gam(k,1)+gam(k+1,1))+m_bp)*deltat(1);
fr(k,:)=v(k,1:3)*deltat(1)+1/3*(a(k,1:3)+0.5*a(k+1,1:3))*(deltat(1)^2);
fv(k,:)=0.5*(a(k,1:3)+a(k+1,1:3))*deltat(1);
difft_fm(k,:)=-alpha*0.5*(gam(k,1)+gam(k+1,1))-m_bp;
diffg_fm(k,:)=-alpha*0.5*deltat(1);
difft_fr(k,:)=v(k,1:3)+2/3*(a(k,1:3)+0.5*a(k+1,1:3))*deltat(1);
diffv_fr(k,:)=deltat(1);
difft_fv(k,:)=0.5*(a(k,1:3)+a(k+1,1:3));
end
for i=2:n_sc
cvx_begin
cvx_precision low
variables m_i(N,1) gam_i(N,1) KAR_i(N,1) etaT_i(N,1) deltat_i etat_i ;
variables r_i(N,3) v_i(N,3) a_i(N,3) T_i(N,3) kar_i(N,3);
minimize(-w_mf*m_i(N)+w_kar*norm(KAR_i(:))+w_t*etat_i+w_T*norm(etaT_i(:)));%
subject to
m_i(1)==m0;
r_i(1,:)==r0;
v_i(1,:)==v0;
T_i(1,:)==gam0*n_hat0;
gam_i(1)==gam0;
r_i(N,:)==rf;
v_i(N,:)==vf;
T_i(N,:)==gam_i(N)*n_hatf;
a_i(1,:)==1/m(1,i-1)*(T_i(1,:)-0.5*rho*sd*cd*norm(v(1,3*i-5:3*i-3))*v_i(1,:))+G;%
(deltat_i-deltat(i-1)).^2<=etat_i;
for k=1:N-1
m_i(k+1,:)==m_i(k,:)+fm(k,:)+difft_fm(k,:)*(deltat_i-deltat(i-1))+diffg_fm(k,:)*(gam_i(k)-gam(k,i-1)+gam_i(k+1)-gam(k+1,i-1));
r_i(k+1,:)==r_i(k,:)+fr(k,:)+difft_fr(k,:)*(deltat_i-deltat(i-1))+diffv_fr(k,:)*(v_i(k,:)-v(k,3*i-5:3*i-3));
v_i(k+1,:)==v_i(k,:)+fv(k,:)+difft_fv(k,:)*(deltat_i-deltat(i-1));
a_i(k+1,:)==1/m(k+1,i-1)*(T_i(k+1,:)-0.5*rho*sd*cd*norm(v(k+1,3*i-5:3*i-3))*v_i(k+1,:))+kar_i(k+1,:)+G;%
0<=deltat_i;%
m_i(k)<=m0; %
m_dry<=m_i(k);
norm(r_i(k,:))*cos(2*pi*80/360)<=r_i(k,1);
norm(T_i(k,:))<=gam_i(k);
Tmin<=gam_i(k);
gam_i(k)<=Tmax;
gam_i(k)*cos(2*pi*15/360)<=T_i(k,1);
Tmin_dot*deltat_i<=gam_i(k+1)-gam_i(k);
gam_i(k+1)-gam_i(k)<=Tmax_dot*deltat_i;
norm(kar_i(k,:))<=KAR_i(k);
(T_i(k,:)-T(k,3*i-5:3*i-3))*(T_i(k,:)-T(k,3*i-5:3*i-3))'<=etaT_i(k);%
end
norm(kar_i(k+1,:))<=KAR_i(k+1);
(T_i(k+1,:)-T(k+1,3*i-5:3*i-3))*(T_i(k+1,:)-T(k+1,3*i-5:3*i-3))'<=etaT_i(k+1);
cvx_end
%%
deltat(i)=deltat_i;
etat(i)=etat_i;
m(:,i)=m_i(:,1);
gam(:,i)=gam_i(:,1);
KAR(:,i)=KAR_i(:,1);
etaT(:,i)=etaT_i(:,1);
r(:,3*i-2:3*i)=r_i(:,1:3);
v(:,3*i-2:3*i)=v_i(:,1:3);
a(:,3*i-2:3*i)=a_i(:,1:3);
T(:,3*i-2:3*i)=T_i(:,1:3);
kar(:,3*i-2:3*i)=kar_i(:,1:3);
%%
for k=1:N-1
fm(k,:)=-(alpha*0.5*(gam_i(k,1)+gam_i(k+1,1))+m_bp)*deltat_i;%
fr(k,:)=v_i(k,1:3)*deltat_i+1/3*(a_i(k,1:3)+0.5*a_i(k+1,1:3))*(deltat_i^2);%
fv(k,:)=0.5*(a_i(k,1:3)+a_i(k+1,1:3))*deltat_i;%
difft_fm(k,:)=-alpha*0.5*(gam_i(k,1)+gam_i(k+1,1))-m_bp;%
diffg_fm(k,:)=-alpha*0.5*deltat_i;%
difft_fr(k,:)=v_i(k,1:3)+2/3*(a_i(k,1:3)+0.5*a_i(k+1,1:3))*deltat_i;%
diffv_fr(k,:)=deltat_i;%
difft_fv(k,:)=0.5*(a_i(k,1:3)+a_i(k+1,1:3));%
end
Calling SDPT3 4.0: 961 variables, 569 equality constraints
num. of constraints = 569
dim. of sdp var = 2, num. of sdp blk = 1
dim. of socp var = 583, num. of socp blk = 124
dim. of linear var = 362
dim. of free var = 13 *** convert ublk to lblk
SDPT3: Infeasible path-following algorithms
version predcorr gam expon scale_data
HKM 1 0.000 1 0
it pstep dstep pinfeas dinfeas gap prim-obj dual-obj cputime
0|0.000|0.000|7.2e+01|1.1e+01|2.0e+13| 4.242669e+11 0.000000e+00| 0:0:00| spchol 2 2
1|0.624|0.793|2.9e+01|2.3e+00|5.4e+12| 1.630238e+11 2.395756e+11| 0:0:01| spchol
linsysolve: Schur complement matrix not positive definite
switch to LU factor. splu 12 ^ 3
2|0.438|0.375|1.6e+01|1.4e+00|3.8e+12| 9.299836e+10 -8.225259e+10| 0:0:01| splu 16 ^19
3|0.444|0.323|9.1e+00|9.7e-01|2.6e+12| 5.279545e+10 -7.805540e+09| 0:0:01| splu 11 ^12
4|0.281|0.107|6.5e+00|8.7e-01|2.2e+12| 3.863987e+10 -1.538032e+10| 0:0:01| splu 30 ^30
5|0.451|0.164|3.6e+00|7.3e-01|1.6e+12| 2.236989e+10 -2.782289e+10| 0:0:01| splu 13 30
6|0.545|0.184|1.6e+00|5.9e-01|1.1e+12| 1.278220e+10 -3.963782e+10| 0:0:02| splu 13 ^16
7|0.395|0.240|9.9e-01|4.5e-01|8.3e+11| 8.788366e+09 -4.724970e+10| 0:0:02| splu 11 ^25
8|0.615|0.135|3.8e-01|3.9e-01|5.3e+11| 5.049415e+09 -4.803999e+10| 0:0:02| splu 15 ^10
9|0.652|0.332|1.3e-01|2.6e-01|3.4e+11| 3.071175e+09 -3.810893e+10| 0:0:02| splu 15 ^22
10|0.638|0.183|4.8e-02|2.1e-01|2.8e+11| 2.770365e+09 -3.446629e+10| 0:0:02| splu 30 30
11|0.820|0.697|8.6e-03|6.4e-02|8.7e+10| 1.380808e+09 -1.474642e+10| 0:0:02| splu 30 ^24
12|0.758|0.352|2.1e-03|4.2e-02|5.7e+10| 1.030729e+09 -1.055073e+10| 0:0:02| splu 30 30
13|0.621|0.702|7.9e-04|1.2e-02|1.7e+10| 6.492244e+08 -2.217756e+09| 0:0:02| splu 19 30
14|0.501|0.832|3.9e-04|2.1e-03|3.5e+09| 3.591611e+08 -3.676312e+08| 0:0:02| splu 14 30
15|0.779|0.814|8.7e-05|4.2e-04|1.2e+09| 1.468642e+08 -3.432020e+08| 0:0:02| splu 17 30
16|0.837|0.783|1.4e-05|1.0e-04|4.0e+08| 5.265053e+07 -1.575533e+08| 0:0:02| splu 11 30
17|0.656|0.546|4.3e-06|4.7e-05|2.2e+08| 2.517001e+07 -9.243113e+07| 0:0:03| splu 30 ^18
18|0.835|0.439|8.5e-07|2.7e-05|1.2e+08| 7.364936e+06 -5.369169e+07| 0:0:03| splu 30 30
19|0.974|0.968|8.9e-08|9.8e-07|4.0e+06| 2.673829e+05 -1.460727e+06| 0:0:03| splu 21 30
20|0.985|0.912|6.5e-08|9.3e-08|2.5e+05|-8.385020e+03 -3.334937e+04| 0:0:03| splu 30 30
21|0.988|0.866|1.5e-09|1.3e-08|3.3e+04|-1.374729e+04 -1.370150e+04| 0:0:03| splu 30 30
22|0.974|0.975|3.3e-02|3.6e-10|1.1e+03|-1.381661e+04 -1.370150e+04| 0:0:03| splu 11 1
23|0.377|0.033|2.0e-02|3.9e-10|1.1e+03|-1.383251e+04 -1.366454e+04| 0:0:03| splu 11 ^10
24|1.000|0.502|1.5e-10|2.2e-10|6.0e+02|-1.386323e+04 -1.359432e+04| 0:0:03| splu 14 30
stop: primal infeas has deteriorated too much, 1.1e+15
25|0.124|0.000|1.5e-10|2.2e-10|6.0e+02|-1.386323e+04 -1.359432e+04| 0:0:03|
number of iterations = 25
primal objective value = -1.38632312e+04
dual objective value = -1.35943176e+04
gap := trace(XZ) = 5.98e+02
relative gap = 2.18e-02
actual relative gap = -9.79e-03
rel. primal infeas (scaled problem) = 1.47e-10
rel. dual " " " = 2.23e-10
rel. primal infeas (unscaled problem) = 0.00e+00
rel. dual " " " = 0.00e+00
norm(X), norm(y), norm(Z) = 1.8e+07, 3.5e+08, 7.4e+06
norm(A), norm(b), norm© = 5.5e+05, 1.5e+06, 5.0e+05
Total CPU time (secs) = 3.38
CPU time per iteration = 0.14
termination code = -7
DIMACS: 9.1e-10 0.0e+00 2.2e-10 0.0e+00 -9.8e-03 2.2e-02
Status: Failed
Optimal value (cvx_optval): NaN