m-fohcde_demo.py

# -*- coding: utf-8 -*-
"""A demonstration of Multiple Pitch Estimation based on FOHCDE.

Copyright (C) 2025 by Akira TAMAMORI

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
"""

from typing import NamedTuple

import matplotlib.pyplot as plt
import numpy as np
import numpy.polynomial.polynomial as poly
import numpy.typing as npt
from scipy.signal import ShortTimeFFT, butter, convolve, filtfilt, find_peaks


class ExperimentConfig(NamedTuple):
    """Config for reproduction of Section 4.2 and 4.3."""

    fs: int = 44100
    n_sources: int = 2
    n_sinusoids_scde: int = 4
    m_orders: tuple[int, int] = (2, 2)
    duration: float = 0.010
    max_iter: int = 15
    tolerance: float = 1e-4
    frame_len_ms: float = 5.0
    hop_len_ms: float = 1.0  # overlap 4ms


class SignalConfig(NamedTuple):
    """Config for signals."""

    f_mod: float = 30.0
    f_carrier: float = 450.0
    f_excursion: float = 50.0
    f_fixed: float = 500.0


def apply_bandpass_filter(
    signal: npt.NDArray[np.float64],
    fs: float,
    lowcut: int,
    highcut: int,
    order: int = 4,
) -> npt.NDArray[np.float64]:
    """Apply a Butterworth bandpass filter to the signal.

    Args:
        signal (np.ndarray): Input discrete-time signal.
        fs (float): Sampling frequency in Hz.
        lowcut (int): Lower cutoff frequency.
        highcut (int): Higher cutoff frequency.
        order (int): Order of bandpass filter coeff.

    Returns:
        filtered_signal (np.ndarray): Bandpass-filtered signal.
    """
    nyquist = 0.5 * fs
    low = lowcut / nyquist
    high = highcut / nyquist
    filter_coef = butter(order, [low, high], btype="band")
    len_coef = 2
    assert (
        isinstance(filter_coef, tuple)
        and len(filter_coef) == len_coef
        and isinstance(filter_coef[0], np.ndarray)
        and isinstance(filter_coef[1], np.ndarray)
    )
    filtered_signal: npt.NDArray[np.float64] = filtfilt(
        filter_coef[0], filter_coef[1], signal
    )
    return filtered_signal


def solve_poly(
    alphas: npt.NDArray[np.float64], n_sinusoids: int
) -> npt.NDArray[np.float64]:
    """Solve polynomial equation for the squared angular frequency.

    Args:
        alphas (np.ndarray): Coefficients of differential equation of SCDE.
        n_sinusoids (int): Number of Sinusoids.

    Returns:
        omegas_squared (np.darray): Estimated squared angular frequency.
    """
    # The polynomial is Ω^3 - α1Ω^2 + α2Ω - α3 = 0 (based on paper's Eq. 13 and 18)
    # polyroots expects coefficients in ascending order of power:
    # [coeff_0, coeff_1, ..., coeff_n]
    # So, for n=3, the coefficients are [-alpha3, alpha2, -alpha1, 1]
    coeffs_list = []
    for i in reversed(range(n_sinusoids)):
        coeffs_list.append((-1) ** (i + 1) * alphas[i])
    coeffs_list.append(1)
    polynomial_coeffs = np.array(coeffs_list, dtype=np.float64)
    omegas_squared: npt.NDArray[np.float64] = poly.polyroots(polynomial_coeffs)
    return omegas_squared


