Bluetooth LE Positioning by Using Direction Finding

numDimensions = 2;       % Dimension of Bluetooth LE devices position in a network
numLocators = 3;         % Number of locators

EbNo = 6:2:16;                % Eb/No in dB
numIterations = 200;       % Number of iterations to average the position error

dfMethod = "AoA";            % Direction finding method
dfPacketType = "ConnectionCTE";        % Direction finding packet type
phyMode = "LE1M";             % PHY transmission mode, must be LE1M or LE2M (for ConnectionCTE) and LE1M (for ConnectionlessCTE)

arraySize = 16;           % Antenna array size, must be a scalar (represents ULA) or a row vector (represents URA) for 2D or 3D positioning, respectively
elementSpacing = 0.5;      % Normalized spacing between the antenna elements with respect to wavelength
switchingPattern = 1:prod(arraySize);    % Antenna switching pattern, must be a 1xM row vector and M must be in the range [2, 74/slotDuration+1]

slotDuration = 2;        % Slot duration in microseconds
cteLength = 160;     % Length of CTE in microseconds, must be in the range [16, 160] with 8 microseconds step size
sps = 8;                 % Samples per symbol
channelIndex = 17;   % Channel index
crcInit = '555551';             % CRC initialization
accessAddress = '01234567';       % Access address
payloadLength = 1;       % Payload length in bytes

minLocators = 2; % Minimum number of locators
if numDimensions == 2 && size(arraySize,2) ~= 1
    error('The arraySize must be a scalar for 2-D position estimation');
end
if numDimensions == 3 && size(arraySize,2) ~= 2
    error('The arraySize must be a 1-by-2 vector for 3-D position estimation');
end
if numLocators < minLocators
    error(['The numLocators must be greater than or equal to ' num2str(minLocators)]);
end
if strcmp(dfPacketType,'ConnectionCTE') && payloadLength ~= 1
    error('The payloadLength must be 1 byte for direction finding packet type of ConnectionCTE');
elseif strcmp(dfPacketType,'ConnectionlessCTE') && payloadLength < 3
    error('The payloadLength must be greater than or equal to 3 bytes for direction finding packet type of ConnectionlessCTE');
end

if numDimensions == 3 && isscalar(elementSpacing)
    elementSpacing = [elementSpacing elementSpacing]; % Element spacing must be a vector to perform 3D positioning
end
cfg = bleAngleEstimateConfig('ArraySize',arraySize,'SlotDuration',slotDuration,'SwitchingPattern', ...
                                    switchingPattern,'ElementSpacing',elementSpacing);
validateConfig(cfg);
if strcmp(dfMethod,'AoA')
    cteType = [0;0];
else
    cteType = [0;1];
    if slotDuration == 1
        cteType = [1;0];
    end
end

% Convert access address in hexadecimal to binary
accessAddBits = int2bit(hex2dec(accessAddress),32,false);

% Initialize the variables to be used outside the parfor loop
numEbNo = numel(EbNo);                                % Number of Eb/No points
posNode = zeros(numDimensions,numEbNo);
posLocator = zeros(numDimensions,numLocators,numEbNo);
angleEst = zeros(numLocators,numDimensions-1,numEbNo);
posNodeEst = zeros(numDimensions,numEbNo);
validResult = zeros(1,numEbNo);
avgPositionError = zeros(1,numEbNo);

