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,:)= (Jwdot(1,:)’+crossm(w(1,:)’)Jw(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_Iyc_B’+yc_Bxs_I’-(xs_I’yc_B+cos(50pi/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+mueye(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