Archived implementation
This is the complete Python source code, displayed verbatim and not executed by this page.
Source code
Complete archived implementation, displayed verbatim. It is not executed by this page.
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()