diff --git a/基于点云的人体定位demo/RAfftMatrix.m b/基于点云的人体定位demo/RAfftMatrix.m new file mode 100644 index 0000000..4a96d65 --- /dev/null +++ b/基于点云的人体定位demo/RAfftMatrix.m @@ -0,0 +1,19 @@ +%% 2DFFT得到距离-角度热力图 + +function fft2dData = RAfftMatrix(rawData) + + rawData = squeeze(rawData); + [angleBin,rangeBin] = size(rawData); + + angleWin = hanning(angleBin); + angleWin2D = repmat(angleWin,1,rangeBin); + + rangeWin = hanning(rangeBin)'; + rangeWin2D = repmat(rangeWin,angleBin,1); + + rawDataWin = rawData .* angleWin2D; + fft1dData = fftshift(fft(rawDataWin,angleBin,1)); + + fft1dDataWin = fft1dData .* rangeWin2D; + fft2dData = fft(fft1dDataWin,rangeBin,2); +end diff --git a/基于点云的人体定位demo/RDfftMatrix.m b/基于点云的人体定位demo/RDfftMatrix.m new file mode 100644 index 0000000..d4853db --- /dev/null +++ b/基于点云的人体定位demo/RDfftMatrix.m @@ -0,0 +1,14 @@ +%% 2DFFT得到距离-速度热力图 + +function fft2dData = RDfftMatrix(rawData) + rawData = squeeze(rawData); + [rangeBin,dopplerBin] = size(rawData); + rangeWin = hanning(rangeBin); + rangeWin2D = repmat(rangeWin,1,dopplerBin); + dopplerWin = hanning(dopplerBin)'; + dopplerWin2D = repmat(dopplerWin,rangeBin,1); + rawDataWin = rawData .* rangeWin2D; + fft1dData = fft(rawDataWin,rangeBin,1); + fft1dDataWin = fft1dData .* dopplerWin2D; + fft2dData = fftshift(fft(fft1dDataWin,dopplerBin,2),2); +end diff --git a/基于点云的人体定位demo/cfar.m b/基于点云的人体定位demo/cfar.m new file mode 100644 index 0000000..c1b1de7 --- /dev/null +++ b/基于点云的人体定位demo/cfar.m @@ -0,0 +1,67 @@ +%% 请了解：什么是CFAR，以及什么是2D-CFAR + +function [pointList,cfarRD] = cfar(parameter,accumulateRD) + + [rangeLen,dopplerLen] = size(accumulateRD); + %% doppler维度搜索 + dopplerMethod = parameter.dopplerMethod; + dopplerSNR = parameter.dopplerSNR; + dopplerWinGuardLen = parameter.dopplerWinGuardLen; + dopplerWinTrainLen = parameter.dopplerWinTrainLen; + + dopplerLeft = accumulateRD(:,dopplerLen - dopplerWinGuardLen - dopplerWinTrainLen + 1:dopplerLen); + dopplerRight = accumulateRD(:,1:dopplerWinGuardLen+dopplerWinTrainLen); + dopplercfar = [dopplerLeft accumulateRD dopplerRight]; + dopplerCfarList = []; + for rangeIdx = 1:rangeLen + for dopplerIdx = 1:dopplerLen + dopplerCfarIdx = dopplerIdx + dopplerWinGuardLen + dopplerWinTrainLen; + leftCell = dopplercfar(rangeIdx,dopplerIdx:dopplerIdx+dopplerWinTrainLen-1); + rightCell = dopplercfar(rangeIdx,dopplerCfarIdx+dopplerWinGuardLen:dopplerCfarIdx+dopplerWinGuardLen+dopplerWinTrainLen-1); + leftNoise = mean(leftCell); + rightNoise = mean(rightCell); + noise = (leftNoise + rightNoise) / 2; + indexdb = dopplercfar(rangeIdx,dopplerCfarIdx); + targetSnr = indexdb - noise; + if targetSnr > dopplerSNR + dopplerCfarList = [dopplerCfarList dopplerIdx]; + cfarRDdoppler(rangeIdx,dopplerIdx) = indexdb; + end + end + end + dopplerCfarList = unique(dopplerCfarList); + %% range维度搜索 + rangeMethod = parameter.rangeMethod; + rangeSNR = parameter.rangeSNR; + rangeWinGuardLen = parameter.rangeWinGuardLen; + rangeWinTrainLen = parameter.rangeWinTrainLen; + + rangeUp = accumulateRD(rangeLen - rangeWinGuardLen - rangeWinTrainLen + 1:rangeLen,:); + rangeDown = accumulateRD(1:rangeWinGuardLen+rangeWinTrainLen,:); + rangecfar = [rangeUp;accumulateRD;rangeDown]; + rangeCfarList = []; + cfarRD = zeros(rangeLen,dopplerLen); + for dopplerIdx = dopplerCfarList + for rangeIdx = 1:rangeLen + + rangeCfarIdx = rangeIdx + rangeWinGuardLen + rangeWinTrainLen; + upCell = rangecfar(rangeIdx:rangeIdx+rangeWinTrainLen-1,dopplerIdx); + downCell = rangecfar(rangeCfarIdx+rangeWinGuardLen:rangeCfarIdx+rangeWinGuardLen+rangeWinTrainLen-1,dopplerIdx); + upNoise = mean(upCell); + downNoise = mean(downCell); + if rangeMethod == 1 + noise = (upNoise + downNoise) / 2; + indexdb = rangecfar(rangeCfarIdx,dopplerIdx); + targetSnr = indexdb - noise; + if targetSnr > rangeSNR + rangeCfarList = [rangeCfarList [rangeIdx;dopplerIdx]]; + cfarRD(rangeIdx,dopplerIdx) = indexdb; + end + elseif dopplerMethod == 2 + elseif dopplerMethod == 3 + else + end + end + end + pointList = rangeCfarList; +end diff --git a/基于点云的人体定位demo/chan_Accumulate.m b/基于点云的人体定位demo/chan_Accumulate.m new file mode 100644 index 0000000..c81babf --- /dev/null +++ b/基于点云的人体定位demo/chan_Accumulate.m @@ -0,0 +1,10 @@ +%% 通道间的非相干累计 + +function accumulateRD = chan_Accumulate(fft2dDataDB) + [channelNum,rangeBin,dopplerBin] = size(fft2dDataDB); + accumulateRD = zeros(rangeBin,dopplerBin); + + for channelId = 1:channelNum + accumulateRD = accumulateRD + (abs(squeeze(fft2dDataDB(channelId,:,:)))); + end +end diff --git a/基于点云的人体定位demo/doa.m b/基于点云的人体定位demo/doa.m new file mode 100644 index 0000000..821e0a8 --- /dev/null +++ b/基于点云的人体定位demo/doa.m @@ -0,0 +1,83 @@ +%% 三种角度估计的方法 + +function [angle,doa_abs] = doa(parameter,antVec) + + doaMethod = parameter.doaMethod; + if doaMethod == 1 + [angle,doa_abs] = dbfMethod(parameter,antVec); + elseif doaMethod == 2 + [angle,doa_abs] = fftMethod(parameter,antVec); + elseif doaMethod == 3 + [angle,doa_abs] = caponMethod(parameter,antVec); + else + end +end + +function [angle,doa_abs] = dbfMethod(parameter,antVec) + + txAntenna = parameter.txAntenna; + rxAntenna = parameter.rxAntenna; + virtualAntenna = parameter.virtualAntenna; + lambda = parameter.lambda; + txNum = length(txAntenna); + rxNum = length(rxAntenna); + dx = parameter.dx; + deg = -90:0.1:90; + weightVec = zeros(virtualAntenna,1); + doa_dbf = zeros(length(deg),1); + kk = 1; + for degscan = deg + for txId = 1:txNum + for rxId = 1:rxNum + dphi = ((txId-1) * rxNum + rxId - 1) * 2 * pi / lambda * dx * sind(degscan); + weightVec((txId-1) * rxNum + rxId) = exp(-1i * dphi); + end + end + doa_dbf(kk) = antVec'*weightVec; + kk = kk + 1; + end + doa_abs = abs(doa_dbf); + [pk,loc]=max(doa_abs); + angle = deg(loc); +end + +function [angle,doa_abs] = fftMethod(parameter,antVec) + angleIndex = asin((-512:1:512-1)/512) * 180 / pi; + doa_fft=fftshift(fft(antVec,1024)); + doa_abs=abs(doa_fft); + [pk,loc]=max(doa_abs); + angle = angleIndex(loc); +end + +function [angle,doa_abs] = caponMethod(parameter,antVec) + + txAntenna = parameter.txAntenna; + rxAntenna = parameter.rxAntenna; + txNum = length(txAntenna); + rxNum = length(rxAntenna); + lambda = parameter.lambda; + dx = parameter.dx; + virtualAntenna = parameter.virtualAntenna; + Rx = antVec * antVec' / virtualAntenna; + deg = -90:0.1:90; + a = zeros(virtualAntenna,1); +% a = zeros(1,virtualAntenna); + kk = 1; + for degscan = deg + for txId = 1:txNum + for rxId = 1:rxNum + virtualAntennaId = (txId-1) * rxNum + rxId - 1; + dphi = 2 * pi / lambda * dx * virtualAntennaId * sind(degscan); + a((txId-1) * rxNum + rxId) = exp(-1i * dphi); + end + end + RxInv = inv(Rx); + P_out(kk) = 1/(a'*RxInv*a); + kk = kk + 1; + end + doa_abs = abs(P_out); + [pk,loc]=max(doa_abs); + angle = deg(loc); + figure; + plot(deg,20*log10(abs(P_out))); +end diff --git a/基于点云的人体定位demo/generateCfarParameter.m b/基于点云的人体定位demo/generateCfarParameter.m new file mode 100644 index 0000000..03848ec --- /dev/null +++ b/基于点云的人体定位demo/generateCfarParameter.m @@ -0,0 +1,14 @@ +%% 生成CFAR所需的参数 + +function parameter = generateCfarParameter() + + parameter.dopplerMethod = 1; %1：ca-cfar 2：so-cfar 3：go-cfar + parameter.dopplerSNR = 10; + parameter.dopplerWinGuardLen = 2; + parameter.dopplerWinTrainLen = 8; + + parameter.rangeMethod = 1; %1：ca-cfar 2：so-cfar 3：go-cfar + parameter.rangeSNR = 10; + parameter.rangeWinGuardLen = 2; + parameter.rangeWinTrainLen = 4; +end diff --git a/基于点云的人体定位demo/generateParameter.m b/基于点云的人体定位demo/generateParameter.m new file mode 100644 index 0000000..63f5b90 --- /dev/null +++ b/基于点云的人体定位demo/generateParameter.m @@ -0,0 +1,36 @@ +% 参数设置 +function parameter = generateParameter() + + parameter.c = 3e8; %光速 + parameter.stratFreq = 76.5e9; %起始频率 + + + parameter.Tr = 10e-6; %扫频时间 也就是周期 + parameter.Samples = 256; %采样点 + parameter.Fs = 25.6e6; %采样率 + + parameter.rangeBin = parameter.Samples ; %rangebin + parameter.Chirps = 512; %chirp数 + parameter.dopplerBin = parameter.Chirps; %dopplerbin + + parameter.Slope = 30e12; %chirp斜率 + parameter.Bandwidth = parameter.Slope * parameter.Tr ; %发射信号有效带宽 + parameter.BandwidthValid = parameter.Samples/parameter.Fs*parameter.Slope; %发射信号带宽 + parameter.centerFreq = parameter.stratFreq + parameter.Bandwidth / 2; %中心频率 + parameter.lambda = parameter.c / parameter.centerFreq; %波长 + + parameter.txAntenna = ones(1,3); %发射天线个数 + parameter.rxAntenna = ones(1,4); %接收天线个数 + parameter.txNum = length(parameter.txAntenna); + parameter.rxNum = length(parameter.rxAntenna); + parameter.virtualAntenna = length(parameter.txAntenna) * length(parameter.rxAntenna); + + parameter.dz = parameter.lambda / 2; %接收天线俯仰间距 + parameter.dx = parameter.lambda / 2; %接收天线水平间距 + parameter.doaMethod = 2; %测角方法选择 1-dbf 2-fft 3-capon + parameter.target = [ + 100 -20 0; %target1 range speed angle + 0 10 -30; %target2 range speed angle + 0 20 30; %target2 range speed angle + ]; +end diff --git a/基于点云的人体定位demo/generateSignal.m b/基于点云的人体定位demo/generateSignal.m new file mode 100644 index 0000000..7518677 --- /dev/null +++ b/基于点云的人体定位demo/generateSignal.m @@ -0,0 +1,67 @@ +%% 生成混频信号 + +function rawData = generateSignal(Parameter) + + c = Parameter.c; %光速 + stratFreq = Parameter.stratFreq; %起始频率 + + Tr = Parameter.Tr; %扫频时间 + samples = Parameter.Samples; %采样点 + fs = Parameter.Fs; %采样率 + + rangeBin = Parameter.rangeBin; %rangeBin + chirps = Parameter.Chirps; %chirp数 + dopplerBin = Parameter.dopplerBin; %dopplerBin + + slope = Parameter.Slope; %chirp斜率 + bandwidth = Parameter.Bandwidth; %发射信号带宽 + centerFreq = Parameter.centerFreq; %中心频率 + lambda = Parameter.lambda; + txAntenna = Parameter.txAntenna; %发射天线 + txNum = length(txAntenna); %发射天线数 + rxAntenna = Parameter.rxAntenna; %接收天线 + rxNum = length(rxAntenna); %接收天线数 + dz = Parameter.dz; %俯仰间距 + dx = Parameter.dx; %水平间距 + + target = Parameter.target; %目标 + targetNum = size(target,1); %目标数 + rawData = zeros(txNum*rxNum,rangeBin,dopplerBin); + + t = 0:1/fs:Tr-(1/fs); %chirp采样的时间序列 + for chirpId = 1:chirps + for txId = 1:txNum + St = exp((1i*2*pi)*(centerFreq*(t+(chirpId-1)*Tr)+slope/2*t.^2)); %发射信号 + + for rxId = 1:rxNum + Sif = zeros(1,rangeBin); + for targetId = 1:targetNum + + %%连续帧 目标设置，如果不需要连续帧，令Parameter.frame=0，即可。 + if targetId==1 + targetRange = target(targetId,1)-Parameter.frame; + targetSpeed = target(targetId,2); + targetAngle = target(targetId,3); + elseif targetId==2 + targetRange = target(targetId,1)+0.5*Parameter.frame; + targetSpeed = target(targetId,2); + targetAngle = target(targetId,3); + elseif targetId==3 + targetRange = target(targetId,1)+Parameter.frame; + targetSpeed = target(targetId,2); + targetAngle = target(targetId,3); + end + + tau = 2 * (targetRange + targetSpeed * (txId - 1) * Tr) / c; + fd = 2 * targetSpeed / lambda; + wx = ((txId-1) * rxNum + rxId) / lambda * dx * sind(targetAngle); + Sr = 10*exp((1i*2*pi)*((centerFreq-fd)*(t-tau+(chirpId-1) * Tr)+slope/2*(t-tau).^2 -wx)); %回波信号 + Sif = Sif + St .* conj(Sr); + %叠加20dB高斯白噪声 + Sif = awgn(Sif,20); + end + rawData((txId-1) * rxNum + rxId,:,chirpId) = Sif; + end + end + end +end diff --git a/基于点云的人体定位demo/peakSearch.m b/基于点云的人体定位demo/peakSearch.m new file mode 100644 index 0000000..093f765 --- /dev/null +++ b/基于点云的人体定位demo/peakSearch.m @@ -0,0 +1,30 @@ +%% 寻找峰值 + +function [RD_pearkSearch,peakSearchList] = peakSearch(RD_cfar,cfarTargetList) + + peakSearchList = []; + [rangeLen, dopplerLen] = size(RD_cfar); + RD_pearkSearch = zeros(rangeLen, dopplerLen); + length = size(cfarTargetList,2); + + for targetIdx = 1:length + + rangeIdx = cfarTargetList(1,targetIdx); + dopplerIdx = cfarTargetList(2,targetIdx); %坐标 + + if rangeIdx > 1 && rangeIdx < rangeLen && dopplerIdx > 1 && dopplerIdx < dopplerLen %边界点不考虑 + + if RD_cfar(rangeIdx,dopplerIdx) > RD_cfar(rangeIdx - 1,dopplerIdx) && ... + RD_cfar(rangeIdx,dopplerIdx) > RD_cfar(rangeIdx + 1,dopplerIdx) && ... + RD_cfar(rangeIdx,dopplerIdx) > RD_cfar(rangeIdx,dopplerIdx - 1) && ... + RD_cfar(rangeIdx,dopplerIdx) > RD_cfar(rangeIdx,dopplerIdx + 1) + + RD_pearkSearch(rangeIdx,dopplerIdx) = RD_cfar(rangeIdx,dopplerIdx); + + cfarTarget = [rangeIdx ; dopplerIdx]; + + peakSearchList = [peakSearchList cfarTarget]; + end + end + end +end