This linear matrix inequality appears to be unsymmetric, a GNSS/UWB positioning

Nothing · June 13, 2023, 12:56pm

This linear matrix inequality appears to be unsymmetric. This is
very likely an error that will produce unexpected results. Please check
the LMI; and, if necessary, re-enter the model.

In cvxprob/newcnstr (line 241)
In >= (line 21)
In SDP_test (line 83)
I will add the symmetric constraint to the matrix T, this warning will not appear, but the problem will converge to the wrong solution.
My code:
clear
clc
N=3;
R=500;
sigma=0.05;
y=[236.263656299561;429.376073908061;78.1658890210092;486.649515964091;-298.504398103803;453.326301664114;188.620530929416;-378.640844732523;170.342142011970;-253.338060397655;-292.895616296679;-220.659136820585;-406.305444985628;-4.87938367947936;111.438277017325;138.391867525876;-261.575408585370;236.390686538070;381.888217572123;206.641958497465];
H=[0.328248918773633,-0.448510091724040,0.0500214306646390;0.527771588160849,-0.418259265791000,0.116092097579161;0.164229767884099,-0.474043287863163,0.0308085282833014;0.626944434861908,-0.651662835235340,0.286886974775612;-0.249732032403846,-0.397441598060094,0.0684563554501567;0.433717100187548,0.284550759315592,0.159597267496138;0.133921385913454,0.387822415921231,0.153007747240594;-0.418075382670027,0.0822323604587277,0.158411042421261;0.268649843349266,-0.509782425068563,0.0570271388243969;0.328248918773633,-0.448510091724040,0.0500214306646390;0.527771588160849,-0.418259265791000,0.116092097579161;0.164229767884099,-0.474043287863163,0.0308085282833014;0.626944434861908,-0.651662835235340,0.286886974775612;0.433717100187548,0.284550759315592,0.159597267496138;0.133921385913454,0.387822415921231,0.153007747240594;-0.418075382670027,0.0822323604587277,0.158411042421261;0.268649843349266,-0.509782425068563,0.0570271388243969;0.527771588160849,-0.418259265791000,0.116092097579161;0.433717100187548,0.284550759315592,0.159597267496138;0.133921385913454,0.387822415921231,0.153007747240594];
H_coop=[0.328248918773633,-0.448510091724040,0.0500214306646390,0,0,0,0,0,0;0.527771588160849,-0.418259265791000,0.116092097579161,0,0,0,0,0,0;0.164229767884099,-0.474043287863163,0.0308085282833014,0,0,0,0,0,0;0.626944434861908,-0.651662835235340,0.286886974775612,0,0,0,0,0,0;-0.249732032403846,-0.397441598060094,0.0684563554501567,0,0,0,0,0,0;0.433717100187548,0.284550759315592,0.159597267496138,0,0,0,0,0,0;0.133921385913454,0.387822415921231,0.153007747240594,0,0,0,0,0,0;-0.418075382670027,0.0822323604587277,0.158411042421261,0,0,0,0,0,0;0.268649843349266,-0.509782425068563,0.0570271388243969,0,0,0,0,0,0;0,0,0,0.328248918773633,-0.448510091724040,0.0500214306646390,0,0,0;0,0,0,0.527771588160849,-0.418259265791000,0.116092097579161,0,0,0;0,0,0,0.164229767884099,-0.474043287863163,0.0308085282833014,0,0,0;0,0,0,0.626944434861908,-0.651662835235340,0.286886974775612,0,0,0;0,0,0,0.433717100187548,0.284550759315592,0.159597267496138,0,0,0;0,0,0,0.133921385913454,0.387822415921231,0.153007747240594,0,0,0;0,0,0,-0.418075382670027,0.0822323604587277,0.158411042421261,0,0,0;0,0,0,0.268649843349266,-0.509782425068563,0.0570271388243969,0,0,0;0,0,0,0,0,0,0.527771588160849,-0.418259265791000,0.116092097579161;0,0,0,0,0,0,0.433717100187548,0.284550759315592,0.159597267496138;0,0,0,0,0,0,0.133921385913454,0.387822415921231,0.153007747240594];
cov=[0.250000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000;0.125000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000;0.125000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000;0.125000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000;0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000;0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000;0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000;0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000;0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000;0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000;0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000;0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000;0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000;0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000;0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000;0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000;0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.250000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000;0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.250000000000000,0.125000000000000,0.125000000000000;0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.250000000000000,0.125000000000000;0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.0625000000000000,0.0625000000000000,0.125000000000000,0.125000000000000,0.250000000000000];
y_r=[952.907615456327,456.289542000234,755.777917072613];
pos(1,1)=-930.5942;pos(1,2)=-150.6849;pos(1,3)=-7.6331;
pos(2,1)=267.0806;pos(2,2)=-366.9058;pos(2,3)=1.2707;
pos(3,1)=-681.3221;pos(3,2)=-294.1323;pos(3,3)=18.6139;
crlb(1).x_crlb=[0.426262666944371,0.0265513890408592,0.118743742140198;0.0265513890408592,0.472768835947331,-0.753530362401018;0.118743742140198,-0.753530362401018,4.33730428860477];
crlb(2).x_crlb=[0.515090198905004,-0.0304564489223878,-0.296245303317532;-0.0304564489223878,0.515011722035465,-0.586663298949717;-0.296245303317532,-0.586663298949717,4.76342754719610];
crlb(3).x_crlb=[1.99866523559141,-0.233290102146183,4.46415325671780;-0.233290102146183,1.05984197142332,-1.18580789286052;4.46415325671780,-1.18580789286052,17.6721837061105];
Q=struct;
for i=1:1:N
Q(i).q=crlb(i).x_crlb;
end
B=[-H,H_coop];
if sum(y_r<R)==0
X_coop=[];
Ds=[];
for i=1:1:N
X_coop=[X_coop;pos(i,:)‘];
Ds=mdiag(Ds,Q(i).q);
end
C=inv(cov+H_coopDsH_coop’);
X=inv(H’CH)H’C(y+H_coopX_coop);
else
cvx_clear
cvx_begin sdp
cvx_solver sdpt3%sedumi%
cvx_quiet(1)
cvx_precision best
variable X(3*(N+1),1)
variable r(1,N)
variable p(1,N)
variable E(3N,3N) semidefinite
variable T(3*(1+N),3*(1+N)) %semidefinite
variable Y(1+N+3,1+N+3) semidefinite
expression t(N)
expression GNSS

