Hello all,

I would really appreciate if someone can please point out the errors in my code. My variables and objective values are coming out to be zero and nearly zeros. The problem I am trying to reproduce is a spacecraft reorientation problem (https://ieeexplore.ieee.org/document/1271671/). The problem and my code are as follows:

clc;clear all;close all;

ti=0;tf=20;dt=1;t=[0:dt:tf]’;

n =((tf-ti)/dt);%divide t0-tf into n intervals

J=diag([100,200,300]);

x=zeros(n,10);

u=x(2:end,1:3);

w=x(:,4:6);

q=x(:,7:10);

X=x(2:end,:);

% Initial and final conditions of angular velocity and quaternions

q(1,:)=[0.5, 0.5, 0.5, 0.5];%q(1,:)=q(1,:)/norm(q(1,:));

w(1,:)=[0.2,0.2,0.2]*pi/180;%[0.2,0.2,0.2]*pi/180;[sin(0.01) sin(0.0085) cos(0.0085)]
wdot(1,:)=zeros(3,1)’;% calculated analytically from w(1,: )
u(1,:)= (J*wdot(1,:)’+crossm(w(1,:)’)

*J*w(1,:)’)’;%zeros(1,3);% calculated from initial values of q and w

q(end,:)=[0.0258, 0.0258, 0.999, 0.0258];%q(end,:)=q(end,:)/norm(q(end,:));

w(end,:)=[0,0,0];

x(1,:)=[ zeros(1,3), w(1,:), q(1,:)];

x(end,:)=[u(end,: ), w(end,: ), q(end,:)];

G1=[eye(3,3) zeros(3,7)];%dim=3x10

gamma1=03;% control bound (rad/s^2)

G2=[zeros(3,3) eye(3) zeros(3,4)];%dim=3x10

gamma2=.3;% angular velocity bound (rad/s)

xs_I=[0; 0; 1];% inertial coordinates of sun vector

yc_B=[0.750;0.4330;0.500];%spacecraft camera vector in body coordinates

A =xs_I*yc_B’+yc_B*xs_I’-(xs_I’*yc_B+cos(50*pi/180))*eye(3);%dim=3x3

b = [0 -xs_I(3) xs_I(2);xs_I(3) 0 -xs_I(1);-xs_I(2) xs_I(1) 0]*yc_B;%dim=3x1
d = xs_I’ yc_B-cos(50pi/180);%dim=1x1
Atilda = [A b;
b’ d];
% testAtilda_sym = issymmetric(Atilda);
% testAtilda_psd = isPositiveDefinite(Atilda);
eig_atilda=eig(Atilda);
tol=1e-12;
mu=max(tol,abs(min(eig(Atilda))));%abs(min(eig(Atilda)))
Atilda1=Atilda+mu*eye(4);% making the matrix in non-convex constraint positive semi-definite

eig_atilda1=eig(Atilda1);

H = [zeros(6,6) zeros(6,4);zeros(4,6) Atilda1];%dim=10x10

C=[eye(3) zeros(3,1)];

Q_n=[q(end, 4) q(end,3) -q(end,2) -q(end,1);

-q(end,3) q(end,4) q(end,1) -q(end,2);

q(end,2) -q(end,1) q(end,4) -q(end,3);

q(end,1) q(end,2) q(end,3) q(end,4)];

E=[zeros(3) zeros(3) zeros(3,4) zeros(3,1);

zeros(3) eye(3) zeros(3,4) w(end,:)’;

zeros(4,3) zeros(4,3) Q_n’*Q_n zeros(4,1);

zeros(1,3) w(end,: ) zeros(1,4) w(end,:)*w(end,:)’];

for k=1:n-1

disp(k);

X(k,:)= [u(k,:), w(k+1,:), q(k+1,:)];%dim=nx10

```
R{k}=0.5*dt*[-q(k,4) q(k,3) -q(k,2);
-q(k,3) -q(k,4) q(k,1);
q(k,2) -q(k,1) -q(k,4);
q(k,1) q(k,2) q(k,3)];
F{k}=[-dt*eye(3,3) J zeros(3,4);
zeros(4,3) R{k} eye(4)];
y{k}=[J(1,1)*w(k,1)+dt*(J(2,2)-J(3,3))*w(k,2)*w(k,3);
J(2,2)*w(k,2)+dt*(J(3,3)-J(1,1))*w(k,3)*w(k,1);
J(3,3)*w(k,3)+dt*(J(1,1)-J(2,2))*w(k,1)*w(k,2);
q(k,:)'];
cvx_begin
cvx_solver sdpt3
% cvx_precision medium
variables u(n-1,3) q(n,4) w(n,3)
```

% expression X(n-1,10)

% X(k,:)= [u(k,:), w(k+1,:), q(k+1,:)];%dim=nx10

```
minimize(sum_square_abs(w(k+1,:)'-w(end,:)')+sum_square_abs(Q_n*q(k+1,:)'))
%minimize(norm(w(k+1,:)'-w(end,:)')+norm(C*(Q_n*q(k+1,:)')))
%minimize ([X(k,:)';1]'*E*[X(k,:)';1]+norm(q(k+1,:)))
%minimize (quad_form([X(k,:)';1],E)+sum_square_abs(q(k+1,:)))
subject to
F{k}*X(k,:)' == y{k};
abs(G1*X(k,:)')<= gamma1*ones(3,1);
abs(G2*X(k,:)')<= gamma2*ones(3,1);
X(k,:)*H*X(k,:)' <=0;
cvx_end
Xstore(k,:)=[u(k,:) w(k+1,:) q(k+1,:)];
```

end