% parfor iEbNo = 1:numEbNo % Use 'parfor' to speed up the simulation
for iEbNo = 1:numEbNo % Use 'for' to debug the simulation
    
    % Set the random substream index to ensure that each iteration uses a
    % repeatable set of random numbers
    stream = RandStream('combRecursive','Seed',12345);
    stream.Substream = iEbNo;
    RandStream.setGlobalStream(stream);
    
    % Loop over the number of iterations to average the positioning error.
    % If the successful links between the locators and node are less than
    % the minimum number of locators required to perform triangulation, then
    % the iteration fails.
    posErr = zeros(1,numIterations);
    iterationFailCount = 0;
    for iterCount = 1:numIterations
        
        % For each iteration, generate random positions for the locators
        [tempPosNode,tempPosLocator,ang] = helperBLEGeneratePositions(numLocators,numDimensions);
        posNode(:,iEbNo) = tempPosNode;
        posLocator(:,:,iEbNo) = tempPosLocator;

        % Loop over the number of locators
        tempAngleEst = zeros(numLocators,numDimensions-1);
        idx = [];
        linkFailFlag = zeros(numLocators,1);
        for i=1:numLocators
            
            % Generate direction finding packet
            data = helperBLEGenerateDFPDU(dfPacketType,cteLength,cteType,payloadLength,crcInit);
            
            % Generate Bluetooth LE waveform
            bleWaveform = bleWaveformGenerator(data,'Mode',phyMode,'SamplesPerSymbol',sps,...
                'ChannelIndex',channelIndex,'DFPacketType',dfPacketType,'AccessAddress',accessAddBits);
            
            % Perform steering and switching between the antennas
            dfWaveform = helperBLESteerSwitchAntenna(bleWaveform,ang(i,:),...
                                        phyMode,sps,dfPacketType,payloadLength,cfg);

            % Pass the waveform through AWGN channel
            snr = EbNo(iEbNo) - 10*log10(sps); % Signal to noise ratio (SNR)
            noiseWaveform = awgn(dfWaveform,snr,'measured');

            % Pass the noisy waveform to bleIdealReceiver which returns
            % the IQ samples
            [~, ~, iqSamples] = bleIdealReceiver(noiseWaveform,'Mode',phyMode,...
                'SamplesPerSymbol',sps,'ChannelIndex',channelIndex,'DFPacketType',...
                dfPacketType,'SlotDuration',slotDuration);
            
            % Estimate the angle(s) using the IQ samples. A packet is
            % detected successfully when the minimum number of non-zero IQ
            % samples are present.
            refSampleLength = 8; % Reference samples length
            % IQ samples must contain at least eight samples from reference period and
            % one sample from each antenna
            minIQSamples = refSampleLength+getNumElements(cfg)-1;
            if length(nonzeros(iqSamples)) >= minIQSamples % If packet detection is successful
                tempAngleEst(i,:) = bleAngleEstimate(iqSamples,cfg);
            else
                linkFailFlag(i) = 1; % If packet detection fails, enable the link fail flag
                idx = [idx i]; %#ok<AGROW> % Store the indices corresponding to the locator-node communication fail links
            end
        end
        
        % If the successful node-locator links are greater than the minimum
        % number of locators and all the locator angles are not same, then
        % estimate the node position using triangulation and compute the
        % position error
        if (numLocators-nnz(linkFailFlag)) >= minLocators && ~isequal(tempAngleEst(:,1),repmat(tempAngleEst(1,1),numLocators,1))
            % If any link fails, then assign NaN to the corresponding angle
            % estimates
            posLocatorEbNo = posLocator(:,:,iEbNo);
            tempAngEstTri = tempAngleEst;
            if any(linkFailFlag == 1)
                posLocatorEbNo(:,idx) = [];
                tempAngleEst(idx,:) = NaN(numel(idx),numDimensions-1);
                tempAngEstTri(idx,:) = [];
            end
            
            % Estimate the node position using triangulation
            posNodeEst(:,iEbNo) = blePositionEstimate(posLocatorEbNo,'angulation',tempAngEstTri.');
            angleEst(:,:,iEbNo) = tempAngleEst;
            
            % Compute the position error
            posErr(iterCount) = sqrt(sum((posNodeEst(:,iEbNo)-posNode(:,iEbNo)).^2));
        else
            iterationFailCount = iterationFailCount + 1; % Count the number of failed links used to average the position error
        end
    end
    
    if(iterationFailCount == numIterations) % If all the links fail at a given Eb/No value
        disp(['At Eb/No = ',num2str(EbNo(iEbNo)),' dB, all direction finding packet transmissions failed'])
        validResult(iEbNo) = 0; % Disable plot flag for failed links
    else
        avgPositionError(iEbNo) = sum(posErr)/(numIterations-iterationFailCount);
        disp(['At Eb/No = ',num2str(EbNo(iEbNo)),' dB, positioning error in meters = ', num2str(avgPositionError(iEbNo))]) 
        validResult(iEbNo) = 1; % Enable plot flag for successful links
    end
end

[~,validIdx] = find(validResult==1);
if ~isempty(validIdx)
    EbNoValid = EbNo(validIdx);
    [~,EbNoIdx] = max(EbNoValid);
    EbNoValidIdx = validIdx(EbNoIdx);
    helperBLEVisualizePosition(posLocator(:,:,EbNoValidIdx),posNode(:,EbNoValidIdx),...
                        angleEst(:,:,EbNoValidIdx),posNodeEst(:,EbNoValidIdx));
    figure
    plot(EbNoValid,avgPositionError(validIdx),'-b*','LineWidth',2, ...
            'MarkerEdgeColor','b','MarkerSize',10)
    grid on
    xlabel('Eb/No (dB)')
    ylabel('Estimated Position Error (meters)')
    title('Position Accuracy in Bluetooth LE network')
end