%
% This script sets up and solves the optimization problem:
%
%  min tr(R)
%   R is PSD
%   R+Q is PSD
%
% where 
%
% Q=[-6 1 0; 1 2 0; 0 0 3];
%
% We fromulate this in SeDuMi standard form by using a block
% diagonal matrix with two blocks of size 3x3.  The first block 
% holds R=X11, while the second block holds R+Q.  Constraints enforce
% the condition that X22=X11+Q (or X22-X11=Q.)
%
% min tr(X11)
%     X22-X11=Q
%     [X11; 0; 0 X22] is PSD
%
%
% First, setup Q.
%
Q=[-6 1 0; 1 2 0; 0 0 3];
%
% Now, setup K and space for A, b, and c.
%
n=3;
K.s=[n n];
A=sparse(n*(n+1)/2,2*n^2);
b=zeros(n*(n+1)/2,1);
c=zeros(1,2*n^2);
%
% Fill in the objective function.
%
tempM=eye(n);
c=sparse([reshape(tempM,1,n^2) zeros(1,n^2)]);
%
% Now, setup the constraints.
%
%
% k will keep track of the row of A that we're filling in.
%
k=1;
%
% One constraint for each entry in the upper triangle of X11 and X22.
%
for i=1:n
%
% Setup the constraint that X22(i,i)-X11(i,i)=Q(i,i)
%
  tempM=zeros(3,3);
  tempM(i,i)=1;
  A(k,:)=sparse([reshape(-tempM,1,n^2) reshape(tempM,1,n^2)]);
  b(k)=Q(i,i);
  k=k+1;
%
% Setup the constraints that X22(i,j)-X11(i,j)=Q(i,j) for j>i.
% These are written in symmetric form as 
%
%  (X22(i,j)+X22(i,j))/2-(X11(i,j)+X11(j,i))/2=Q(i,j)
%
  for j=(i+1):n
    tempM=zeros(3,3);
    tempM(i,j)=1/2;
    tempM(j,i)=1/2;
    A(k,:)=sparse([reshape(-tempM,1,n^2) reshape(tempM,1,n^2)]);
    b(k)=Q(i,j);
    k=k+1;
  end
end
%
% Now, solve the problem.
%
[x,y,z]=csdp(A,b,c,K);
%
% Extract R and RplusQ.
%
R=reshape(x(1:n^2),n,n)
RplusQ=reshape(x((n^2+1):end),n,n)
%
% Look at the trace of R.
%
fprintf('Trace(R)=%f\n',[trace(R)]);
%
% Look at the eigenvalues of R.
%
fprintf('Eigenvalues of R: %f\n',eig(R));
%
% Look at the eigenvalues of RplusQ.
%
fprintf('Eigenvalues of RplusQ: %f\n',eig(RplusQ));