Первый заход в алгоритм фазовой демодуляции

Infarh · Infarh · commit 5c04cdb03a2a · 2025-08-08T21:45:07.000+03:00
diff --git a/MathCore.DSP/Samples/Extensions/SampleSI16Ex.cs b/MathCore.DSP/Samples/Extensions/SampleSI16Ex.cs
@@ -1,4 +1,6 @@
-﻿namespace MathCore.DSP.Samples.Extensions;
+﻿using System.Runtime.CompilerServices;
+
+namespace MathCore.DSP.Samples.Extensions;
 
 public static class SampleSI16Ex
 {
@@ -9,6 +11,70 @@ public static class SampleSI16Ex
     /// <returns>Возвращает массив вещественных значений отсчётов демодулированного сигнала</returns>
     public static float[] PhaseDemodulation(this Span<SampleSI16> samples, double f0, double fd)
     {
-        throw new NotImplementedException();
+        if (samples.IsEmpty) return [];
+        if (samples.Length == 1) return [0f];
+
+        var result = new float[samples.Length];
+        result[0] = 0f; // Первый отсчёт всегда 0, так как нет предыдущего для вычисления производной
+        
+        // Вычисляем массив фаз
+        var phases = new float[samples.Length];
+        for (var i = 0; i < samples.Length; i++)
+        {
+            phases[i] = GetPhase(samples[i]);
+        }
+        
+        // Разворачиваем фазы (phase unwrapping)
+        UnwrapPhases(phases);
+        
+        // Вычисляем мгновенную частоту как производную фазы
+        var dt = 1.0 / fd;
+        var scale_factor = (float)(fd / (2.0 * Math.PI));
+        
+        for (var i = 1; i < samples.Length; i++)
+        {
+            // Производная фазы дает мгновенную частоту в рад/с
+            // Делим на 2π для перевода в Гц
+            var instantaneous_frequency = (phases[i] - phases[i - 1]) * scale_factor;
+            
+            // Вычитаем центральную частоту f0
+            result[i] = instantaneous_frequency - (float)f0;
+        }
+        
+        return result;
+    }
+
+    /// <summary>Быстрое вычисление фазы с использованием статических функций SampleSI16</summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    private static float GetPhase(SampleSI16 sample)
+    {
+        // Используем уже оптимизированную функцию GetArg из SampleSI16
+        return SampleSI16.GetArg(sample);
+    }
+
+    /// <summary>Разворачивание фаз - устранение скачков ±2π</summary>
+    private static void UnwrapPhases(Span<float> phases)
+    {
+        if (phases.Length <= 1) return;
+        
+        const float two_pi = (float)(2.0 * Math.PI);
+        const float pi = (float)Math.PI;
+        
+        for (var i = 1; i < phases.Length; i++)
+        {
+            var diff = phases[i] - phases[i - 1];
+            
+            // Приводим разность к диапазону (-π, π]
+            while (diff > pi)
+            {
+                diff -= two_pi;
+                phases[i] -= two_pi;
+            }
+            while (diff <= -pi)
+            {
+                diff += two_pi;
+                phases[i] += two_pi;
+            }
+        }
     }
 }
