gusucode.com > Beam Alignment and Tracking for Autonomous Vehicular Communication using IEEE 802.11ad-based Radar > mmWave-V2I-Radar-master/v2i_update_location.m
% This function computes the location of cars based on the real and % estimated velocities. The real location is updated for every car. The % estimated location is only updated for those cars that were detected upon % the function call. % Input parameters: % - loc: (Vector-double) The real location of the cars. % - locEst: (Vector-double) The estimated location of the cars. % - vEst: (Vector-cell) The estimated velocities. % - tEst: (Vecto-cell) The time at which the detection happened. % - carsID: (Vector-double) The ID of every car in the network % - tInt: (Vector-cell) The intervals at which cars experience a % reduction in their velocity due to collision. % - vInt: (Vector-cell) The velocities for every car. % - s_radar: (Integer) Number of slots the radar operation takes % - tSym: (Double) Elapsed time in the simulation % - TSLOT: (Double) Window of time in the simulation. Fixed. % Output parameters: % - loc: (Vector-double) The real location of the cars (updated). % - locSys: (Vector-double) The estimated location of the cars (updated). function [loc,locSys] = v2i_update_location(loc,locSys,vEst,tEst,carsID,tInt,vInt,s_radar,tSym,TSLOT,kmul) % Update actual car location (known velocities) for cID = carsID idxInt = find(tInt{cID} <= (kmul*tSym), 1, 'last'); loc(cID) = loc(cID) + vInt{cID}(idxInt)*(kmul*TSLOT); end % Update car location (estimation using vEst and tEst) tEst represents % the moment at which the car was detected vEst represents the vector % of velocities carIDToUpdt = find(~cellfun(@isempty,tEst)); % cars to be updated for cID = (carIDToUpdt).' % We take into account the group of measures that happened % consecutivelly to improve the accuracy. That is, the measures % that happened within (s_radar*TSLOT) intervals. Otherwise, the % system interprets the measure correspond to a period before % experiencing a colision. Thus, it shall not be taken into % account. idxSet = detectMySet(tEst{cID},s_radar*TSLOT); % Improved measure by taking the mean of the last measurement block vEst_improved = mean(vEst{cID}(idxSet)); % Elapsed time from last measure elapsed = tSym - tEst{cID}(end); % Estimated new location locSys(cID) = locSys(cID) + (vEst_improved*elapsed); end end function [idxSet,mySet] = detectMySet(inVect,tWindow) % Required number of consecutive numbers following a first one. In % other words, blocks of size (N+1). % We need to round it up to the 5th decimal to account for unaccuracies % in the resolutions. A bug in Matlab, can't deal with it. Keep out. x = (round(diff(inVect),5)==tWindow); if length(inVect) > 1 && ~isempty(find(x==1, 1)) % Vector has more than one measure f = find([false,x]~=[x,false]); if f(end)==length(inVect) % There is a block with the last measure idxSet = [f(end-1) f(end)]; else % Vector has no block containing the last measure idxSet = length(inVect); end else % Vector has only one measure idxSet = 1; end mySet = inVect(idxSet); end