1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
| """
雷达心率估计 - 呼吸谐波消除算法
基于 Sensors 2026 论文思路复现
适用于 FMCW / UWB 雷达
"""
import numpy as np
from scipy.signal import butter, filtfilt, iirnotch
from typing import Tuple
class RadarHeartRateEstimator:
"""
雷达心率估计器
流程：
1. 雷达相位提取（胸部微动）
2. 带通滤波分离呼吸和心跳
3. 呼吸频率检测
4. 谐波自适应陷波
5. 心率提取
性能目标：
- 心率误差 < 5 bpm
- 呼吸频率误差 < 2 /min
- 处理延迟 < 5s
"""
def __init__(self, sample_rate: float = 50.0):
self.fs = sample_rate
def process(self, radar_phase: np.ndarray) -> dict:
"""
处理雷达相位信号
Args:
radar_phase: 雷达相位数据 (N,), 已提取的胸部微动信号
Returns:
dict with 'heart_rate', 'resp_rate', 'snr', 'quality'
"""
resp_signal = self._bandpass(radar_phase, 0.1, 0.5)
heart_signal = self._bandpass(radar_phase, 0.8, 2.0)
resp_freq = self._find_dominant_freq(resp_signal, 0.1, 0.5)
resp_rate = resp_freq * 60
harmonics = [resp_freq * n for n in range(2, 8)]
harmonics = [f for f in harmonics if 0.8 <= f <= 2.0]
filtered_heart = heart_signal.copy()
for harmonic in harmonics:
if harmonic > 0:
w0 = harmonic / (self.fs / 2)
Q = 30
b, a = iirnotch(w0, Q)
filtered_heart = filtfilt(b, a, filtered_heart)
heart_freq = self._find_dominant_freq(filtered_heart, 0.8, 2.0)
heart_rate = heart_freq * 60
snr = self._compute_snr(filtered_heart, heart_freq)
quality = 'good' if snr > 3 else 'poor'
return {
'heart_rate': heart_rate,
'resp_rate': resp_rate,
'snr': snr,
'quality': quality,
'harmonics_removed': len(harmonics),
}
def _bandpass(self, signal, low, high):
"""带通滤波"""
b, a = butter(4, [low / (self.fs/2), high / (self.fs/2)], btype='band')
return filtfilt(b, a, signal)
def _find_dominant_freq(self, signal, low, high):
"""找到频带内的主频"""
fft = np.fft.rfft(signal)
freqs = np.fft.rfftfreq(len(signal), 1/self.fs)
power = np.abs(fft)**2
mask = (freqs >= low) & (freqs <= high)
if not mask.any():
return 0
peak_idx = np.argmax(power[mask])
return freqs[mask][peak_idx]
def _compute_snr(self, signal, freq):
"""计算信噪比"""
if freq == 0:
return 0
fft = np.fft.rfft(signal)
freqs = np.fft.rfftfreq(len(signal), 1/self.fs)
power = np.abs(fft)**2
peak_idx = np.argmin(np.abs(freqs - freq))
signal_power = power[peak_idx]
noise_range = np.concatenate([
power[max(0, peak_idx-10):peak_idx-5],
power[peak_idx+5:peak_idx+10]
])
noise_power = np.mean(noise_range) if len(noise_range) > 0 else 1
return 10 * np.log10(signal_power / (noise_power + 1e-10))
if __name__ == "__main__":
import time
estimator = RadarHeartRateEstimator(sample_rate=50)
fs = 50
duration = 10
t = np.arange(0, duration, 1/fs)
true_hr = 72 / 60
true_rr = 16 / 60
heart = 0.05 * np.sin(2 * np.pi * true_hr * t)
resp = 0.5 * np.sin(2 * np.pi * true_rr * t)
for n in range(2, 8):
resp += 0.5 / n * np.sin(2 * np.pi * n * true_rr * t)
noise = 0.02 * np.random.randn(len(t))
radar_signal = heart + resp + noise
start = time.time()
result = estimator.process(radar_signal)
elapsed = (time.time() - start) * 1000
print("=" * 60)
print("雷达心率估计测试")
print("=" * 60)
print(f"真实心率: {true_hr*60:.0f} bpm")
print(f"真实呼吸: {true_rr*60:.0f} /min")
print(f"\n检测结果:")
print(f" 心率: {result['heart_rate']:.1f} bpm")
print(f" 呼吸: {result['resp_rate']:.1f} /min")
print(f" SNR: {result['snr']:.1f} dB")
print(f" 质量: {result['quality']}")
print(f" 消除谐波数: {result['harmonics_removed']}")
print(f"\n处理时间: {elapsed:.1f} ms")
error = abs(result['heart_rate'] - true_hr*60)
print(f"心率误差: {error:.1f} bpm {'✅' if error < 5 else '❌'}")
|