diff --git a/Tests/MathCore.DSP.Tests/Samples/Extensions/SampleSI16ExTests.cs b/Tests/MathCore.DSP.Tests/Samples/Extensions/SampleSI16ExTests.cs
@@ -0,0 +1,186 @@
+﻿using MathCore.DSP.Samples;
+using MathCore.DSP.Samples.Extensions;
+
+namespace MathCore.DSP.Tests.Samples.Extensions;
+
+[TestClass]
+public class SampleSI16ExTests
+{
+    /// <summary>Тест фазовой демодуляции для пустого массива</summary>
+    [TestMethod]
+    public void PhaseDemodulation_EmptyArray_ReturnsEmpty()
+    {
+        // Arrange
+        var samples = Span<SampleSI16>.Empty;
+        const double f0 = 1000.0;
+        const double fd = 48000.0;
+
+        // Act
+        var result = samples.PhaseDemodulation(f0, fd);
+
+        // Assert
+        Assert.AreEqual(0, result.Length);
+    }
+
+    /// <summary>Тест фазовой демодуляции для массива с одним элементом</summary>
+    [TestMethod]
+    public void PhaseDemodulation_SingleElement_ReturnsZero()
+    {
+        // Arrange
+        var samples = new SampleSI16[] { new(100, 50) }.AsSpan();
+        const double f0 = 1000.0;
+        const double fd = 48000.0;
+
+        // Act
+        var result = samples.PhaseDemodulation(f0, fd);
+
+        // Assert
+        Assert.AreEqual(1, result.Length);
+        Assert.AreEqual(0f, result[0]);
+    }
+
+    /// <summary>Тест фазовой демодуляции для постоянного сигнала</summary>
+    [TestMethod]
+    public void PhaseDemodulation_ConstantSignal_ReturnsNearZeros()
+    {
+        // Arrange - создаем постоянный сигнал
+        var samples = new SampleSI16[]
+        {
+            new(100, 0),
+            new(100, 0),
+            new(100, 0),
+            new(100, 0),
+            new(100, 0)
+        }.AsSpan();
+
+        const double f0 = 1000.0;
+        const double fd = 48000.0;
+
+        // Act
+        var result = samples.PhaseDemodulation(f0, fd);
+
+        // Assert
+        Assert.AreEqual(5, result.Length);
+        Assert.AreEqual(0f, result[0]); // Первый всегда 0
+
+        // Для постоянного сигнала мгновенная частота должна быть близка к нулю
+        // после вычитания центральной частоты результат должен быть близок к -f0
+        for (var i = 1; i < result.Length; i++)
+            Assert.IsTrue(Math.Abs(result[i] + f0) < 50,
+                $"Sample {i}: expected ~{-f0}, got {result[i]}");
+    }
+
+    /// <summary>Тест фазовой демодуляции для синусоидального сигнала с известной частотой</summary>
+    [TestMethod]
+    public void PhaseDemodulation_SinusoidalSignal_ReturnsExpectedFrequency()
+    {
+        // Arrange - создаем синусоидальный сигнал с частотой f_signal
+        const double fd = 48000.0;
+        const double f0 = 1000.0;      // Центральная частота
+        const double f_signal = 1500.0; // Частота сигнала
+        const int samples_count = 200;
+
+        var samples = new SampleSI16[samples_count];
+        var dt = 1.0 / fd;
+
+        for (var i = 0; i < samples_count; i++)
+        {
+            var t = i * dt;
+            var angle = 2.0 * Math.PI * f_signal * t;
+            var amplitude = 100.0;
+
+            samples[i] = new SampleSI16(
+                (sbyte)(amplitude * Math.Cos(angle)),
+                (sbyte)(amplitude * Math.Sin(angle))
+            );
+        }
+
+        // Act
+        var result = samples.AsSpan().PhaseDemodulation(f0, fd);
+
+        // Assert
+        Assert.AreEqual(samples_count, result.Length);
+        Assert.AreEqual(0f, result[0]); // Первый всегда 0
+
+        // Проверяем, что результат близок к ожидаемой частоте (f_signal - f0)
+        var expected_frequency = f_signal - f0;
+        var tolerance = 100.0; // Допуск в Гц
+
+        // Проверяем стабильную часть сигнала (пропускаем начальные образцы)
+        var stable_samples = 0;
+        for (var i = 20; i < result.Length - 20; i++) // Пропускаем края для стабилизации
+            if (Math.Abs(result[i] - expected_frequency) < tolerance)
+                stable_samples++;
+
+        // Проверяем, что большинство образцов дает правильную частоту
+        var expected_stable_count = (result.Length - 40) * 0.8; // 80% образцов должны быть стабильными
+        Assert.IsTrue(stable_samples > expected_stable_count,
+            $"Expected at least {expected_stable_count} stable samples, got {stable_samples}");
+    }
+
+    /// <summary>Тест производительности фазовой демодуляции</summary>
+    [TestMethod]
+    public void PhaseDemodulation_Performance_CompletesQuickly()
+    {
+        // Arrange - большой массив для тестирования производительности
+        const int samples_count = 100_000;
+        var samples = new SampleSI16[samples_count];
+        var random = new Random(42); // Фиксированное семя для воспроизводимости
+
+        for (var i = 0; i < samples_count; i++)
+            samples[i] = new SampleSI16(
+                (sbyte)random.Next(-128, 128),
+                (sbyte)random.Next(-128, 128)
+            );
+
+        const double f0 = 1000.0;
+        const double fd = 48000.0;
+
+        // Act
+        var stopwatch = System.Diagnostics.Stopwatch.StartNew();
+        var result = samples.AsSpan().PhaseDemodulation(f0, fd);
+        stopwatch.Stop();
+
+        // Assert
+        Assert.AreEqual(samples_count, result.Length);
+
+        // Проверяем, что выполняется достаточно быстро (менее 200 мс для 100k образцов)
+        Assert.IsTrue(stopwatch.ElapsedMilliseconds < 200,
+            $"Деmodulation took {stopwatch.ElapsedMilliseconds} ms, expected < 200 ms");
+
+        Console.WriteLine($"Фазовая демодуляция {samples_count} образцов заняла {stopwatch.ElapsedMilliseconds} мс");
+    }
+
+    /// <summary>Тест проверки правильности unwrap фазы</summary>
+    [TestMethod]
+    public void PhaseDemodulation_PhaseUnwrap_WorksCorrectly()
+    {
+        // Arrange - создаем сигнал с плавно изменяющейся фазой, но с перескоками ±2π
+        const double fd = 1000.0;
+        const double f0 = 0.0; // Нулевая центральная частота для простоты
+
+        var samples = new SampleSI16[]
+        {
+            new(100, 0),     // фаза ≈ 0
+            new(71, 71),     // фаза ≈ π/4
+            new(0, 100),     // фаза ≈ π/2  
+            new(-71, 71),    // фаза ≈ 3π/4
+            new(-100, 0),    // фаза ≈ π
+            new(-71, -71),   // фаза ≈ 5π/4 (но с unwrap должна быть ≈ -3π/4)
+            new(0, -100),    // фаза ≈ 3π/2 (но с unwrap должна быть ≈ -π/2)
+            new(71, -71),    // фаза ≈ 7π/4 (но с unwrap должна быть ≈ -π/4)
+        }.AsSpan();
+
+        // Act
+        var result = samples.PhaseDemodulation(f0, fd);
+
+        // Assert
+        Assert.AreEqual(samples.Length, result.Length);
+        Assert.AreEqual(0f, result[0]); // Первый всегда 0
+
+        // Проверяем, что результаты имеют разумные значения без больших скачков
+        for (var i = 1; i < result.Length; i++)
+            Assert.IsTrue(Math.Abs(result[i]) < 1000,
+                $"Sample {i}: frequency {result[i]} Hz seems too high");
+    }
+}
diff --git a/docs/PhaseDemodulation.md b/docs/PhaseDemodulation.md
@@ -0,0 +1,73 @@
+﻿# Фазовая демодуляция для SampleSI16
+
+## Описание
+
+Реализован высокопроизводительный алгоритм фазовой демодуляции радиосигнала для квадратурных отсчетов типа `SampleSI16`. Алгоритм предназначен для использования в системах реального времени.
+
+## Алгоритм
+
+### Входные параметры
+- `samples` - последовательность квадратурных отсчетов (I/Q) типа `SampleSI16`
+- `f0` - центральная частота фазовой модуляции (Гц)
+- `fd` - частота дискретизации (Гц)
+
+### Выходные данные
+- Массив `float[]` с демодулированными значениями мгновенной частоты (Гц)
+
+### Основные этапы
+
+1. **Вычисление фазы** - для каждого квадратурного отсчета вычисляется фаза с помощью `atan2(Q, I)`
+
+2. **Разворачивание фазы (Phase Unwrapping)** - устранение скачков ±2π для получения непрерывной фазы
+
+3. **Вычисление мгновенной частоты** - как производная фазы по времени:
+   ```
+   instantaneous_frequency = (phase[i] - phase[i-1]) * fd / (2π)
+   ```
+
+4. **Вычитание центральной частоты** - для получения демодулированного сигнала:
+   ```
+   result[i] = instantaneous_frequency - f0
+   ```
+
+## Оптимизации
+
+- Использование `MethodImpl(MethodImplOptions.AggressiveInlining)` для критических функций
+- Минимизация выделения памяти (только один выделенный массив для результата и временный массив фаз)
+- Использование оптимизированных функций `SampleSI16.GetArg()` для вычисления фазы
+- Эффективный алгоритм unwrapping фазы с линейной сложностью O(n)
+
+## Производительность
+
+Алгоритм обрабатывает 100 000 образцов за время менее 200 мс на современном оборудовании, что обеспечивает работу в режиме реального времени.
+
+## Пример использования
+
+```csharp
+using MathCore.DSP.Samples;
+using MathCore.DSP.Samples.Extensions;
+
+// Создаем массив квадратурных отсчетов
+var samples = new SampleSI16[]
+{
+    new(100, 0),   // I=100, Q=0
+    new(71, 71),   // I=71, Q=71
+    // ... другие отсчеты
+};
+
+// Параметры демодуляции
+const double f0 = 1000.0;  // Центральная частота 1 кГц
+const double fd = 48000.0; // Частота дискретизации 48 кГц
+
+// Выполняем фазовую демодуляцию
+var demodulated = samples.AsSpan().PhaseDemodulation(f0, fd);
+
+// demodulated[0] всегда равен 0
+// demodulated[1..] содержат мгновенные частоты в Гц
+```
+
+## Ограничения
+
+- Первый элемент результирующего массива всегда равен 0, так как для вычисления производной требуется предыдущий отсчет
+- Точность демодуляции зависит от отношения сигнал/шум входных данных
+- Алгоритм предполагает, что входной сигнал имеет достаточную амплитуду для корректного вычисления фазы
