	
	function [Y_opt,X_opt,tvar,error]=adhocnsedumiei2(PP,m,n,Radius,nf,degree)
    
    % Solves position estimation problem (SDP form). Handle both equality and
    % inequality, developed on 9/21. Last updated 9/24: add comments and nf.
    
    % 9/28: minimize the sum of errors.
    
    % PP: sensor point positions

    % m: number of assumed known points, they are PP(:,1:m)

    % n: number of total points, they are PP(:,1:n)

    % Radius: like a radio range; the distance estimation of two points whose 
    % distance is less than Radius will be used by the algorithm. Other
    % paris wise distances are assummed greater than Radius
    
    % nf: noise factor. The distance estimation between two nearby points
    % is computed by true-distance*(1+randn(1)*nf). 
    
    % Since sedumi use homogeneous and self-dual model and allow to start from 
    % infeasible point, no BigM is needed
    
    % X_opt: position estimation for all n-m unknown points made by the algorithm
    
    % Y_opt: the output SDP matrix representing the second moment of X_opt, that is
    % X_opt'*X_opt.
    
    % tvar(i): Y_opt(i,i)-X_opt(:,i)'*X_opt(:,i), ith sensor's trace (variance).
    % the smaller the trace, the more accurate localization of the point.
    
    % error(i): norm(X_opt(:,i)- PP(:,m+i)): distance between the true position and
    % computed position from by the algorithm.
    
    close all;
    PP=[PP(:,m+1:n) PP(:,1:m)];
    dd = n-m;
    C=vec(sparse(zeros(dd+2,dd+2)));
    b=zeros(3,1);
    a=sparse([zeros(1,dd) 1 0]');
    A=vec(a*a');
    b(1)=1;
    a=sparse([zeros(1,dd) 0 1]');
    A=[A vec(a*a')];
    b(2)=1;
    a=sparse([zeros(1,dd) 1 1]');
    A=[A vec(a*a')];
    b(3)=2;

    % Compute pair-wise distances within R limit between known to unknown and
    % uunknown to unkown. If distance is greater than R, set it to R+.
    %
    d=sparse(zeros(n,n));
    mtotal=3;
    for i=1:dd
        flag = 0 ;
        for j=(i+1):n
            
            rr= norm(PP(:,i)-PP(:,j))*max(0,(1+randn(1)*nf));
            
            if rr < Radius && flag < degree
               flag =flag+1;
               mtotal=mtotal+1;
               d(i,j)=1;
               if j <= dd
                  % j is an unknown points
                  a=sparse([zeros(1,i-1) 1 zeros(1,j-i-1) -1 zeros(1,dd-j+2)]');
                  A(:,mtotal)=vec(a*a');    
               else
                  % j is a known point 
                  a=sparse([zeros(1,i-1) 1 zeros(1,dd-i) -PP(:,j)']');
                  A(:,mtotal)=vec(a*a');
               end;
               b(mtotal) = rr^2;
            end;

        end;

    end;
    % Include only equality constraints in the first round, since the
    % algorithm uses active constraint strategy.
    mequal=mtotal
    % Set up input data to call dsdp2
    K.s=[dd+2];
    K.s=[K.s ones(1,mtotal-3)];
    pars.eps=1.e-3;
    A=[A; sparse([zeros(mtotal-3, 3) -eye(mtotal-3)])];
    C=[C;ones(mtotal-3,1)];
    [x,y,info] = sedumi(A,b,C,K,pars);
    Y_opt=mat(x(1:(dd+2)^2));
	X_opt=Y_opt(dd+1:dd+2,1:dd);
    Y_opt=Y_opt(1:dd,1:dd);
    % Plot the first round position estimations
    figure(1);
    hold on ;
    plot(PP(1,dd+1:n),PP(2,dd+1:n),'d');    
    grid on;    
	plot(X_opt(1,:),X_opt(2,:),'*r');

	plot(PP(1,1:dd),PP(2,1:dd),'og');
    
     for i=1:dd
        plot([X_opt(1,i)  PP(1,i)] , [X_opt(2,i)  PP(2,i)]);
    end;
    % Now find possibly violated lower bound constraints and add them into
    % SDP solver, till no constraint is violated:
    violation=0;
 while violation > 0;
    A=A(1:(dd+2)^2,:);
    C=C(1:(dd+2)^2);
    violation=0;
    solver=0;
    for i=1:dd
        for j=(i+1):n
            % j is an unknown points
            if j <= dd
                rr= Y_opt(i,i)+Y_opt(j,j)-2*Y_opt(i,j);
                if (d(i,j) == 0) & (rr < .9*Radius^2)
                    violation=violation+1;
                    rr=rand;
                    if rand < .98
                      d(i,j)=1;
                      solver=solver+1;
                      mtotal=mtotal+1;
                      a=sparse([zeros(1,i-1) 1 zeros(1,j-i-1) -1 zeros(1,dd-j+2)]');
                      A(:,mtotal)=vec(a*a');
                      b(mtotal)=Radius^2;
                      K.s=[K.s 1];
                    end;
                end;
            else
            % j is a known point
                rr= Y_opt(i,i)+norm(PP(:,j))^2-2*X_opt(:,i)'*PP(:,j);
                if  (d(i,j) == 0) & (rr  < .9*Radius^2)
                    violation=violation+1;
                    rr=rand;
                    if rand < .98
                      d(i,j)=1;
                      solver=solver+1;
                      mtotal=mtotal+1;
                      a=sparse([zeros(1,i-1) 1 zeros(1,dd-i) -PP(:,j)']');
                      A(:,mtotal)=vec(a*a');
                      b(mtotal)=Radius^2;
                      K.s=[K.s 1];
                    end;
                end;
            end;
        end;
    end;
    % Call solver again
    mtotal
    if solver > 0
        K.s=[dd+2];
        K.s=[K.s ones(1,mtotal-3)];
        A=[A; [zeros(mtotal-3,3) -eye(mtotal-3)]];
        C=[C;ones(mequal-3,1);zeros(mtotal-mequal,1)];
        [x,y,info] = sedumi(A,b,C,K,pars);
        Y_opt=mat(x(1:(dd+2)^2));
	    X_opt=Y_opt(dd+1:dd+2,1:dd);
        Y_opt=Y_opt(1:dd,1:dd);
    end;
 end;
    % Compute the trace solution and true error
    for j=1:dd
    tvar(j)=max(0,Y_opt(j,j)-(X_opt(:,j)'*X_opt(:,j)));
    error(j)=norm(X_opt(:,j)-PP(:,j));
    end;
    % Plot the position estimations
    figure(2);
    %hold on ;
    %plot(PP(1,dd+1:n),PP(2,dd+1:n),'d');    
    %grid on;    
	%plot(X_opt(1,:),X_opt(2,:),'*r');

	%plot(PP(1,1:dd),PP(2,1:dd),'og');

    %for i=1:dd
    %    plot([X_opt(1,i)  PP(1,i)] , [X_opt(2,i)  PP(2,i)]);
    %end;
    % Plot and compare the trace and true error
    %figure(3)
    hold on
    plot(error,'bd');
    plot(sqrt(tvar),'rs');