def get_kth_even_derivative(
    s: npt.NDArray[np.float64], k_order: int, dt: float
) -> tuple[npt.NDArray[np.float64], int]:
    """Calculates the k-th order even derivative using centered differences.

    Args:
        s (np.ndarray): Input signal.
        k_order (int): The order of the derivative (must be an even number).
        dt (float): Sampling period.

    Returns:
        tuple: (derived_signal, pad_length_per_side)
               derived_signal: The calculated derivative.
               pad_length_per_side: Number of points trimmed from each
                                    side of the original signal.
    """
    if k_order % 2 != 0:
        raise ValueError("Only even order derivatives are supported.")
    if k_order < 0:
        raise ValueError("Derivative order cannot be negative.")

    kernel = np.array([1.0])
    for _ in range(k_order // 2):
        kernel = convolve(kernel, np.array([1.0, -2.0, 1.0]), mode="full")

    derived_s = convolve(s, kernel, mode="valid") / (dt**k_order)
    return derived_s, k_order // 2


def calc_covmat_mn(
    m: int,
    n: int,
    signal_original: npt.NDArray[np.float64],
    dt: float,
    max_derivative_order: int,
) -> np.float64 | float:
    """Calculates the sum of products of (2m)-th and (2n)-th derivatives for FOHCDE.

    All derivatives are trimmed to the length of the highest required derivative.
    The dt scaling is applied *after* the sum.

    Args:
        m (int): The order of the derivative.
        n (int): The order of the derivative
        signal_original (np.darray): Original signal.
        dt (float): Sampling period.
        max_derivative_order (int): Maximum of the order of derivative

    Returns:
        cov_mat (float): The (m, n) element of covariance matrix.
    """
    # Check if the signal is long enough for the highest derivative
    if len(signal_original) < max_derivative_order + 1:
        return 0.0

    # Calculate all required derivatives (without dt scaling)
    deriv_x_m_full, _ = get_kth_even_derivative(signal_original, 2 * m, dt)
    deriv_x_n_full, _ = get_kth_even_derivative(signal_original, 2 * n, dt)

    # Determine the effective length after applying the highest order derivative
    target_len = len(signal_original) - max_derivative_order  # This is 2*M

    if target_len <= 0:
        return 0.0

    # Calculate padding for each derivative to align to the target_len
    # For a 2k-th derivative, its length is L - 2k.
    # We want to trim it to L - 2M.
    # So, we trim ( (L - 2k) - (L - 2M) ) / 2 = (2M - 2k) / 2 = M - k points
    # from each side.
    # M is max_derivative_order // 2. k is m or n.
    pad_m_to_align = (max_derivative_order // 2) - m
    pad_n_to_align = (max_derivative_order // 2) - n

    trimmed_deriv_m = deriv_x_m_full[
        pad_m_to_align : len(deriv_x_m_full) - pad_m_to_align
    ]
    trimmed_deriv_n = deriv_x_n_full[
        pad_n_to_align : len(deriv_x_n_full) - pad_n_to_align
    ]

    if len(trimmed_deriv_m) != target_len or len(trimmed_deriv_n) != target_len:
        # This indicates a logic error or unexpected behavior.
        # It's crucial for numerical stability that these lengths match exactly.
        print(
            f"Debug: Trimmed lengths mismatch target! m={m}, n={n}, "
            + f"len_m={len(trimmed_deriv_m)}, len_n={len(trimmed_deriv_n)}, "
            + f"target={target_len}"
        )
        return 0.0

    covmat_mn: np.float64 = np.sum(trimmed_deriv_m * trimmed_deriv_n)
    return covmat_mn


def estimate_frequencies_scde(
    signal: npt.NDArray[np.float64], fs: float, n_sinusoids: int = 3
) -> npt.NDArray[np.float64]:
    """Estimates the frequencies of three sinusoids by SCDE.

    This function implements the estimation method of frequencies using the Sinusoidal
    Constraint Differential Equation (SCDE) method, as described in the following paper:

    Kenta Yamada, Yoshiki Masuyama, Yukoh Wakabayashi, and Nobutaka Ono
    "Simultaneous Frequency Estimation for Three or More Sinusoids Based on Sinusoidal
    Constraint Differential Equation," Proceedings of 2022 APSIPA ASC, pp. 975-978.
    https://ieeexplore.ieee.org/document/9980228

    Args:
        signal (np.ndarray): Input discrete-time signal.
        fs (float): Sampling frequency in Hz.
        n_sinusoids (int): Number of Sinusoids.

    Returns:
        np.ndarray: An array of estimated frequencies in Hz, sorted in ascending order.
                    Returns an empty array if no valid frequencies are found.
    """
    dt = 1.0 / fs

    # Determine the maximum padding required (for the highest derivative)
    # This ensures all derivatives are trimmed to the same length and time segment.
    # For 6th order derivative (k_order=6), pad_len is 3
    max_pad_len = 2 * n_sinusoids

    # Construction of the Covariance Matrix 'B' and Right-Hand Side Vector 'V'
    # For n=3 sinusoids, the system of equations for alphas (alpha1, alpha2, alpha3)
    # corresponds to equation (11) in the paper.
    # The notation S_a,b in the paper's matrix elements corresponds to the sum of
    # (2a)-th derivative * (2b)-th derivative.

    # Construct the coefficient matrix B (from paper's Eq. 11)
    b_mat = np.zeros((n_sinusoids, n_sinusoids), dtype=np.float64)
    for m in range(n_sinusoids):
        for n in range(n_sinusoids):
            b_mat[m, n] = calc_covmat_mn(
                n_sinusoids - 1 - n, n_sinusoids - 1 - m, signal, dt, max_pad_len
            )

    # Calculate individual elements of the V vector (right-hand side)
    rhs = np.zeros(n_sinusoids, dtype=np.float64)
    for i in range(n_sinusoids):
        rhs[i] = calc_covmat_mn(
            n_sinusoids, n_sinusoids - 1 - i, signal, dt, max_pad_len
        )

    # Solving the system of linear equations
    # e.g., B * [alpha1, alpha2, alpha3]^T = -V
    try:
        alphas = np.linalg.solve(b_mat, -rhs).astype(np.float64)
    except np.linalg.LinAlgError:
        print("Warning: Failed to solve the linear system (e.g., singular matrix).")
        return np.array([])

    # Solving the polynomial equation for roots
    omegas_squared_complex = solve_poly(alphas, n_sinusoids)

    estimated_frequencies_hz = []
    for omega_sq in omegas_squared_complex:
        # Ω (omega_sq) should ideally be real and positive,
        # as it represents ω^2 (squared angular frequency).

        # Small imaginary parts or negative real parts might be due to
        # numerical errors or noise.
        if np.isreal(omega_sq) and omega_sq.real >= 0:
            # Convert angular frequency (rad/s) to Hz
            frequency_hz = np.sqrt(omega_sq.real) / (2 * np.pi)
            estimated_frequencies_hz.append(frequency_hz)

    return np.array(sorted(estimated_frequencies_hz))


def solve_for_one_omega(
    target_m_order: int,
    fixed_omegas_sq: npt.NDArray[np.float64],
    fixed_m_orders: npt.NDArray[np.int64],
    cov_mat: npt.NDArray[np.float64],
) -> np.float64 | None:
    """Solve dJ/dω = 0 for one ω, fixing the ω of the other sources.

    Args:
        target_m_order (int): 最適化対象の音源の倍音次数 (M_n).
        fixed_omegas_sq (np.ndarray): 固定された音源のω^2の配列.
        fixed_m_orders (np.ndarray): 固定された音源の倍音次数の配列.
        cov_mat (np.ndarray): 事前計算された共分散行列.

    Returns:
        float or None: 最適化されたω^2. 解が見つからない場合は None.
    """
    # 微分作用素を「固定部分」と「可変部分」に分ける
    # 微分作用素 L(p) のうち、現在定数と見なしている周波数 fixed_omegas_sq から
    # 作られる部分 P_fixed(p) を計算
    p_fixed_coeffs = np.array([1.0])
    if fixed_omegas_sq.size > 0:
        for i, omega_sq in enumerate(fixed_omegas_sq):
            m_order = fixed_m_orders[i]
            for m in range(1, m_order + 1):
                p_fixed_coeffs = poly.polymul(p_fixed_coeffs, [m**2 * omega_sq, 1])

    # 目的関数Jにおいて，多項式の係数を代数的に解くのは困難
    # -> J(Ω_t) の値を、いくつかのΩ_tのサンプル点で実際に計算する
    poly_degree_j = 2 * target_m_order  # JはΩ_tの 2 * target_m_order 次の多項式
    n_sample_points = poly_degree_j + 1  # N次多項式は、N+1個の点が決まれば一意に定まる
    min_freq_hz, max_freq_hz = 50, 1500  # サンプリングする周波数範囲（広め）
    min_omega_sq, max_omega_sq = (
        (min_freq_hz * 2 * np.pi) ** 2,
        (max_freq_hz * 2 * np.pi) ** 2,
    )
    # 評価用のサンプル点
    omega_sq_samples = np.linspace(min_omega_sq, max_omega_sq, n_sample_points)

    # 各サンプル点で目的関数 J の値を計算する
    j_values = np.zeros(n_sample_points)
    for k, omega_t_sq in enumerate(omega_sq_samples):
        p_target_coeffs = np.array([1.0])
        for m in range(1, target_m_order + 1):
            # 微分作用素の多項式（一次式）を次々と掛けて，係数を保持
            p_target_coeffs = poly.polymul(p_target_coeffs, [m**2 * omega_t_sq, 1])
        # 当該サンプル点における作用素 L(p) の係数 gamma_coeffs を取得
        gamma_coeffs = poly.polymul(p_fixed_coeffs, p_target_coeffs)
        # 論文の式(21)を計算
        j_values[k] = gamma_coeffs @ cov_mat @ gamma_coeffs

    # サンプル点から J の多項式を復元 -> (poly_degree_j)次の多項式フィッティング
    beta_coeffs = poly.polyfit(omega_sq_samples, j_values, poly_degree_j)

    # 多項式の最小値を見つける（Jを微分して0と置いた式の根を求める）
    dj_coeffs_ascending = np.zeros(poly_degree_j)
    for k in range(1, poly_degree_j + 1):
        dj_coeffs_ascending[k - 1] = k * beta_coeffs[k]  # 昇べき
    roots = np.roots(dj_coeffs_ascending[::-1])  # 降べきにして渡す

    # 物理的に妥当な根だけを取る
    # →根が実数かつ正 (ω² > 0)，妥当な範囲内にあるもの
    valid_roots = []
    for r in roots:
        if np.isreal(r) and r.real > 0:
            freq_hz = np.sqrt(r.real) / (2 * np.pi)
            if min_freq_hz <= freq_hz <= max_freq_hz:
                valid_roots.append(r.real)
    if not valid_roots:
        return None

    # 根の候補値を復元した目的関数に代入，実際に値が最も小さくなるものを選択
    j_of_valid_roots = poly.polyval(valid_roots, beta_coeffs)
    valid_root: np.float64 = valid_roots[np.argmin(j_of_valid_roots)]
    return valid_root


def estimate_squared_angular_freqs(
    cov_mat: npt.NDArray[np.float64],
    initial_freqs_hz: tuple[float, np.float64 | float],
    config: ExperimentConfig,
) -> npt.NDArray[np.float64]:
    """Estimate squared angular frequencies.

    Args:
        cov_mat (np.ndarray): covariance matrix.
    """
    # 初期推定値を角周波数の2乗の形に変換
    omegas_sq = (np.array(initial_freqs_hz) * 2 * np.pi) ** 2
    for _ in range(config.max_iter):
        prev_omegas_sq = np.copy(omegas_sq)
        # n番目以外の音源の周波数をfixed_omegas_sqとして固定し，
        # n番目の音源だけを「可変」とする
        for n in range(config.n_sources):
            target_m_order = config.m_orders[n]
            fixed_omegas_sq = np.delete(omegas_sq, n)
            fixed_m_orders = np.delete(config.m_orders, n)
            # 他の音源のωを固定した上で、一つのωについて dJ/dω = 0 を解く
            new_omega_sq = solve_for_one_omega(
                target_m_order, fixed_omegas_sq, fixed_m_orders, cov_mat
            )
            if new_omega_sq is not None:
                omegas_sq[n] = new_omega_sq
        if np.linalg.norm(omegas_sq - prev_omegas_sq) < config.tolerance:
            break
    return omegas_sq


def estimate_multiple_pitch(
    signal: npt.NDArray[np.float64],
    initial_freqs_hz: tuple[float, np.float64 | float],
    config: ExperimentConfig,
) -> npt.NDArray[np.float64]:
    """Estimate multiple fundamental frequencies via coordinate descent.

    Args:
        signal (np.ndarray): Input discrete-time signal.
    """
    dt = 1.0 / config.fs
    max_deriv_order = 2 * sum(config.m_orders)
    if len(signal) < max_deriv_order + 1:
        return np.array([])
    cov_mat_size = sum(config.m_orders) + 1
    cov_mat = np.zeros((cov_mat_size, cov_mat_size))
    for i in range(cov_mat_size):
        for j in range(cov_mat_size):
            cov_mat[i, j] = calc_covmat_mn(i, j, signal, dt, max_deriv_order)
    omegas_sq = estimate_squared_angular_freqs(cov_mat, initial_freqs_hz, config)
    return np.sqrt(omegas_sq) / (2 * np.pi)


def estimate_peak_freq(
    fs: int,
    peaks: npt.NDArray[np.int64],
    fft_size: int,
    fft_mag: npt.NDArray[np.float64],
    freq_bins: npt.NDArray[np.floating],
) -> list[np.float64]:
    """Estimate peaks over frequencies through parabolic interpolation."""
    estimated_peak_freqs = []
    for peak_idx in peaks:
        if 1 <= peak_idx < len(fft_mag) - 1:
            y_minus_1, y_0, y_plus_1 = fft_mag[peak_idx - 1 : peak_idx + 2]
            denominator = y_minus_1 - 2 * y_0 + y_plus_1
            if np.isclose(denominator, 0):
                estimated_peak_freqs.append(freq_bins[peak_idx])
                continue
            delta = 0.5 * (y_minus_1 - y_plus_1) / denominator
            estimated_peak_freqs.append((peak_idx + delta) * fs / fft_size)
        else:
            estimated_peak_freqs.append(freq_bins[peak_idx])
    return estimated_peak_freqs


def estimate_freq_dft_parabolic(
    signal: npt.NDArray[np.float64], fs: int, target_freq: float
) -> np.float64 | float:
    """Estimating frequency using DFT and parabolic interpolation."""
    n = len(signal)
    if n == 0:
        return np.nan

    # FFTを計算（ゼロパディングで解像度を上げる）
    fft_size = max(n, 65536)  # ある程度のFFTサイズを確保
    fft_spectrum = np.fft.rfft(signal * np.hanning(n), n=fft_size)
    fft_mag = np.abs(fft_spectrum)
    freq_bins: npt.NDArray[np.floating] = np.fft.rfftfreq(fft_size, 1 / fs)

    # 複数のピークを検出
    # height: ある程度の大きさを持つピークのみを検出
    # distance: ピークどうしが近すぎないようにする（FFTビン単位）
    peaks, _ = find_peaks(fft_mag, height=np.max(fft_mag) * 0.1, distance=5)
    if len(peaks) == 0:
        return np.nan
    _estimated_peak_freqs = estimate_peak_freq(fs, peaks, fft_size, fft_mag, freq_bins)
    # 検出されたピークの中から、target_freqに最も近いものを選択
    estimated_peak_freqs = np.array(_estimated_peak_freqs).astype(np.float64)
    closest_freq: np.float64 = estimated_peak_freqs[
        np.argmin(np.abs(estimated_peak_freqs - target_freq))
    ]
    return closest_freq


def estimate_freq_reassignment(
    signal: npt.NDArray[np.float64], fs: int, target_freq: float
) -> np.float64 | float:
    """Estimate frequency using Spectral Reassignment."""
    n = len(signal)
    if n == 0:
        return np.nan

    # 2種類のSTFTを計算
    stf = ShortTimeFFT(np.hanning(n), fs=fs, hop=n, mfft=max(n, 65536))
    freq_bins = stf.f
    stft_w = stf.stft(signal)
    stf = ShortTimeFFT(np.arange(n) * np.hanning(n), fs=fs, hop=n, mfft=max(n, 65536))
    stft_tw = stf.stft(signal)

    # 時間次元は1なので消去
    stft_w = stft_w[:, 0]
    stft_tw = stft_tw[:, 0]

    # 補正周波数を計算
    epsilon = 1e-12  # ゼロ除算を避ける
    # 式の(1/2π)と時間窓のサンプル単位を合わせるため、(1/(2π*dt))としたいところだが、
    # 割り算でdtは相殺されるので、以下の形で良い
    reassigned_freqs = freq_bins - np.imag(stft_tw / (2 * np.pi * (stft_w + epsilon)))

    # ピークを検出して、対応する補正周波数を返す
    mag_spec = np.abs(stft_w)
    peaks, _ = find_peaks(mag_spec, height=np.max(mag_spec) * 0.1, distance=5)

    if len(peaks) == 0:
        return np.nan

    # 全てのピーク位置に対応する補正周波数を取得
    estimated_peak_freqs = reassigned_freqs[peaks]

    # ターゲット周波数に最も近いものを選択
    closest_freq: np.float64 = estimated_peak_freqs[
        np.argmin(np.abs(estimated_peak_freqs - target_freq))
    ]
    return closest_freq


def run_experiment_4_2_signal_length() -> None:
    """Reproduce Section 4.2 (Figure 3) of the paper.

    Relationship between signal length and estimation error (comparison of four methods)
    """
    print(
        "--- Running Experiment 4.2: Signal Length vs. Error (4-method comparison) ---"
    )
    config = ExperimentConfig()
    f_true = [200.0, 440.0]
    signal_lengths_ms = np.arange(1.0, 51.0, 1.0)
    errors: dict[str, list[npt.NDArray[np.float64] | float]] = {
        "fohcde": [],
        "scde": [],
        "dft": [],
        "reassignment": [],
    }

    for length_ms in signal_lengths_ms:
        duration = length_ms / 1000.0
        t = np.linspace(0, duration, int(config.fs * duration), endpoint=False)
        signal = sum(
            np.cos(2 * np.pi * m * freq * t)
            for freq in f_true
            for m in range(1, config.m_orders[0] + 1)
        )
        signal = np.array(signal).astype(np.float64)

        # M-FOHCDE
        est = estimate_multiple_pitch(signal, (250.0, 500.0), config)
        errors["fohcde"].append(
            est[np.argmin(np.abs(est - f_true[0]))] - f_true[0]
            if est.size > 0
            else np.nan
        )

        # SCDE
        est = estimate_frequencies_scde(signal, config.fs, config.n_sinusoids_scde)
        errors["scde"].append(
            est[np.argmin(np.abs(est - f_true[0]))] - f_true[0]
            if est.size > 0
            else np.nan
        )

        # DFT
        _est = estimate_freq_dft_parabolic(signal, config.fs, target_freq=f_true[0])
        errors["dft"].append(_est - f_true[0] if not np.isnan(_est) else np.nan)

        # Reassignment
        _est = estimate_freq_reassignment(signal, config.fs, target_freq=f_true[0])
        errors["reassignment"].append(
            _est - f_true[0] if not np.isnan(_est) else np.nan
        )

    plt.figure(figsize=(10, 6))
    plt.plot(
        signal_lengths_ms, np.array(errors["fohcde"]), "o-", label="M-FOHCDE (Proposed)"
    )
    plt.plot(
        signal_lengths_ms, np.array(errors["scde"]), "^-", label="SCDE (Conventional)"
    )
    plt.plot(
        signal_lengths_ms,
        np.array(errors["reassignment"]),
        "x-",
        label="Reassignment (Conventional)",
    )
    plt.plot(
        signal_lengths_ms,
        np.array(errors["dft"]),
        "s-",
        label="DFT + Parabolic (Conventional)",
    )
    plt.title("Experiment 4.2 Reproduction (Fig. 3)")
    plt.xlabel("Signal Length (ms)")
    plt.ylabel("Estimation Error for 200Hz (Hz)")
    plt.grid(True)
    plt.legend()
    plt.xlim(0, 50)
    plt.tight_layout()
    plt.show()


def run_experiment_4_3_frequency_combination() -> None:
    """論文4.3節（図4）の再現：周波数の組み合わせと推定精度."""
    print("--- Running Experiment 4.3: Frequency Combination vs. Accuracy ---")
    config = ExperimentConfig(duration=0.010)
    f1_fixed = 440.0
    f2_range = np.arange(10.0, 1001.0, 10.0).astype(np.float64)
    estimated_f2_list = []

    for _, f2_test in enumerate(f2_range):
        t = np.linspace(
            0, config.duration, int(config.fs * config.duration), endpoint=False
        )
        f_true = [f1_fixed, f2_test]
        signal = np.zeros_like(t).astype(np.float64)
        for freq in f_true:
            for m in range(1, config.m_orders[0] + 1):
                signal += np.cos(2 * np.pi * m * freq * t)

        initial_freqs = (f1_fixed + 20, f2_test + 20)
        estimated_freqs = estimate_multiple_pitch(signal, initial_freqs, config)

        if estimated_freqs.size > 0:
            est_f2 = estimated_freqs[np.argmin(np.abs(estimated_freqs - f2_test))]
            estimated_f2_list.append(est_f2)
        else:
            estimated_f2_list.append(np.nan)

    plt.figure(figsize=(10, 6))
    plt.plot([0, 1000], [0, 1000], "r--", label="Ideal (y=x)")
    plt.scatter(f2_range, estimated_f2_list, s=10, label="Estimated f2")
    plt.scatter(f2_range, 440.0 * np.ones_like(f2_range), s=10, label="Estimated f1")
    plt.title("Experiment 4.3 Reproduction (Fig. 4)")
    plt.xlabel("True Frequency f2 (Hz)")
    plt.ylabel("Estimated Frequency f2 (Hz)")
    plt.grid(True)
    plt.legend()
    plt.xlim(0, 1000)
    plt.tight_layout()
    plt.show()


def generate_signal(
    fs: int, duration: float, config: SignalConfig
) -> npt.NDArray[np.float64]:
    """Generate a mixed signal for the experiment."""
    t = np.arange(0, duration, 1 / fs)

    f_mod = config.f_mod
    f_carrier = config.f_carrier
    f_excursion = config.f_excursion
    f_fixed = config.f_fixed

    f1_t = f_carrier + f_excursion * np.sin(2 * np.pi * f_mod * t)  # 変動する周波数
    phi1_t = np.cumsum(2 * np.pi * f1_t / fs)

    signal_f1 = np.cos(phi1_t) + np.cos(2 * phi1_t)
    signal_f2 = np.cos(2 * np.pi * f_fixed * t) + np.cos(2 * np.pi * 2 * f_fixed * t)
    signal: npt.NDArray[np.float64] = signal_f1 + signal_f2

    return signal


def run_experiment_4_4_time_varying_signal() -> None:
    """論文4.4節（図5）の再現：時間変化する信号への追従性."""
    print("--- Running Experiment 4.4: Tracking of Time-Varying Frequencies ---")
    exp_config = ExperimentConfig(duration=0.1, max_iter=5)

    t = np.arange(0, exp_config.duration, 1 / exp_config.fs)
    sig_config = SignalConfig()
    f1_t = sig_config.f_carrier + sig_config.f_excursion * np.sin(
        2 * np.pi * sig_config.f_mod * t
    )
    signal = generate_signal(exp_config.fs, exp_config.duration, sig_config)

    frame_len_smp = int(exp_config.frame_len_ms / 1000 * exp_config.fs)
    hop_len_smp = int(exp_config.hop_len_ms / 1000 * exp_config.fs)

    time_stamps = []
    estimated_freqs_list = []

    num_frames = (len(signal) - frame_len_smp) // hop_len_smp + 1
    for i in range(num_frames):
        frame = signal[i * hop_len_smp : i * hop_len_smp + frame_len_smp]
        t_center_ms = (i * hop_len_smp + frame_len_smp / 2) / exp_config.fs * 1000
        estimated = estimate_multiple_pitch(
            frame, initial_freqs_hz=(450.0, 510.0), config=exp_config
        )
        if estimated.size > 0:
            time_stamps.append(t_center_ms)
            estimated_freqs_list.append(np.sort(estimated))

    estimated_freqs = np.array(estimated_freqs_list)
    plt.figure(figsize=(10, 6))
    plt.plot(t * 1000, f1_t, "r--", label="True f1(t)")
    plt.axhline(sig_config.f_fixed, color="g", linestyle="--", label="True f2")
    plt.scatter(
        time_stamps, estimated_freqs[:, 0], s=10, alpha=0.7, label="Estimated 1"
    )
    plt.scatter(
        time_stamps, estimated_freqs[:, 1], s=10, alpha=0.7, label="Estimated 2"
    )

    plt.title("Experiment 4.4 Reproduction (Fig. 5)")
    plt.xlabel("Time (ms)")
    plt.ylabel("Frequency (Hz)")
    plt.grid(True)
    plt.legend()
    plt.xlim(0, 100)
    plt.ylim(390, 561)
    plt.tight_layout()
    plt.show()


if __name__ == "__main__":
    # 4.2節：信号長と誤差の関係
    run_experiment_4_2_signal_length()

    # 4.3節：周波数の組み合わせと精度
    run_experiment_4_3_frequency_combination()

    # 4.4節：時間変化する信号への追従性
    run_experiment_4_4_time_varying_signal()