转发的 很好的 传感器网络定位 mallab

%function [stdevs, bound, F, avgNeighb] = calcLocalizationCRB(method, x, y, blinds, total, channelParam, measMade, d_thr) % %OUTPUTS: % stdevs: Bounds for the standard deviation of localization error (sqrt of % bound on x-variance + bound on y-variance). % bound: The matrix bound on covariance % F: The fisher information matrix % avgNeighb: Average number of neighbors per sensor. Useful if measMade % radius is given for neighbor inclusion matrix. % %INPUTS: % method: Method must be 'T' (TOA), 'A' (AOA), 'R' (RSS), or 'Q' (QRSS). % x and y: Actual coordinate vectors. The first 'blinds' elements are % blindfolded, and the remaining are 'reference' or 'anchor' % nodes. x and y must be row vectors. % blinds: Total # of blindfolded devices. The first 'blinds' devices must % be correspond to the blindfolded devices, the rest are references. % total: Total # of devices. % channelParam: Equal to sigmadB/n_p for the case of RSS/QRSS measurements, the dB % standard deviation of fading divided by the path loss exponent. % Or, for the case of TOA measurements , equal to sigma_d, % ie., the std. dev. of distance measurement error (meters). For the case of % AOA channelParam = sigma_a, the std. dev. of angle measurement % error (radians). The value of channelParam can be scalar if the channel % parameter is the same for all links, or matrix (total by % total) if each link has a different channel parameter % measMade: Measurement matrix. measMade(i,j)=1 if i and j make a % measurement, or =0 if not. Default is all ones. If a % scalar is sent for measMade, it is considered to be a % radius: if ||z_i-z_j|| < radius, then i and j makes measurements, % otherwise they don't. % d_thr: Distance thresholds used in QRSS. Not needed for other % methods. The i'th value indicates the distance at which the mean % received power seperates the i'th RSS quantization level and % the (i+1)'th quantization level. Note, length(d_thr) = #levels - 1. % Also, the d_thr values must be a row vector, and in order (either % decreasing or increasing). % % AUTHOR: Neal Patwari, npatwari (at) umich (dot) edu, April 2004 % http://www.engin.umich.edu/~npatwari/ % AOA part added by Josh Ash, ashj (at) ece (dot) osu (dot) edu, 7/28/04 % % REF: "It Takes a Network: Cooperative Geolocation of Wireless Sensors" % (N. Patwari, J. Ash, S. Kyperountas, A. O. Hero, R. M. Moses, N. S. Correal), % accepted to IEEE Signal Processing Magazine, expected publication, July 2005. % % EXAMPLE: >> n=20; x=rand(1,n); y=rand(1,n); % >> [s, B, F, a] = calcCRBGivenLocsAny('T', x, y, 15, n, 0.2, 0.75); % >> calcCRBGivenLocsAny('R', x, y, 15, n, 2.1); % >> calcCRBGivenLocsAny('Q', x, y, 15, n, 2.1, ones(n), 0.8:-0.2:0.2) % function [stdevs, bound, F, avgNeighb] = calcLocalizationCRB(... method, x, y, blinds, total, channelParam, measMade, d_thr) method = upper(method); if (method ~='T') & (method ~='R') & (method ~= 'Q' & (method ~= 'A')), error('Method must be T (TOA), A (AOA), R (RSS), or Q (QRSS)'); end % 1. For incomplete measurements, use symmetric matrix measMade to indicate which pairs % made measurements (1 for a measurement, 0 for no measurement). The default % is that all pairs made measurements. % Or, if measMade is just a scalar distance, consider it to be a range, all % devices within this range radius 'make measurements' and those outside of % the range don't. if ~exist('measMade'), measMade = ones(total); end if length(measMade) == 1, % Of course total > 1 rangeRadiusSqr = measMade^2; measMade = zeros(total); else rangeRadiusSqr = -1; % Key for don't use range radius end % 2. For the case of identical channel variation on every link, % accept a scalar value for 'channelParam'. if length(channelParam) == 1, channelParam = ones(total).*channelParam; end % Otherwise, use a channel parameter matrix (only the lower triangle is % used) if (method == 'T') | (method == 'A'), sigmaConst = channelParam; % standard deviation of measured distance or angle error elseif (method == 'R') | (method == 'Q'), sigmaConst = channelParam*(log(10)/10); % 1/sqrt(b), where 'b' is the constant in [1] end if method == 'Q', K = length(d_thr)+1; % The number of quantization levels % Make sure the d_thr is in decreasing order if d_thr(end) > d_thr(1), d_thr = fliplr(d_thr); end end % 3. Calculate the non-diagonal elements of each of the four sub-blocks of F. % Each matrix is a superset of the elements needed in F11, F12, and F22, % the additional terms are needed for the next step. for k = 2:total, el = 1:(k-1); deltax = x(k) - x(el); deltay = y(k) - y(el); dSqr = deltax.^2 + deltay.^2; if method == 'T', denom(k,el) = dSqr; else % for either AOA, RSS or QRSS denom(k,el) = dSqr.^2; end if rangeRadiusSqr > 0, measMade(k,el) = (dSqr <= rangeRadiusSqr); end frontTerm(k,el) = measMade(k,el) ./ (sigmaConst(k,el).^2); if method == 'Q', % There is an additional multiplicative term for QRSS d = sqrt(dSqr); for j=el; sqrtbLogTerms = log(d(j)./d_thr)./sigmaConst(k,j); PhiTerms = [0, N(sqrtbLogTerms), 1]; ExpTerms = [0, exp(-0.5.*sqrtbLogTerms.^2), 0]; % If d(j) is too low, numerical limitations cause div-by-zero. % But if d(j) is too low, devices are very very close, and % location information possible from QRSS is essentially zero. if min(abs(PhiTerms(2:K+1)-PhiTerms(1:K))) < eps, nzInd = find(abs(PhiTerms(2:K+1)-PhiTerms(1:K)) > eps); frontTerm(k,j) = frontTerm(k,j) * sum((ExpTerms(1+nzInd)-ExpTerms(nzInd)).^2 ./ ... (PhiTerms(1+nzInd)-PhiTerms(nzInd))) / (2*pi); else frontTerm(k,j)= frontTerm(k,j) * sum((ExpTerms(2:K+1)-ExpTerms(1:K)).^2 ./ ... (PhiTerms(2:K+1)-PhiTerms(1:K))) / (2*pi); end end end if method == 'A' term12(k,el) = frontTerm(k,el) .* (-deltay).*(deltax) ./ denom(k,el); term12(el,k) = term12(k,el)'; term11(k,el) = frontTerm(k,el) .* deltay.*deltay ./ denom(k,el); term11(el,k) = term11(k,el)'; term22(k,el) = frontTerm(k,el) .* deltax.*deltax ./ denom(k,el); term22(el,k) = term22(k,el)'; else term12(k,el) = frontTerm(k,el) .* deltax.*deltay ./ denom(k,el); term12(el,k) = term12(k,el)'; term11(k,el) = frontTerm(k,el) .* deltax.*deltax ./ denom(k,el); term11(el,k) = term11(k,el)'; term22(k,el) = frontTerm(k,el) .* deltay.*deltay ./ denom(k,el); term22(el,k) = term22(k,el)'; end end % 4. Calculate the diagonal elements, which are sums of the elements on each column. v_xx = sum(term11(:, 1:blinds)); v_xy = sum(term12(:, 1:blinds)); v_yy = sum(term22(:, 1:blinds)); % 5. Combine the two, using only the terms for the blind devices. F11 = -term11(1:blinds, 1:blinds) + diag(v_xx); F12 = -term12(1:blinds, 1:blinds) + diag(v_xy); F22 = -term22(1:blinds, 1:blinds) + diag(v_yy); F = [F11, F12; F12', F22]; bound = inv(F); % 6. The location estimate stdev bound: sqrt( var(x) + var(y) ). stdevs = sqrt(diag(bound(1:blinds,1:blinds)) + diag(bound(1+blinds:2*blinds,1+blinds:2*blinds)))'; % 7. Avg number of neighbors: note that only the lower triangle is % calculated when using a radius, and self-measurement isn't allowed,