Add files via upload
This commit is contained in:
St. Zhang
GitHub
parent
d91f202e2b
commit
31171fa508
19
基于点云的人体定位demo/RAfftMatrix.m
Normal file
19
基于点云的人体定位demo/RAfftMatrix.m
Normal file
@ -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
|
||||
14
基于点云的人体定位demo/RDfftMatrix.m
Normal file
14
基于点云的人体定位demo/RDfftMatrix.m
Normal file
@ -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
|
||||
67
基于点云的人体定位demo/cfar.m
Normal file
67
基于点云的人体定位demo/cfar.m
Normal file
@ -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
|
||||
10
基于点云的人体定位demo/chan_Accumulate.m
Normal file
10
基于点云的人体定位demo/chan_Accumulate.m
Normal file
@ -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
|
||||
83
基于点云的人体定位demo/doa.m
Normal file
83
基于点云的人体定位demo/doa.m
Normal file
@ -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
|
||||
14
基于点云的人体定位demo/generateCfarParameter.m
Normal file
14
基于点云的人体定位demo/generateCfarParameter.m
Normal file
@ -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
|
||||
36
基于点云的人体定位demo/generateParameter.m
Normal file
36
基于点云的人体定位demo/generateParameter.m
Normal file
@ -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
|
||||
67
基于点云的人体定位demo/generateSignal.m
Normal file
67
基于点云的人体定位demo/generateSignal.m
Normal file
@ -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
|
||||
30
基于点云的人体定位demo/peakSearch.m
Normal file
30
基于点云的人体定位demo/peakSearch.m
Normal file
@ -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
|
||||
Loading…
Reference in New Issue
Block a user