for i=1:1:N
    if y_r(i)>R
        t(i)=trace(inv(Q(i).q)*E(3*i-2:3*i,3*i-2:3*i))-2*pos(i,:)*inv(Q(i).q)*X((3*i+1):3*(i+1),1)...
            +pos(i,:)*inv(Q(i).q)*pos(i,:)';
    else
        t(i)=1/sigma^2*(y_r(i)^2+r(i)-2*y_r(i)*p(i))+trace(inv(Q(i).q)*E(3*i-2:3*i,3*i-2:3*i))...
            -2*pos(i,:)*inv(Q(i).q)*X((3*i+1):3*(i+1),1)...
            +pos(i,:)*inv(Q(i).q)*pos(i,:)';
    end
end
if ~isempty(cov)
    GNSS=y'*inv(cov)*y+y'*inv(cov)*B*X+X'*B'*inv(cov)*y+trace(B'*inv(cov)*B*T);
else
    GNSS=0;
end

minimize  ((sum(t)+GNSS))
subject to 
%% 
for j=1:1:N
    if y_r(j)>R
        r(1,j)-0==0;
        p(1,j)-0==0;
    else
        r(1,j)-(Y(1,1)+Y(j+1,j+1)-Y(1,j+1)-Y(j+1,1))==0;
        p(1,j)^2-r(1,j)<=0;
    end
    trace(E((3*j-2):3*j,(3*j-2):3*j))-Y(j+1,j+1)==0;
    [E(3*j-2:3*j,3*j-2:3*j) X(3*(j+1)-2:3*(j+1),1);X(3*(j+1)-2:3*(j+1),1)',1]>=zeros(4,4);
end
for k=1:1:N+1
    [Y(k,N+1+1),Y(k,N+1+2),Y(k,N+1+3)]'-X((3*k-2):3*k,1)==zeros(3,1);
end
[T,X;X',1]>=zeros(4+3*N,4+3*N);
for u=2:1:N+1
    T((3*u-2):3*u,(3*u-2):3*u)-E((3*(u-1)-2):3*(u-1),(3*(u-1)-2):3*(u-1))==zeros(3,3);
end
for m=1:1:N+1
    for n=1:1:N+1
        trace(T((3*m-2):3*m,(3*n-2):3*n))-Y(m,n)==0; 
    end  
end
Y>=zeros(N+1+3,N+1+3);
Y(N+1+1:end,N+1+1:end)-eye(3)==zeros(3,3);
cvx_end

end

Mark_L_Stone · June 13, 2023, 1:27pm

Show your program with T declared symmetric, i.e., variable T(3*(1+N),3*(1+N)) symmetric

Do not use quiet until everything is working correctly.
Do not use cvx_precision best.

Show all solver and CVX output and explain clearly how the solution is wrong.

Nothing · June 13, 2023, 1:36pm

The correct result is given by X，for example:
X=[9.1075;9.3099;2.0511;-930.0762;-150.9812;-6.5641;266.7157;-367.2827;0.9258;-681.2277;-292.5242;18.7703]
and the first three results of the column vector X are close to the true position (10, 10, 1)

Mark_L_Stone · June 13, 2023, 1:50pm

Please follow my instructions.