├── _config.yml ├── Semidefinite Programming for NLOS Error Mitigation in TDOA Localization.pdf ├── gauss_samples.m ├── LICENSE ├── README.md ├── sdp_ce.m ├── test.m ├── SDP.m └── SDP_CL_SU.m /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-slate -------------------------------------------------------------------------------- /Semidefinite Programming for NLOS Error Mitigation in TDOA Localization.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xmuszq/Semidefinite-Programming-SDP-optimization/HEAD/Semidefinite Programming for NLOS Error Mitigation in TDOA Localization.pdf -------------------------------------------------------------------------------- /gauss_samples.m: -------------------------------------------------------------------------------- 1 | function s = gauss_samples(x,P,n) 2 | %function s = gauss_samples(x,P,N) 3 | % 4 | % INPUTS: 5 | % x, P - mean vector and covariance matrix 6 | % N - number of samples 7 | % 8 | % OUTPUT: 9 | % s - set of N samples 10 | % 11 | % Produce N random sample from multivariate Gaussian distribution. 12 | % Renamed from multivariate_gauss.m. 13 | % 14 | % Tim Bailey 2005. 15 | 16 | len= length(x); 17 | S= chol(P)'; 18 | X = randn(len,n); 19 | s = S*X + x*ones(1,n); 20 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 zhenqiang 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Semidefinite-Programming-SDP-optimization 2 | Codes for paper "Semidefinite Programming for NLOS Error Mitigation in TDOA Localization". 3 | ** For those who don't have access to the paper, the PDF file was also uploaded in this repo with name 'Semidefinite Programming for NLOS Error Mitigation in TDOA Localization.pdf'. 4 | 5 | Run the test.m file to simulate the algorithms including SDP, SDP-robust and the proposed SDP. 6 | 7 | The simulation settings here are a bit different from the original paper in two ways (but the algorithm is the same): a. here 8 | the NLOS distributions are randomly generated (how many links and which links are in NLOS); b. The source nodes, whoes position to be 9 | found, are randomly generated. In the paper, NLOS distributions are emulated and the source positions are evenly distributed but fixed. 10 | 11 | There are several hyper-parameters need to be tuned/optimized to obtain the best performance. However, in the implementation, the hyper-parameters 12 | are only manually selected and therefore they are not optimized. 13 | 14 | Simulation with 1000 MC runs: 15 | https://user-images.githubusercontent.com/15931069/45726434-41970800-bb8d-11e8-8f4e-0b0a52acc1f0.png 16 | Note: (the y-label for graphs in the paper and this project should be 'absolute distance error' not MSE) 17 | 18 | 19 | *If you want to do performance comparison between your algorithm and the one in this paper, please use the file 'SDP_CL_SU.m'; 20 | 21 | ** The algorithm 'sdp_ce.m' is provided by Prof. Gang Wang, from Ningbo University. Please cite his paper if you use this algorithm; 22 | 23 | Contact me at xiamenhai[at]gmail.com if you have questions. 24 | -------------------------------------------------------------------------------- /sdp_ce.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % the source code implementation of the paper: "Robust convex 3 | % approximation methods for TDOA-based localization under NLOS conditions" 4 | % 5 | % Author: Gang Wang, 6 | % Ningbo University 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | function [y1]=sdp_ce(s,d,b,N,p1,a_) 10 | 11 | c_=[]; 12 | C=[]; 13 | c_conj=[]; 14 | C_conj=[]; 15 | k=[]; 16 | k_conj=[]; 17 | seita=[]; 18 | seita_conj=[]; 19 | v=[]; 20 | for i=1:N-1%1:N %SDP参量 v 21 | v(:,i)=[zeros(1,i+1),1,zeros(1,N-i-1)].'; 22 | end 23 | for i=1:N-1%1:N 24 | c(:,i)=a_(:,i)+2*p1*v(:,i); 25 | C(:,:,i)=c(:,i)*c(:,i).'; 26 | c_conj(:,i)=a_(:,i)-2*p1*v(:,i); 27 | C_conj(:,:,i)=c_conj(:,i)*c_conj(:,i).'; 28 | end 29 | 30 | for i=1:N-1%1:N 31 | k(i)=p1^2+2*d(i)*p1+b(i); 32 | k_conj(i)=p1^2+2*d(i)*p1-b(i); 33 | seita(i)=p1^2-2*d(i)*p1+b(i); 34 | seita_conj(i)=p1^2-2*d(i)*p1-b(i); 35 | end 36 | for i=1:N-1%1:N 37 | D(:,:,i)=diag([zeros(1,i-1),-1,zeros(1,N-i-1)],0); 38 | 39 | Q1(:,:,i)=blkdiag(eye(2),D(:,:,i)); 40 | 41 | end 42 | 43 | 44 | cvx_clear 45 | cvx_begin sdp 46 | cvx_solver sdpt3% sdpt3 47 | cvx_quiet(1) 48 | cvx_precision best %输出结果可能因此报错 49 | 50 | 51 | variable tao(N-1,1)%tao(N-1,1) 52 | variable y1(1+N,1) 53 | variable Y(1+N,1+N) symmetric 54 | 55 | minimize (sum(tao)) 56 | 57 | subject to 58 | 59 | 60 | for i=1:N-1%1:N 61 | trace(C_conj(:,:,i)*Y)+2*k_conj(i)*c_conj(:,i).'*y1+k_conj(i)^2<=tao(i); 62 | 63 | trace(C(:,:,i)*Y)+2*seita_conj(i)*c(:,i).'*y1+seita_conj(i)^2<=tao(i); 64 | 65 | trace(C(:,:,i)*Y)-2*k(i)*c(:,i).'*y1+k(i)^2<=tao(i); 66 | 67 | trace(C_conj(:,:,i)*Y)-2*seita(i)*c_conj(:,i).'*y1+seita(i)^2<=tao(i); 68 | 69 | end 70 | 71 | for j=1:N-1%1:N 72 | trace(Q1(:,:,j)*Y)-2*s(:,j+1)'*[y1(1);y1(2)]+norm(s(:,j+1))^2==0; 73 | 74 | norm([y1(1);y1(2)]-s(:,j+1))<=y1(2+j); 75 | 76 | end 77 | 78 | [Y y1;y1.' 1]>=zeros(N+2,N+2); 79 | 80 | cvx_end 81 | end 82 | -------------------------------------------------------------------------------- /test.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear all 3 | close all 4 | tic 5 | 6 | 7 | MC=10; % how many runs 8 | p=10; % maximal NLOS value 9 | N=8; % number of anchors 10 | t0=0.1; 11 | 12 | c=1; 13 | 14 | %sigma=0.3; 15 | %x_(1)=0; 16 | %y_(1)=0; 17 | %s=[]; 18 | %s(:,1)=[x_(1);y_(1)];% 19 | 20 | s(:,1)=[-20;-20];%anchors 21 | s(:,2)=[-20;20]; 22 | s(:,3)=[20;-20]; 23 | s(:,4)=[20;20]; 24 | s(:,5)=[-20;0]; 25 | s(:,6)=[0;-20]; 26 | s(:,7)=[0;20]; 27 | s(:,8)=[20;0]; 28 | 29 | 30 | cond=[1,5,6,8] %number of NLOS 31 | % loop for nlos for 1~5 and 6~8 respectively 32 | for ci=1:2 33 | if ci==1 34 | nrange=[1,5] 35 | else 36 | nrange=[6,8] 37 | end 38 | 39 | 40 | K=0.1:0.1:1; 41 | for m=1:10 42 | sigma=0.1*m; 43 | %Q=0.5*sigma^2*(eye(N-1)+ones(N-1,N-1));% 44 | Q=0.5*sigma^2*(eye(N)+ones(N,N));% 45 | parfor j=1:MC 46 | j; 47 | 48 | NLOS_number = randi([6,8],1,1); %randsrc(1,1,randperm(5));% 49 | %n=gauss_samples(zeros(N-1,1),Q,1); % get the noise 50 | n=gauss_samples(zeros(N,1),Q,1); % get the noise 51 | 52 | x=unifrnd(-25,25); 53 | y=unifrnd(-25,25); 54 | X=[x;y]; 55 | 56 | w = []; 57 | for i=1:N% get the nlos bias for 8 links 58 | w(i)=unifrnd(0,p); 59 | end 60 | 61 | g_index=randperm(8,NLOS_number); % assign nlos bias to certain links 62 | g = zeros(N,1); 63 | g(g_index) = 1; 64 | 65 | 66 | %%%%%%%%%%%% SDP wang 2016 %%%%%%%%%%%%%% 67 | e=[]; 68 | d=[]; 69 | b=[]; 70 | for i=1:N-1%1:N; 71 | e(i,1)=g(i+1)*w(i+1)-g(1)*w(1);%w(i+1)-w(1) 72 | % e_(i-1,1)=w_(i)-w_(1); 73 | d(i)=norm(X-s(:,i+1))-norm(X-s(:,1))+n(i)+e(i); 74 | %d(i)=norm(X-s(:,i))-norm(X-s0)+n(i)+e(i); 75 | b(i)=-d(i)^2-norm(s(:,i+1))^2+norm(s(:,1))^2; 76 | %b(i-1)=-d(i-1)^2-norm(s(:,i))^2+norm(s0)^2; 77 | end 78 | 79 | a_=[]; 80 | for i=1:N-1%2:N 81 | %a_(:,i-1)=[2*(s(:,1)-s(:,i))',zeros(1,i-1),-2*d(i-1),zeros(1,N-i)].'; 82 | a_(:,i)=[2*(s(:,1)-s(:,i+1))',zeros(1,i-1),-2*d(i),zeros(1,N-i-1)].'; 83 | end 84 | y0=sdp_ce(s,d,b,N,p,a_); 85 | X1=[y0(1);y0(2)]; 86 | t00(j, 1)=norm(X1-X); 87 | 88 | 89 | 90 | %%%%%%%%%%%% SDP CL chen %%%%%%%%%%%%%%%%% 91 | %% NOTE: remove this part when you run test.m 92 | %% , as SDP_CL_chen.m is only 93 | %% used for my own testing. 94 | e = []; 95 | d = []; 96 | t1= []; 97 | for i=2:N %generate TDOA with NLosֵ 98 | e(i-1,1)=g(i)*w(i)-g(1)*w(1);%w(i)-w(1); 99 | % e_(i-1,1)=w_(i)-w_(1); 100 | d(i-1)=norm(X-s(:,i))-norm(X-s(:,1))+n(i-1)+e(i-1); 101 | % t1(i)=d(i-1)/(3*10^8)+t1(1); 102 | end 103 | %t2=t1*10^8; 104 | %t1(1)=norm(X-s(:,1))/c+t0; 105 | t1(1)=norm(X-s(:,1))/c+t0;% ???????it's better not to add e(1) n(1) 106 | %t1(1)=abs(min(d))+15; 107 | %t1(1)=0.1; 108 | 109 | t1(2:N)=d/c+t1(1); 110 | t2=t1; 111 | % t2 112 | y1=SDP_CL_chen(c,s,N,t2); 113 | t11(j,1)=norm(y1-X); 114 | 115 | 116 | 117 | %%%%%%%%%%%%%%% SDP CL SU %%%%%%%%%%%%%%%%%% 118 | % generate TOA 119 | toa=zeros(N, 1); 120 | for i=1:N 121 | toa(i)= norm(X-s(:,i))+g(i)*w(i)+n(i); 122 | end 123 | Cp=2.99792458 ; 124 | y2=SDP_CL_SU(toa, s, t0, Cp); 125 | t22(j,1)=norm(y2-X); 126 | 127 | 128 | 129 | %%%%%%%%%%%%%%% SDP %%%%%%%%%%%%%%%%%% 130 | c_speed=1%2.99792458; %m/s 131 | y3=SDP(toa, s, c_speed) 132 | t33(j,1)=norm(y3-X); 133 | 134 | 135 | 136 | end 137 | T0(m)=sum(t00)/MC; 138 | T1(m)=sum(t11)/MC; 139 | T2(m)=sum(t22)/MC; 140 | T3(m)=sum(t33)/MC; 141 | 142 | end 143 | 144 | 145 | 146 | 147 | if ci==1 148 | save res_me-chen_nlos1to5 T1 149 | save res_me-su_nlos1to5 T2 150 | save res_wang_nlos1to5 T0 151 | save res_liu_nlos1to5 T3 152 | 153 | figure(1) 154 | plot(K, T0, 'b-') 155 | hold on; 156 | plot(K, T1,'g-') 157 | plot(K, T2, 'r-') 158 | plot(K, T3, 'c-') 159 | title('nlos1 1 - 5') 160 | xlabel('sigma'); 161 | ylabel('RMSE'); 162 | legend('SDP-robust wang', 'Proposed-code from Chen', 'Proposed', 'SDP - Wang') 163 | 164 | else 165 | save res_me-chen_nlos6to8 T1 166 | save res_me-su_nlos6to8 T2 167 | save res_wang_nlos6to8 T0 168 | save res_liu_nlos6to8 T3 169 | 170 | figure(2) 171 | plot(K, T0, 'b-') 172 | hold on; 173 | plot(K,T1,'g-') 174 | plot(K, T2, 'r-') 175 | plot(K, T3, 'c-') 176 | 177 | title('nlos1 6 -8') 178 | xlabel('sigma'); 179 | ylabel('RMSE'); 180 | legend('SDP-robust wang', 'Proposed-code from Chen', 'Proposed - code from Su', 'SDP - Wang') 181 | 182 | end 183 | grid on 184 | end 185 | toc 186 | -------------------------------------------------------------------------------- /SDP.m: -------------------------------------------------------------------------------- 1 | function [x]=SDP(d, anchor_posi, c_speed) 2 | % Implementation of paper "Efficient convex relaxation methods for robust 3 | % target localization by a sensor network using time difference of 4 | % arrivals, K. Yang, G. Wang, and Z. Luo, 2009 5 | 6 | 7 | % coded by Zhenqinag Su 8 | 9 | %% 10 | d1_observe = d(1); 11 | d2_observe = d(2); 12 | d3_observe = d(3); 13 | d4_observe = d(4); 14 | d5_observe = d(5); 15 | d6_observe = d(6); 16 | d7_observe = d(7); 17 | d8_observe = d(8); 18 | 19 | 20 | 21 | c=c_speed;%c=299792458;%m/s 22 | tao12=d1_observe-d2_observe; 23 | tao13=d1_observe-d3_observe; 24 | tao14=d1_observe-d4_observe; 25 | tao15=d1_observe-d5_observe; 26 | tao16=d1_observe-d6_observe; 27 | tao17=d1_observe-d7_observe; 28 | tao18=d1_observe-d8_observe; 29 | tao23=d2_observe-d3_observe; 30 | tao24=d2_observe-d4_observe; 31 | tao25=d2_observe-d5_observe; 32 | tao26=d2_observe-d6_observe; 33 | tao27=d2_observe-d7_observe; 34 | tao28=d2_observe-d8_observe; 35 | tao34=d3_observe-d4_observe; 36 | tao35=d3_observe-d5_observe; 37 | tao36=d3_observe-d6_observe; 38 | tao37=d3_observe-d7_observe; 39 | tao38=d3_observe-d8_observe; 40 | tao45=d4_observe-d5_observe; 41 | tao46=d4_observe-d6_observe; 42 | tao47=d4_observe-d7_observe; 43 | tao48=d4_observe-d8_observe; 44 | tao56=d5_observe-d6_observe; 45 | tao57=d5_observe-d7_observe; 46 | tao58=d5_observe-d8_observe; 47 | tao67=d6_observe-d7_observe; 48 | tao68=d6_observe-d8_observe; 49 | tao78=d7_observe-d8_observe; 50 | 51 | tao_wave=[tao12 tao13 tao14 tao15 tao16 tao17 tao18 tao23 tao24 tao25 tao26 tao27 tao28 tao34 tao35 tao36 tao37 tao38 tao45 tao46 tao47 tao48 tao56 tao57 tao58 tao67 tao68 tao78]'; 52 | tao_wave=tao_wave/c;%transfer to time delay. 53 | tao_line=-tao_wave; 54 | G=[1 -1 0 0 0 0 0 0 55 | 1 0 -1 0 0 0 0 0 56 | 1 0 0 -1 0 0 0 0 57 | 1 0 0 0 -1 0 0 0 58 | 1 0 0 0 0 -1 0 0 59 | 1 0 0 0 0 0 -1 0 60 | 1 0 0 0 0 0 0 -1 61 | 0 1 -1 0 0 0 0 0 62 | 0 1 0 -1 0 0 0 0 63 | 0 1 0 0 -1 0 0 0 64 | 0 1 0 0 0 -1 0 0 65 | 0 1 0 0 0 0 -1 0 66 | 0 1 0 0 0 0 0 -1 67 | 0 0 1 -1 0 0 0 0 68 | 0 0 1 0 -1 0 0 0 69 | 0 0 1 0 0 -1 0 0 70 | 0 0 1 0 0 0 -1 0 71 | 0 0 1 0 0 0 0 -1 72 | 0 0 0 1 -1 0 0 0 73 | 0 0 0 1 0 -1 0 0 74 | 0 0 0 1 0 0 -1 0 75 | 0 0 0 1 0 0 0 -1 76 | 0 0 0 0 1 -1 0 0 77 | 0 0 0 0 1 0 -1 0 78 | 0 0 0 0 1 0 0 -1 79 | 0 0 0 0 0 1 -1 0 80 | 0 0 0 0 0 1 0 -1 81 | 0 0 0 0 0 0 1 -1 82 | ]; 83 | F=[2*G'*G G'*(tao_line-tao_wave);(tao_line-tao_wave)'*G tao_line'*tao_line+tao_wave'*tao_wave]; 84 | delta=0.000001; 85 | 86 | %%%%%%%%%%%%%%%%%%%%%%%%%% cvx %%%%%%%%%%%%%%%%%%%%%%%%%%% 87 | 88 | cvx_begin sdp quiet 89 | %cvx_solver sedumi 90 | variable x(2,1) 91 | variable t(8,1) 92 | variable T(8,8) 93 | variable z 94 | 95 | nn=[T, t;t', 1]; 96 | 97 | minimize trace(nn*F) + delta*(sum(sum(T))) 98 | 99 | 100 | subject to 101 | 102 | T(1,1)==c^(-2)*[anchor_posi(1,1) anchor_posi(2,1) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,1) anchor_posi(2,1) -1]'); 103 | T(2,2)==c^(-2)*[anchor_posi(1,2) anchor_posi(2,2) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,2) anchor_posi(2,2) -1]'); 104 | T(3,3)==c^(-2)*[anchor_posi(1,3) anchor_posi(2,3) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,3) anchor_posi(2,3) -1]'); 105 | T(4,4)==c^(-2)*[anchor_posi(1,4) anchor_posi(2,4) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,4) anchor_posi(2,4) -1]'); 106 | T(5,5)==c^(-2)*[anchor_posi(1,5) anchor_posi(2,5) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,5) anchor_posi(2,5) -1]'); 107 | T(6,6)==c^(-2)*[anchor_posi(1,6) anchor_posi(2,6) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,6) anchor_posi(2,6) -1]'); 108 | T(7,7)==c^(-2)*[anchor_posi(1,7) anchor_posi(2,7) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,7) anchor_posi(2,7) -1]'); 109 | T(8,8)==c^(-2)*[anchor_posi(1,8) anchor_posi(2,8) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,8) anchor_posi(2,8) -1]'); 110 | 111 | T(1,2)>=c^(-2)*abs([anchor_posi(1,1) anchor_posi(2,1) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,2) anchor_posi(2,2) -1]')); 112 | T(1,3)>=c^(-2)*abs([anchor_posi(1,1) anchor_posi(2,1) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,3) anchor_posi(2,3) -1]')); 113 | T(1,4)>=c^(-2)*abs([anchor_posi(1,1) anchor_posi(2,1) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,4) anchor_posi(2,4) -1]')); 114 | T(1,5)>=c^(-2)*abs([anchor_posi(1,1) anchor_posi(2,1) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,5) anchor_posi(2,5) -1]')); 115 | T(1,6)>=c^(-2)*abs([anchor_posi(1,1) anchor_posi(2,1) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,6) anchor_posi(2,6) -1]')); 116 | T(1,7)>=c^(-2)*abs([anchor_posi(1,1) anchor_posi(2,1) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,7) anchor_posi(2,7) -1]')); 117 | T(1,8)>=c^(-2)*abs([anchor_posi(1,1) anchor_posi(2,1) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,8) anchor_posi(2,8) -1]')); 118 | 119 | T(2,3)>=c^(-2)*abs([anchor_posi(1,2) anchor_posi(2,2) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,3) anchor_posi(2,3) -1]')); 120 | T(2,4)>=c^(-2)*abs([anchor_posi(1,2) anchor_posi(2,2) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,4) anchor_posi(2,4) -1]')); 121 | T(2,5)>=c^(-2)*abs([anchor_posi(1,2) anchor_posi(2,2) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,5) anchor_posi(2,5) -1]')); 122 | T(2,6)>=c^(-2)*abs([anchor_posi(1,2) anchor_posi(2,2) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,6) anchor_posi(2,6) -1]')); 123 | T(2,7)>=c^(-2)*abs([anchor_posi(1,2) anchor_posi(2,2) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,7) anchor_posi(2,7) -1]')); 124 | T(2,8)>=c^(-2)*abs([anchor_posi(1,2) anchor_posi(2,2) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,8) anchor_posi(2,8) -1]')); 125 | 126 | T(3,4)>=c^(-2)*abs([anchor_posi(1,3) anchor_posi(2,3) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,4) anchor_posi(2,4) -1]')); 127 | T(3,5)>=c^(-2)*abs([anchor_posi(1,3) anchor_posi(2,3) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,5) anchor_posi(2,5) -1]')); 128 | T(3,6)>=c^(-2)*abs([anchor_posi(1,3) anchor_posi(2,3) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,6) anchor_posi(2,6) -1]')); 129 | T(3,7)>=c^(-2)*abs([anchor_posi(1,3) anchor_posi(2,3) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,7) anchor_posi(2,7) -1]')); 130 | T(3,8)>=c^(-2)*abs([anchor_posi(1,3) anchor_posi(2,3) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,8) anchor_posi(2,8) -1]')); 131 | 132 | T(4,5)>=c^(-2)*abs([anchor_posi(1,4) anchor_posi(2,4) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,5) anchor_posi(2,5) -1]')); 133 | T(4,6)>=c^(-2)*abs([anchor_posi(1,4) anchor_posi(2,4) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,6) anchor_posi(2,6) -1]')); 134 | T(4,7)>=c^(-2)*abs([anchor_posi(1,4) anchor_posi(2,4) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,7) anchor_posi(2,7) -1]')); 135 | T(4,8)>=c^(-2)*abs([anchor_posi(1,4) anchor_posi(2,4) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,8) anchor_posi(2,8) -1]')); 136 | 137 | T(5,6)>=c^(-2)*abs([anchor_posi(1,5) anchor_posi(2,5) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,6) anchor_posi(2,6) -1]')); 138 | T(5,7)>=c^(-2)*abs([anchor_posi(1,5) anchor_posi(2,5) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,7) anchor_posi(2,7) -1]')); 139 | T(5,8)>=c^(-2)*abs([anchor_posi(1,5) anchor_posi(2,5) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,8) anchor_posi(2,8) -1]')); 140 | 141 | T(6,7)>=c^(-2)*abs([anchor_posi(1,6) anchor_posi(2,6) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,7) anchor_posi(2,7) -1]')); 142 | T(6,8)>=c^(-2)*abs([anchor_posi(1,6) anchor_posi(2,6) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,8) anchor_posi(2,8) -1]')); 143 | T(7,8)>=c^(-2)*abs([anchor_posi(1,7) anchor_posi(2,7) -1]*[1 0 x(1);0 1 x(2);x(1) x(2) z]*([anchor_posi(1,8) anchor_posi(2,8) -1]')); 144 | 145 | 146 | [T t;t' 1]>=0; 147 | [1 0 x(1);0 1 x(2);x(1) x(2) z]>=0; 148 | 149 | cvx_end; 150 | %%%%%%%%%%%%%%%%%%%%%%%%%% end cvx %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 151 | -------------------------------------------------------------------------------- /SDP_CL_SU.m: -------------------------------------------------------------------------------- 1 | function [x]=SDP_CL_SU_1(d, anchor_posi, dt, Cp) 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % d - the real TOA including the error and NLOS bias 4 | % dt - a small positive value to compensate the timing 5 | % anchor_posi - the anchor positions (hard-coded for 8 anchors) 6 | % x - the estimated position 7 | % Cp - the speed of light and is default to 2.99792 8 | 9 | % Implementation of the paper:"Semidefinite Programming for NLOS Error 10 | % Mitigation in TDOA Localization" 11 | % Author: Zhenqiang Su 12 | % Email: xiamenhai@gmail.com 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | 15 | %speed of light 16 | Cp=Cp;%2.99792; 17 | %general parameter for algorithms 18 | v=[1 1 1 1 1 1 1 1]; 19 | u=[.2 .2 .2 .2 .2 .2 .2 .2];% u>2*variance 20 | u=u*1.5; 21 | umuso=0.00; umuso_z=0.00; extra=0.0; 22 | 23 | d1_observe = d(1); 24 | d2_observe = d(2); 25 | d3_observe = d(3); 26 | d4_observe = d(4); 27 | d5_observe = d(5); 28 | d6_observe = d(6); 29 | d7_observe = d(7); 30 | d8_observe = d(8); 31 | 32 | % TOA distance to timing 33 | t1= d1_observe/Cp; 34 | t2= d2_observe/Cp; 35 | t3= d3_observe/Cp; 36 | t4= d4_observe/Cp; 37 | t5= d5_observe/Cp; 38 | t6= d6_observe/Cp; 39 | t7= d7_observe/Cp; 40 | t8= d8_observe/Cp; 41 | tmin=min([t1,t2,t3,t4,t5,t6,t7,t8]); 42 | 43 | % remove the TOA information and add a constant positive dt 44 | % after this operation only TDOA information is reserved 45 | t1= t1-tmin+dt; 46 | t2= t2-tmin+dt; 47 | t3= t3-tmin+dt; 48 | t4= t4-tmin+dt; 49 | t5= t5-tmin+dt; 50 | t6= t6-tmin+dt; 51 | t7= t7-tmin+dt; 52 | t8= t8-tmin+dt; 53 | 54 | index=find([t1,t2,t3,t4,t5,t6,t7,t8]==tmin); 55 | alf_sdp_rev=5; alf=0.01;%0.01 is better than 0.1 and better than 0.02 56 | %%%%%%%%%%%%%%%%%%%%%%%% CpVX %%%%%%%%%%%%%%%%%%%%%%%%%%%% 57 | cvx_begin sdp quiet 58 | %cvx_solver sedumi 59 | variable x(2,1) 60 | variable c(8,1) 61 | variable z 62 | variable h(8,1) 63 | variable t0 64 | variable t0_s 65 | variable f 66 | 67 | minimize v(1)*(Cp^2*t0_s-2*Cp^2*t1*t0+Cp^2*t1^2-h(1)-c(1))^2+alf*c(1)^2... 68 | +v(2)*(Cp^2*t0_s-2*Cp^2*t2*t0+Cp^2*t2^2-h(2)-c(2))^2+alf*c(2)^2... 69 | +v(3)*(Cp^2*t0_s-2*Cp^2*t3*t0+Cp^2*t3^2-h(3)-c(3))^2+alf*c(3)^2... 70 | +v(4)*(Cp^2*t0_s-2*Cp^2*t4*t0+Cp^2*t4^2-h(4)-c(4))^2+alf*c(4)^2... 71 | +v(5)*(Cp^2*t0_s-2*Cp^2*t5*t0+Cp^2*t5^2-h(5)-c(5))^2+alf*c(5)^2... 72 | +v(6)*(Cp^2*t0_s-2*Cp^2*t6*t0+Cp^2*t6^2-h(6)-c(6))^2+alf*c(6)^2... 73 | +v(7)*(Cp^2*t0_s-2*Cp^2*t7*t0+Cp^2*t7^2-h(7)-c(7))^2+alf*c(7)^2... 74 | +v(8)*(Cp^2*t0_s-2*Cp^2*t8*t0+Cp^2*t8^2-h(8)-c(8))^2+alf*c(8)^2... 75 | +alf*(t0_s*8*8)^2 +f^2; 76 | 77 | subject to 78 | 79 | h(1)==[anchor_posi(1,1) anchor_posi(2,1) -1]*[ 1 0 x(1);0 1 x(2);x(1) x(2) z]*[anchor_posi(1,1); anchor_posi(2,1);-1]; 80 | h(2)==[anchor_posi(1,2) anchor_posi(2,2) -1]*[ 1 0 x(1);0 1 x(2);x(1) x(2) z]*[anchor_posi(1,2); anchor_posi(2,2);-1]; 81 | h(3)==[anchor_posi(1,3) anchor_posi(2,3) -1]*[ 1 0 x(1);0 1 x(2);x(1) x(2) z]*[anchor_posi(1,3); anchor_posi(2,3);-1]; 82 | h(4)==[anchor_posi(1,4) anchor_posi(2,4) -1]*[ 1 0 x(1);0 1 x(2);x(1) x(2) z]*[anchor_posi(1,4); anchor_posi(2,4);-1]; 83 | h(5)==[anchor_posi(1,5) anchor_posi(2,5) -1]*[ 1 0 x(1);0 1 x(2);x(1) x(2) z]*[anchor_posi(1,5); anchor_posi(2,5);-1]; 84 | h(6)==[anchor_posi(1,6) anchor_posi(2,6) -1]*[ 1 0 x(1);0 1 x(2);x(1) x(2) z]*[anchor_posi(1,6); anchor_posi(2,6);-1]; 85 | h(7)==[anchor_posi(1,7) anchor_posi(2,7) -1]*[ 1 0 x(1);0 1 x(2);x(1) x(2) z]*[anchor_posi(1,7); anchor_posi(2,7);-1]; 86 | h(8)==[anchor_posi(1,8) anchor_posi(2,8) -1]*[ 1 0 x(1);0 1 x(2);x(1) x(2) z]*[anchor_posi(1,8); anchor_posi(2,8);-1]; 87 | 88 | [1 0 anchor_posi(1,1)-x(1);0 1 anchor_posi(2,1)-x(2);anchor_posi(1,1)-x(1) anchor_posi(2,1)-x(2) Cp^2*t0_s-2*Cp^2*t1*t0+Cp^2*t1^2+f]>=0; 89 | [1 0 anchor_posi(1,2)-x(1);0 1 anchor_posi(2,2)-x(2);anchor_posi(1,2)-x(1) anchor_posi(2,2)-x(2) Cp^2*t0_s-2*Cp^2*t2*t0+Cp^2*t2^2+f]>=0; 90 | [1 0 anchor_posi(1,3)-x(1);0 1 anchor_posi(2,3)-x(2);anchor_posi(1,3)-x(1) anchor_posi(2,3)-x(2) Cp^2*t0_s-2*Cp^2*t3*t0+Cp^2*t3^2+f]>=0; 91 | [1 0 anchor_posi(1,4)-x(1);0 1 anchor_posi(2,4)-x(2);anchor_posi(1,4)-x(1) anchor_posi(2,4)-x(2) Cp^2*t0_s-2*Cp^2*t4*t0+Cp^2*t4^2+f]>=0; 92 | [1 0 anchor_posi(1,5)-x(1);0 1 anchor_posi(2,5)-x(2);anchor_posi(1,5)-x(1) anchor_posi(2,5)-x(2) Cp^2*t0_s-2*Cp^2*t5*t0+Cp^2*t5^2+f]>=0; 93 | [1 0 anchor_posi(1,6)-x(1);0 1 anchor_posi(2,6)-x(2);anchor_posi(1,6)-x(1) anchor_posi(2,6)-x(2) Cp^2*t0_s-2*Cp^2*t6*t0+Cp^2*t6^2+f]>=0; 94 | [1 0 anchor_posi(1,7)-x(1);0 1 anchor_posi(2,7)-x(2);anchor_posi(1,7)-x(1) anchor_posi(2,7)-x(2) Cp^2*t0_s-2*Cp^2*t7*t0+Cp^2*t7^2+f]>=0; 95 | [1 0 anchor_posi(1,8)-x(1);0 1 anchor_posi(2,8)-x(2);anchor_posi(1,8)-x(1) anchor_posi(2,8)-x(2) Cp^2*t0_s-2*Cp^2*t8*t0+Cp^2*t8^2+f]>=0; 96 | f>=0; 97 | [1 0 x(1);0 1 x(2);x(1) x(2) z]>=0; 98 | 99 | % c(1)>=0; %weak constraint 100 | % c(2)>=0; 101 | % c(3)>=0; 102 | % c(4)>=0; 103 | % c(5)>=0; 104 | % c(6)>=0; 105 | % c(7)>=0; 106 | % c(8)>=0; 107 | 108 | 109 | t1>=t0;t2>=t0;t3>=t0;t4>=t0;t5>=t0;t6>=t0;t7>=t0;t8>=t0;%really important constrain 110 | [t0_s t0;t0 1]>=0; % 111 | 112 | 113 | (t2-t0)*Cp+(t1-t0)*Cp>=norm(anchor_posi(:,2)-anchor_posi(:,1)); 114 | (t3-t0)*Cp+(t1-t0)*Cp>=norm(anchor_posi(:,3)-anchor_posi(:,1)); 115 | (t4-t0)*Cp+(t1-t0)*Cp>=norm(anchor_posi(:,4)-anchor_posi(:,1)); 116 | (t5-t0)*Cp+(t1-t0)*Cp>=norm(anchor_posi(:,5)-anchor_posi(:,1)); 117 | (t6-t0)*Cp+(t1-t0)*Cp>=norm(anchor_posi(:,6)-anchor_posi(:,1)); 118 | (t7-t0)*Cp+(t1-t0)*Cp>=norm(anchor_posi(:,7)-anchor_posi(:,1)); 119 | (t8-t0)*Cp+(t1-t0)*Cp>=norm(anchor_posi(:,8)-anchor_posi(:,1)); 120 | 121 | (t1-t0)*Cp+(t2-t0)*Cp>=norm(anchor_posi(:,1)-anchor_posi(:,2)); 122 | (t3-t0)*Cp+(t2-t0)*Cp>=norm(anchor_posi(:,3)-anchor_posi(:,2)); 123 | (t4-t0)*Cp+(t2-t0)*Cp>=norm(anchor_posi(:,4)-anchor_posi(:,2)); 124 | (t5-t0)*Cp+(t2-t0)*Cp>=norm(anchor_posi(:,5)-anchor_posi(:,2)); 125 | (t6-t0)*Cp+(t2-t0)*Cp>=norm(anchor_posi(:,6)-anchor_posi(:,2)); 126 | (t7-t0)*Cp+(t2-t0)*Cp>=norm(anchor_posi(:,7)-anchor_posi(:,2)); 127 | (t8-t0)*Cp+(t2-t0)*Cp>=norm(anchor_posi(:,8)-anchor_posi(:,2)); 128 | 129 | (t2-t0)*Cp+(t3-t0)*Cp>=norm(anchor_posi(:,2)-anchor_posi(:,3)); 130 | (t1-t0)*Cp+(t3-t0)*Cp>=norm(anchor_posi(:,1)-anchor_posi(:,3)); 131 | (t4-t0)*Cp+(t3-t0)*Cp>=norm(anchor_posi(:,4)-anchor_posi(:,3)); 132 | (t5-t0)*Cp+(t3-t0)*Cp>=norm(anchor_posi(:,5)-anchor_posi(:,3)); 133 | (t6-t0)*Cp+(t3-t0)*Cp>=norm(anchor_posi(:,6)-anchor_posi(:,3)); 134 | (t7-t0)*Cp+(t3-t0)*Cp>=norm(anchor_posi(:,7)-anchor_posi(:,3)); 135 | (t8-t0)*Cp+(t3-t0)*Cp>=norm(anchor_posi(:,8)-anchor_posi(:,3)); 136 | 137 | (t2-t0)*Cp+(t4-t0)*Cp>=norm(anchor_posi(:,2)-anchor_posi(:,4)); 138 | (t3-t0)*Cp+(t4-t0)*Cp>=norm(anchor_posi(:,3)-anchor_posi(:,4)); 139 | (t1-t0)*Cp+(t4-t0)*Cp>=norm(anchor_posi(:,1)-anchor_posi(:,4)); 140 | (t5-t0)*Cp+(t4-t0)*Cp>=norm(anchor_posi(:,5)-anchor_posi(:,4)); 141 | (t6-t0)*Cp+(t4-t0)*Cp>=norm(anchor_posi(:,6)-anchor_posi(:,4)); 142 | (t7-t0)*Cp+(t4-t0)*Cp>=norm(anchor_posi(:,7)-anchor_posi(:,4)); 143 | (t8-t0)*Cp+(t4-t0)*Cp>=norm(anchor_posi(:,8)-anchor_posi(:,4)); 144 | 145 | (t2-t0)*Cp+(t5-t0)*Cp>=norm(anchor_posi(:,2)-anchor_posi(:,5)); 146 | (t3-t0)*Cp+(t5-t0)*Cp>=norm(anchor_posi(:,3)-anchor_posi(:,5)); 147 | (t4-t0)*Cp+(t5-t0)*Cp>=norm(anchor_posi(:,4)-anchor_posi(:,5)); 148 | (t1-t0)*Cp+(t5-t0)*Cp>=norm(anchor_posi(:,1)-anchor_posi(:,5)); 149 | (t6-t0)*Cp+(t5-t0)*Cp>=norm(anchor_posi(:,6)-anchor_posi(:,5)); 150 | (t7-t0)*Cp+(t5-t0)*Cp>=norm(anchor_posi(:,7)-anchor_posi(:,5)); 151 | (t8-t0)*Cp+(t5-t0)*Cp>=norm(anchor_posi(:,8)-anchor_posi(:,5)); 152 | 153 | (t2-t0)*Cp+(t6-t0)*Cp>=norm(anchor_posi(:,2)-anchor_posi(:,6)); 154 | (t3-t0)*Cp+(t6-t0)*Cp>=norm(anchor_posi(:,3)-anchor_posi(:,6)); 155 | (t4-t0)*Cp+(t6-t0)*Cp>=norm(anchor_posi(:,4)-anchor_posi(:,6)); 156 | (t5-t0)*Cp+(t6-t0)*Cp>=norm(anchor_posi(:,5)-anchor_posi(:,6)); 157 | (t1-t0)*Cp+(t6-t0)*Cp>=norm(anchor_posi(:,1)-anchor_posi(:,6)); 158 | (t7-t0)*Cp+(t6-t0)*Cp>=norm(anchor_posi(:,7)-anchor_posi(:,6)); 159 | (t8-t0)*Cp+(t6-t0)*Cp>=norm(anchor_posi(:,8)-anchor_posi(:,6)); 160 | 161 | (t2-t0)*Cp+(t7-t0)*Cp>=norm(anchor_posi(:,2)-anchor_posi(:,7)); 162 | (t3-t0)*Cp+(t7-t0)*Cp>=norm(anchor_posi(:,3)-anchor_posi(:,7)); 163 | (t4-t0)*Cp+(t7-t0)*Cp>=norm(anchor_posi(:,4)-anchor_posi(:,7)); 164 | (t5-t0)*Cp+(t7-t0)*Cp>=norm(anchor_posi(:,5)-anchor_posi(:,7)); 165 | (t6-t0)*Cp+(t7-t0)*Cp>=norm(anchor_posi(:,6)-anchor_posi(:,7)); 166 | (t1-t0)*Cp+(t7-t0)*Cp>=norm(anchor_posi(:,1)-anchor_posi(:,7)); 167 | (t8-t0)*Cp+(t7-t0)*Cp>=norm(anchor_posi(:,8)-anchor_posi(:,7)); 168 | 169 | (t2-t0)*Cp+(t8-t0)*Cp>=norm(anchor_posi(:,2)-anchor_posi(:,8)); 170 | (t3-t0)*Cp+(t8-t0)*Cp>=norm(anchor_posi(:,3)-anchor_posi(:,8)); 171 | (t4-t0)*Cp+(t8-t0)*Cp>=norm(anchor_posi(:,4)-anchor_posi(:,8)); 172 | (t5-t0)*Cp+(t8-t0)*Cp>=norm(anchor_posi(:,5)-anchor_posi(:,8)); 173 | (t6-t0)*Cp+(t8-t0)*Cp>=norm(anchor_posi(:,6)-anchor_posi(:,8)); 174 | (t7-t0)*Cp+(t8-t0)*Cp>=norm(anchor_posi(:,7)-anchor_posi(:,8)); 175 | (t1-t0)*Cp+(t8-t0)*Cp>=norm(anchor_posi(:,1)-anchor_posi(:,8)); 176 | 177 | cvx_end; 178 | %%%%%%%%%%%%%%%%%%%%%%%%%% end cvx %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% --------------------------------------------------------------------------------