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