feat: add realistic portable localization core

2026-08-28 18:19:31 +09:00
parent bc63ada1cd
commit d002a2bd70
20 changed files with 2345 additions and 70 deletions
@@ -1,4 +1,5 @@
 import { dbToLinear, createScanPoints, distance3, findPeak, scanIndexToPosition, TAU } from "./math.js";
+import { interpolateFrequencyResponse } from "./frequency-response.js";
 import { binsForBand, extractStftBins, getFrameCount } from "./stft.js";

 function throwIfAborted(signal) {
@@ -13,6 +14,10 @@ function yieldToEventLoop() {
  return new Promise((resolve) => setTimeout(resolve, 0));
 }

+export const DEFAULT_MAXIMUM_CPU_STEERING_CACHE_BYTES = 64 * 1024 * 1024;
+export const DEFAULT_MAXIMUM_CPU_ANALYSIS_PLAN_BYTES = 512 * 1024 * 1024;
+const DAS_ANALYSIS_PLAN_KIND = "micarrsim.das-analysis-plan/1";
+
 export function createDistanceMatrix(arrayDefinition, scan) {
  const points = createScanPoints(scan);
  const microphoneCount = arrayDefinition.microphones.length;
@@ -31,6 +36,94 @@ export function createDistanceMatrix(arrayDefinition, scan) {
  return distances;
 }

+function createBandIndexLookup(bandBins) {
+  const lookup = new Map();
+  bandBins.forEach((bins, bandIndex) => {
+    bins.forEach((bin) => {
+      if (!lookup.has(bin)) lookup.set(bin, []);
+      lookup.get(bin).push(bandIndex);
+    });
+  });
+  return lookup;
+}
+
+function createBinCalibration(arrayDefinition, bin, sampleRate, fftSize, compensateFrequencyResponse) {
+  const microphoneCount = arrayDefinition.microphones.length;
+  const gains = new Float64Array(microphoneCount);
+  const phases = new Float64Array(microphoneCount);
+  const frequencyHz = (bin * sampleRate) / fftSize;
+  const response = {};
+  for (let microphoneIndex = 0; microphoneIndex < microphoneCount; microphoneIndex += 1) {
+    const microphone = arrayDefinition.microphones[microphoneIndex];
+    let responseGainDb = 0;
+    let responsePhaseDeg = 0;
+    if (compensateFrequencyResponse && microphone.frequencyResponse) {
+      interpolateFrequencyResponse(microphone.frequencyResponse, frequencyHz, response);
+      responseGainDb = response.gainDb;
+      responsePhaseDeg = response.phaseDeg;
+    }
+    gains[microphoneIndex] = dbToLinear(
+      -(microphone.sensitivityDb ?? 0) - responseGainDb,
+    );
+    phases[microphoneIndex] =
+      ((microphone.phaseOffsetDeg ?? 0) + responsePhaseDeg) * Math.PI / 180;
+  }
+  return { gains, phases };
+}
+
+function fillSteeringVector({
+  angularWaveNumber,
+  distances,
+  distanceOffset,
+  calibration,
+  outputReal,
+  outputImaginary,
+  outputOffset,
+}) {
+  for (let microphoneIndex = 0; microphoneIndex < calibration.gains.length; microphoneIndex += 1) {
+    const phase =
+      angularWaveNumber * distances[distanceOffset + microphoneIndex] -
+      calibration.phases[microphoneIndex];
+    outputReal[outputOffset + microphoneIndex] =
+      Math.cos(phase) * calibration.gains[microphoneIndex];
+    outputImaginary[outputOffset + microphoneIndex] =
+      Math.sin(phase) * calibration.gains[microphoneIndex];
+  }
+}
+
+async function createSteeringMatrix(options) {
+  const {
+    bin,
+    pointCount,
+    microphoneCount,
+    distances,
+    sampleRate,
+    fftSize,
+    speedOfSound,
+    calibration,
+    signal,
+    pointBatchSize,
+  } = options;
+  const real = new Float64Array(pointCount * microphoneCount);
+  const imaginary = new Float64Array(pointCount * microphoneCount);
+  const angularWaveNumber = (TAU * bin * sampleRate) / (fftSize * speedOfSound);
+  for (let pointIndex = 0; pointIndex < pointCount; pointIndex += 1) {
+    throwIfAborted(signal);
+    const offset = pointIndex * microphoneCount;
+    fillSteeringVector({
+      angularWaveNumber,
+      distances,
+      distanceOffset: offset,
+      calibration,
+      outputReal: real,
+      outputImaginary: imaginary,
+      outputOffset: offset,
+    });
+    if ((pointIndex + 1) % pointBatchSize === 0) await yieldToEventLoop();
+  }
+  return { real, imaginary };
+}
+
 export async function computeBinMap(options) {
  const {
    bin,
@@ -42,26 +135,64 @@ export async function computeBinMap(options) {
    sampleRate,
    fftSize,
    speedOfSound,
+    compensateFrequencyResponse = true,
+    calibration: providedCalibration,
+    steeringMatrix,
+    output: providedOutput,
+    steeringReal: providedSteeringReal,
+    steeringImaginary: providedSteeringImaginary,
    signal,
-    pointBatchSize,
+    pointBatchSize = 64,
  } = options;
  const microphoneCount = arrayDefinition.microphones.length;
-  const output = new Float32Array(pointCount);
-  const steeringReal = new Float64Array(microphoneCount);
-  const steeringImaginary = new Float64Array(microphoneCount);
+  const output = providedOutput ?? new Float32Array(pointCount);
+  const steeringReal = providedSteeringReal ?? new Float64Array(microphoneCount);
+  const steeringImaginary = providedSteeringImaginary ?? new Float64Array(microphoneCount);
  const angularWaveNumber = (TAU * bin * sampleRate) / (fftSize * speedOfSound);
  const microphoneScale = 1 / microphoneCount;
+  const calibration = providedCalibration ?? createBinCalibration(
+    arrayDefinition,
+    bin,
+    sampleRate,
+    fftSize,
+    compensateFrequencyResponse,
+  );
+  const phaseRealValues = steeringMatrix?.real ?? steeringReal;
+  const phaseImaginaryValues = steeringMatrix?.imaginary ?? steeringImaginary;
+  if (output.length !== pointCount) throw new RangeError("output length must match pointCount.");
+  if (steeringReal.length !== microphoneCount || steeringImaginary.length !== microphoneCount) {
+    throw new RangeError("steering workspace length must match microphone count.");
+  }
+  if (
+    calibration.gains.length !== microphoneCount ||
+    calibration.phases.length !== microphoneCount
+  ) {
+    throw new RangeError("calibration length must match microphone count.");
+  }
+  if (
+    steeringMatrix &&
+    (
+      steeringMatrix.real.length !== pointCount * microphoneCount ||
+      steeringMatrix.imaginary.length !== pointCount * microphoneCount
+    )
+  ) {
+    throw new RangeError("steering matrix length must match point and microphone counts.");
+  }

  for (let pointIndex = 0; pointIndex < pointCount; pointIndex += 1) {
    throwIfAborted(signal);
    const distanceOffset = pointIndex * microphoneCount;
-    for (let microphoneIndex = 0; microphoneIndex < microphoneCount; microphoneIndex += 1) {
-      const microphone = arrayDefinition.microphones[microphoneIndex];
-      const calibrationPhase = ((microphone.phaseOffsetDeg ?? 0) * Math.PI) / 180;
-      const phase = angularWaveNumber * distances[distanceOffset + microphoneIndex] - calibrationPhase;
-      const calibrationGain = dbToLinear(-(microphone.sensitivityDb ?? 0));
-      steeringReal[microphoneIndex] = Math.cos(phase) * calibrationGain;
-      steeringImaginary[microphoneIndex] = Math.sin(phase) * calibrationGain;
+    const steeringOffset = steeringMatrix ? distanceOffset : 0;
+    if (!steeringMatrix) {
+      fillSteeringVector({
+        angularWaveNumber,
+        distances,
+        distanceOffset,
+        calibration,
+        outputReal: steeringReal,
+        outputImaginary: steeringImaginary,
+        outputOffset: 0,
+      });
    }

    let accumulatedPower = 0;
@@ -73,8 +204,8 @@ export async function computeBinMap(options) {
        const spectrumIndex = spectrumOffset + microphoneIndex;
        const inputReal = spectrum.real[spectrumIndex];
        const inputImaginary = spectrum.imaginary[spectrumIndex];
-        const phaseReal = steeringReal[microphoneIndex];
-        const phaseImaginary = steeringImaginary[microphoneIndex];
+        const phaseReal = phaseRealValues[steeringOffset + microphoneIndex];
+        const phaseImaginary = phaseImaginaryValues[steeringOffset + microphoneIndex];
        sumReal += inputReal * phaseReal - inputImaginary * phaseImaginary;
        sumImaginary += inputReal * phaseImaginary + inputImaginary * phaseReal;
      }
@@ -86,11 +217,200 @@ export async function computeBinMap(options) {
  return output;
 }

-export function estimateDasWorkload(options) {
-  const { channels, sampleRate, arrayDefinition, experiment } = options;
-  const { fftSize, hopSize } = experiment.stft;
+/**
+ * Cache geometry and optional steering matrices for repeated CPU DAS analyses.
+ *
+ * Plans are intentionally stateful and must not be used concurrently. They are
+ * local runtime objects (Maps and workspaces), not serializable data contracts.
+ */
+export function createDasAnalysisPlan(options) {
+  const {
+    sampleRate,
+    arrayDefinition,
+    scene,
+    experiment,
+    maximumSteeringCacheBytes = DEFAULT_MAXIMUM_CPU_STEERING_CACHE_BYTES,
+    maximumPlanBytes = DEFAULT_MAXIMUM_CPU_ANALYSIS_PLAN_BYTES,
+  } = options;
+  if (!Number.isFinite(sampleRate) || sampleRate <= 0) {
+    throw new RangeError("sampleRate must be a positive finite number.");
+  }
+  if (!Array.isArray(arrayDefinition?.microphones) || arrayDefinition.microphones.length === 0) {
+    throw new TypeError("arrayDefinition.microphones must be a non-empty array.");
+  }
+  if (!Number.isFinite(scene?.speedOfSound) || scene.speedOfSound <= 0) {
+    throw new RangeError("scene.speedOfSound must be a positive finite number.");
+  }
+  if (!Number.isFinite(maximumSteeringCacheBytes) || maximumSteeringCacheBytes < 0) {
+    throw new RangeError("maximumSteeringCacheBytes must be a non-negative finite number.");
+  }
+  if (!Number.isSafeInteger(maximumPlanBytes) || maximumPlanBytes <= 0) {
+    throw new RangeError("maximumPlanBytes must be a positive safe integer.");
+  }
+
+  const { fftSize } = experiment.stft;
  const bandBins = experiment.frequencyBands.map((band) => binsForBand(band, sampleRate, fftSize));
  const selectedBins = [...new Set(bandBins.flat())].sort((a, b) => a - b);
+  const microphoneCount = arrayDefinition.microphones.length;
+  const pointCount = experiment.scan.columns * experiment.scan.rows;
+  const pointMicrophonePairs = pointCount * microphoneCount;
+  const distanceMatrixBytes = pointMicrophonePairs * Float32Array.BYTES_PER_ELEMENT;
+  const retainedBaseBytes =
+    distanceMatrixBytes +
+    pointCount * Float32Array.BYTES_PER_ELEMENT +
+    microphoneCount * Float64Array.BYTES_PER_ELEMENT * 2;
+  if (
+    !Number.isSafeInteger(pointMicrophonePairs) ||
+    !Number.isSafeInteger(retainedBaseBytes) ||
+    retainedBaseBytes > maximumPlanBytes
+  ) {
+    throw new RangeError(
+      "CPU analysis plan requires " + retainedBaseBytes.toLocaleString() +
+      " base bytes, above maximumPlanBytes " + maximumPlanBytes.toLocaleString() + ".",
+    );
+  }
+  const distances = createDistanceMatrix(arrayDefinition, experiment.scan);
+  const binMap = new Float32Array(pointCount);
+  const steeringReal = new Float64Array(microphoneCount);
+  const steeringImaginary = new Float64Array(microphoneCount);
+  const steeringBytesPerBin =
+    pointCount * microphoneCount * Float64Array.BYTES_PER_ELEMENT * 2;
+  const requestedSteeringCacheBytes = steeringBytesPerBin * selectedBins.length;
+  const steeringCacheEnabled =
+    Number.isSafeInteger(requestedSteeringCacheBytes) &&
+    requestedSteeringCacheBytes > 0 &&
+    requestedSteeringCacheBytes <= maximumSteeringCacheBytes &&
+    retainedBaseBytes + requestedSteeringCacheBytes <= maximumPlanBytes;
+
+  return {
+    kind: DAS_ANALYSIS_PLAN_KIND,
+    sampleRate,
+    arrayDefinition,
+    scene,
+    experiment,
+    microphoneCount,
+    pointCount,
+    bandBins,
+    selectedBins,
+    distances,
+    bandIndexesByBin: createBandIndexLookup(bandBins),
+    calibrations: new Map(),
+    steeringMatrices: new Map(),
+    steeringCacheEnabled,
+    steeringBytesPerBin,
+    requestedSteeringCacheBytes,
+    maximumSteeringCacheBytes,
+    maximumPlanBytes,
+    binMap,
+    steeringReal,
+    steeringImaginary,
+    busy: false,
+    statistics: {
+      analysisAttempts: 0,
+      completedAnalyses: 0,
+      distanceMatrixBuilds: 1,
+      distanceEvaluations: pointCount * microphoneCount,
+      avoidedDistanceEvaluations: 0,
+      calibrationBuilds: 0,
+      calibrationCacheHits: 0,
+      steeringMatrixBuilds: 0,
+      steeringCacheHits: 0,
+      steeringPointMicrophonePairs: 0,
+      avoidedSteeringPointMicrophonePairs: 0,
+      binMapUses: 0,
+      avoidedBinMapAllocations: 0,
+      retainedBytes: retainedBaseBytes,
+    },
+  };
+}
+
+function assertCompatibleAnalysisPlan(plan, options) {
+  if (plan?.kind !== DAS_ANALYSIS_PLAN_KIND) {
+    throw new TypeError("analysisPlan was not created by createDasAnalysisPlan().");
+  }
+  if (
+    plan.sampleRate !== options.sampleRate ||
+    plan.arrayDefinition !== options.arrayDefinition ||
+    plan.scene !== options.scene ||
+    plan.experiment !== options.experiment
+  ) {
+    throw new Error("analysisPlan must be used with the exact configuration objects that created it.");
+  }
+}
+
+function planCalibration(plan, bin) {
+  let calibration = plan.calibrations.get(bin);
+  if (calibration) {
+    plan.statistics.calibrationCacheHits += 1;
+    return calibration;
+  }
+  calibration = createBinCalibration(
+    plan.arrayDefinition,
+    bin,
+    plan.sampleRate,
+    plan.experiment.stft.fftSize,
+    plan.experiment.beamforming.compensateFrequencyResponse !== false,
+  );
+  plan.calibrations.set(bin, calibration);
+  plan.statistics.calibrationBuilds += 1;
+  plan.statistics.retainedBytes += calibration.gains.byteLength + calibration.phases.byteLength;
+  return calibration;
+}
+
+async function planSteeringMatrix(plan, bin, calibration, signal, pointBatchSize) {
+  if (!plan.steeringCacheEnabled) return undefined;
+  let matrix = plan.steeringMatrices.get(bin);
+  if (matrix) {
+    plan.statistics.steeringCacheHits += 1;
+    plan.statistics.avoidedSteeringPointMicrophonePairs +=
+      plan.pointCount * plan.microphoneCount;
+    return matrix;
+  }
+  matrix = await createSteeringMatrix({
+    bin,
+    pointCount: plan.pointCount,
+    microphoneCount: plan.microphoneCount,
+    distances: plan.distances,
+    sampleRate: plan.sampleRate,
+    fftSize: plan.experiment.stft.fftSize,
+    speedOfSound: plan.scene.speedOfSound,
+    calibration,
+    signal,
+    pointBatchSize,
+  });
+  plan.steeringMatrices.set(bin, matrix);
+  plan.statistics.steeringMatrixBuilds += 1;
+  plan.statistics.steeringPointMicrophonePairs +=
+    plan.pointCount * plan.microphoneCount;
+  plan.statistics.retainedBytes += matrix.real.byteLength + matrix.imaginary.byteLength;
+  return matrix;
+}
+
+function analysisCacheSummary(plan) {
+  return {
+    distanceMatrixBuilds: plan.statistics.distanceMatrixBuilds,
+    distanceEvaluations: plan.statistics.distanceEvaluations,
+    avoidedDistanceEvaluations: plan.statistics.avoidedDistanceEvaluations,
+    calibrationBuilds: plan.statistics.calibrationBuilds,
+    calibrationCacheHits: plan.statistics.calibrationCacheHits,
+    steeringCacheEnabled: plan.steeringCacheEnabled,
+    steeringMatrixBuilds: plan.statistics.steeringMatrixBuilds,
+    steeringCacheHits: plan.statistics.steeringCacheHits,
+    steeringPointMicrophonePairs: plan.statistics.steeringPointMicrophonePairs,
+    avoidedSteeringPointMicrophonePairs:
+      plan.statistics.avoidedSteeringPointMicrophonePairs,
+    avoidedBinMapAllocations: plan.statistics.avoidedBinMapAllocations,
+    retainedBytes: plan.statistics.retainedBytes,
+  };
+}
+
+export function estimateDasWorkload(options) {
+  const { channels, sampleRate, arrayDefinition, experiment, analysisPlan } = options;
+  const { fftSize, hopSize } = experiment.stft;
+  const bandBins = analysisPlan?.bandBins ??
+    experiment.frequencyBands.map((band) => binsForBand(band, sampleRate, fftSize));
+  const selectedBins = analysisPlan?.selectedBins ??
+    [...new Set(bandBins.flat())].sort((a, b) => a - b);
  const frameCount = getFrameCount(channels[0].length, fftSize, hopSize);
  const microphoneCount = arrayDefinition.microphones.length;
  const pointCount = experiment.scan.columns * experiment.scan.rows;
@@ -100,7 +420,11 @@ export function estimateDasWorkload(options) {
  const binMapBytes = pointCount * Float32Array.BYTES_PER_ELEMENT;
  const spectrumBytesPerBin = frameCount * microphoneCount * Float32Array.BYTES_PER_ELEMENT * 2;
  const fftWorkspaceBytes = fftSize * Float64Array.BYTES_PER_ELEMENT * 2;
-  const baseWorkingBytes = distanceBytes + outputBytes + binMapBytes + fftWorkspaceBytes;
+  const steeringCacheBytes = analysisPlan?.steeringCacheEnabled
+    ? analysisPlan.requestedSteeringCacheBytes
+    : 0;
+  const baseWorkingBytes =
+    distanceBytes + outputBytes + binMapBytes + fftWorkspaceBytes + steeringCacheBytes;
  const complexMacCount = pointCount * microphoneCount * frameCount * selectedBins.length;
  return {
    bandBins,
@@ -110,6 +434,7 @@ export function estimateDasWorkload(options) {
    pointCount,
    distanceBytes,
    outputBytes,
+    steeringCacheBytes,
    spectrumBytesPerBin,
    baseWorkingBytes,
    complexMacCount,
@@ -147,7 +472,7 @@ export function finalizeDasBands(rawMaps, workload, experiment) {
  });
 }

-export async function computeDasMap(options) {
+async function computeDasMapInternal(options, analysisPlan) {
  const {
    channels,
    sampleRate,
@@ -167,7 +492,13 @@ export async function computeDasMap(options) {
    throw new Error(`채널 수 ${channels.length}와 마이크 수 ${arrayDefinition.microphones.length}가 다릅니다.`);
  }
  if (!Number.isInteger(pointBatchSize) || pointBatchSize < 1) throw new Error("pointBatchSize는 1 이상의 정수여야 합니다.");
-  const workload = estimateDasWorkload({ channels, sampleRate, arrayDefinition, experiment });
+  const workload = estimateDasWorkload({
+    channels,
+    sampleRate,
+    arrayDefinition,
+    experiment,
+    analysisPlan,
+  });
  if (workload.complexMacCount > maximumComplexMacs) {
    throw new Error(
      `CPU DAS 예상 복소 누산 ${workload.complexMacCount.toLocaleString()}회가 ` +
@@ -199,15 +530,10 @@ export async function computeDasMap(options) {
        `현재 한도 ${maximumFftButterflies.toLocaleString()}회를 넘습니다. duration, FFT 크기 또는 대역폭을 줄이세요.`,
    );
  }
-  const distances = createDistanceMatrix(arrayDefinition, experiment.scan);
+  const distances = analysisPlan?.distances ?? createDistanceMatrix(arrayDefinition, experiment.scan);
  const rawMaps = experiment.frequencyBands.map(() => new Float32Array(workload.pointCount));
-  const bandIndexesByBin = new Map();
-  workload.bandBins.forEach((bins, bandIndex) => {
-    bins.forEach((bin) => {
-      if (!bandIndexesByBin.has(bin)) bandIndexesByBin.set(bin, []);
-      bandIndexesByBin.get(bin).push(bandIndex);
-    });
-  });
+  const bandIndexesByBin =
+    analysisPlan?.bandIndexesByBin ?? createBandIndexLookup(workload.bandBins);

  let completedBins = 0;
  for (let batchStart = 0; batchStart < workload.selectedBins.length; batchStart += batchSize) {
@@ -236,6 +562,17 @@ export async function computeDasMap(options) {
    throwIfAborted(signal);

    for (const bin of batchBins) {
+      const calibration = analysisPlan ? planCalibration(analysisPlan, bin) : undefined;
+      const steeringMatrix = analysisPlan
+        ? await planSteeringMatrix(analysisPlan, bin, calibration, signal, pointBatchSize)
+        : undefined;
+      if (analysisPlan) {
+        analysisPlan.statistics.binMapUses += 1;
+        analysisPlan.statistics.avoidedBinMapAllocations = Math.max(
+          0,
+          analysisPlan.statistics.binMapUses - 1,
+        );
+      }
      const binMap = await computeBinMap({
        bin,
        spectrum: stft.spectra.get(bin),
@@ -246,6 +583,13 @@ export async function computeDasMap(options) {
        sampleRate,
        fftSize: experiment.stft.fftSize,
        speedOfSound: scene.speedOfSound,
+        compensateFrequencyResponse:
+          experiment.beamforming.compensateFrequencyResponse !== false,
+        calibration,
+        steeringMatrix,
+        output: analysisPlan?.binMap,
+        steeringReal: analysisPlan?.steeringReal,
+        steeringImaginary: analysisPlan?.steeringImaginary,
        signal,
        pointBatchSize,
      });
@@ -270,5 +614,33 @@ export async function computeDasMap(options) {
    workload,
    fftButterflyCount,
    bands,
+    ...(analysisPlan ? { analysisCache: analysisCacheSummary(analysisPlan) } : {}),
  };
 }
+
+export async function computeDasMap(options) {
+  const analysisPlan = options.analysisPlan;
+  if (!analysisPlan) return computeDasMapInternal(options, undefined);
+  assertCompatibleAnalysisPlan(analysisPlan, options);
+  if (analysisPlan.busy) {
+    throw new Error("analysisPlan is already in use by another DAS analysis.");
+  }
+  analysisPlan.busy = true;
+  analysisPlan.statistics.analysisAttempts += 1;
+  if (analysisPlan.statistics.analysisAttempts > 1) {
+    analysisPlan.statistics.avoidedDistanceEvaluations +=
+      analysisPlan.pointCount * analysisPlan.microphoneCount;
+  }
+  try {
+    const result = await computeDasMapInternal(options, analysisPlan);
+    analysisPlan.statistics.completedAnalyses += 1;
+    result.analysisCache = {
+      ...result.analysisCache,
+      analysisAttempts: analysisPlan.statistics.analysisAttempts,
+      completedAnalyses: analysisPlan.statistics.completedAnalyses,
+    };
+    return result;
+  } finally {
+    analysisPlan.busy = false;
+  }
+}