import numpy as np import sounddevice as sd from scipy.signal import butter, lfilter import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation import queue import sys import time as pytime # ============================================================================== # CONFIGURATION # ============================================================================== FS = 48000 # Sample Rate (Hz) F_CARRIER = 20000 # Pilot Tone (Ultrasound) CHUNK_SIZE = 2400 # 50ms windows @ 48kHz (Audio Thread Stability) STEP = 480 # 10ms slices for DSP (100 FPS Temporal Resolution) FPS = FS / STEP # 100 FPS T = 1.0 / FS # Sampling Period CALIBRATION_SEC = 3.0 # Baseline duration HISTORY_LEN = 500 # 5 seconds of display history at 100 FPS # Filter: 2nd Order Butterworth (19.5kHz - 20.5kHz) NYQ = 0.5 * FS LOW = 19500 / NYQ HIGH = 20500 / NYQ B, A = butter(2, [LOW, HIGH], btype='band') # Heartbeat Filter (0.8 Hz to 2.5 Hz) # Note: at 100 FPS, Nyquist is 50Hz NYQ_100 = 0.5 * FPS B_heart, A_heart = butter(2, [0.8 / NYQ_100, 2.5 / NYQ_100], btype='band') # Data Queues input_queue = queue.Queue() velocity_history = np.zeros(HISTORY_LEN) position_history = np.zeros(HISTORY_LEN) heart_history = np.zeros(HISTORY_LEN) # State Variables v_dc_fast = 0.0 # For responsive velocity v_dc_slow = 0.0 # For stable breathing v_smoothed = 0.0 # For clean integration dsp_buffer = np.zeros(CHUNK_SIZE) # Rolling 50ms buffer for V1 stability # Open a CSV file to log the raw data for offline analysis log_file = open("ghost_data_v2.csv", "w") log_file.write("time_ms,f_inst,v_raw,v_filtered,position,heartbeat\n") frame_counter = 0 # ============================================================================== # THE CORE SPLINE ENGINE # ============================================================================== def extract_instantaneous_freq(x): """ Stabilized 2nd-Order Cardinal Exponential Spline Operator. Solves for the 'Annihilating Filter' pole in the Null Space. """ x_n = x[1:-1] x_prev = x[:-2] x_next = x[2:] num = np.sum((x_prev + x_next) * x_n) den = 2.0 * np.sum(x_n**2) if den == 0: return F_CARRIER cos_wT = num / den cos_wT = np.clip(cos_wT, -1.0, 1.0) wT = np.arccos(cos_wT) f_inst = wT / (2 * np.pi * T) return f_inst # ============================================================================== # AUDIO CALLBACK # ============================================================================== pilot_tone = np.sin(2 * np.pi * F_CARRIER * np.arange(CHUNK_SIZE) * T).astype(np.float32) def audio_callback(indata, outdata, frames, time, status): outdata[:] = pilot_tone.reshape(-1, 1) input_queue.put(indata.copy()) # ============================================================================== # MAIN TRACKING LOOP # ============================================================================== def main(): print(f"--- DIGITAL GHOST V2: 100 FPS SPLINE SENSING ---") print(f"Carrier: {F_CARRIER} Hz | FS: {FS} Hz | Engine: {FPS} FPS") print("\nGet ready! Calibration starts in:") for i in range(3, 0, -1): print(f"{i}...") pytime.sleep(1) print(f"\nStep 1: Calibration. HOLD PERFECTLY STILL for {CALIBRATION_SEC}s...") with sd.Stream(channels=1, samplerate=FS, blocksize=CHUNK_SIZE, callback=audio_callback): # 1. CALIBRATION PHASE baseline_freqs = [] required_chunks = int(CALIBRATION_SEC * FS / CHUNK_SIZE) global dsp_buffer while len(baseline_freqs) < (required_chunks * (CHUNK_SIZE // STEP)): chunk = input_queue.get() filtered = lfilter(B, A, chunk.flatten()) # Rolling buffer calibration to perfectly match tracking behavior for i in range(0, len(filtered) - STEP + 1, STEP): sub_chunk = filtered[i:i+STEP] # Slide the buffer and append new data dsp_buffer = np.roll(dsp_buffer, -STEP) dsp_buffer[-STEP:] = sub_chunk # Skip the first few frames until the buffer is fully populated if len(baseline_freqs) == 0 and np.sum(dsp_buffer[:STEP]) == 0: continue # Extract frequency using the FULL 50ms buffer f = extract_instantaneous_freq(dsp_buffer) baseline_freqs.append(f) baseline = np.mean(baseline_freqs) print(f"Calibration Complete. Baseline: {baseline:.2f} Hz") print(f"Step 2: Tracking. (Ctrl+C to stop)") # 2. VISUALIZATION SETUP (3 Panels Now!) fig, (ax1, ax2, ax3) = plt.subplots(3, 1, figsize=(10, 10)) line_v, = ax1.plot(velocity_history, color='#00ffcc', lw=1.5) ax1.set_title("Velocity (Doppler Shift @ 100 FPS)") ax1.set_ylabel("Hz") ax1.grid(True, alpha=0.3) line_p, = ax2.plot(position_history, color='#ff0066', lw=1.5) ax2.set_title("Macro Displacement (Breathing)") ax2.set_ylabel("Phase Shift") ax2.grid(True, alpha=0.3) line_h, = ax3.plot(heart_history, color='#ffcc00', lw=1.5) ax3.set_title("Micro Tremors (Cardiac Bandpass 0.8-2.5Hz)") ax3.set_ylabel("Amplitude") ax3.grid(True, alpha=0.3) plt.tight_layout() with input_queue.mutex: input_queue.queue.clear() # 3. ANIMATION UPDATE def update(frame): global velocity_history, position_history, heart_history global v_dc_fast, v_dc_slow, v_smoothed, frame_counter global dsp_buffer while not input_queue.empty(): chunk = input_queue.get() filtered_chunk = lfilter(B, A, chunk.flatten()) # Process in 10ms slices, but compute math over a rolling 50ms window for i in range(0, len(filtered_chunk) - STEP + 1, STEP): sub_chunk = filtered_chunk[i:i+STEP] dsp_buffer = np.roll(dsp_buffer, -STEP) dsp_buffer[-STEP:] = sub_chunk # Compute frequency on the full 2400-sample buffer (V1 Stability) f = extract_instantaneous_freq(dsp_buffer) v_raw = f - baseline # --- PATH A: RESPONSIVE VELOCITY (V1 Math Restored) --- # Because we restored the V1 noise floor, we can use the responsive V1 alpha v_dc_fast = (0.95 * v_dc_fast) + (0.05 * v_raw) v_instant = v_raw - v_dc_fast velocity_history = np.roll(velocity_history, -1) velocity_history[-1] = v_instant # --- PATH B: BREATHING INTEGRATION --- v_dc_slow = (0.99 * v_dc_slow) + (0.01 * v_raw) v_for_int = v_raw - v_dc_slow v_smoothed = (0.96 * v_smoothed) + (0.04 * v_for_int) new_pos = position_history[-1] + v_smoothed position_history = np.roll(position_history, -1) position_history[-1] = new_pos # --- PATH C: HEARTBEAT EXTRACTION --- # Apply the heart filter to the raw position to extract the micro-tremors heart_signal = lfilter(B_heart, A_heart, position_history)[-1] heart_history = np.roll(heart_history, -1) heart_history[-1] = heart_signal # 4. Log to CSV time_ms = frame_counter * 10 # 10ms per chunk at 100 FPS log_file.write(f"{time_ms},{f:.4f},{v_raw:.4f},{v_smoothed:.4f},{new_pos:.4f},{heart_signal:.4f}\n") frame_counter += 1 # Update Plot Data line_v.set_ydata(velocity_history) line_p.set_ydata(position_history) line_h.set_ydata(heart_history) # Auto-scale Top (Velocity) v_max = np.max(np.abs(velocity_history)) ax1.set_ylim(-max(5, v_max*1.2), max(5, v_max*1.2)) # Auto-scale Middle (Breathing Position) p_min, p_max = np.min(position_history), np.max(position_history) p_range = max(0.5, p_max - p_min) center_p = (p_max + p_min) / 2.0 ax2.set_ylim(center_p - (p_range * 0.7), center_p + (p_range * 0.7)) # Auto-scale Bottom (Heartbeat) - Locked to small micro-tremor bounds h_max = np.max(np.abs(heart_history)) h_range = max(0.2, h_max * 1.5) ax3.set_ylim(-h_range, h_range) return line_v, line_p, line_h ani = FuncAnimation(fig, update, interval=10, blit=True, cache_frame_data=False) plt.show() if __name__ == "__main__": try: main() except KeyboardInterrupt: print("\nStopping...") log_file.close()