eteP немного поразмыслив, придумал более простой и точный алгоритм.
Параметры (а точнее, частота настройки) двух полосовых фильтров подстраиваются в зависимости от разницы фаз гармонических колебаний, полученных на выходе этих фильтров в предыдущей итерации (полосовой фильтр сдвигает фазу гармонического сигнала на функцию, зависещую от отношения разницы частот фильтра и подаваемой на него частоты к полосе пропускания фильтра). Это даёт нам возможность по разности фаз этих полосовых фильтров определить отклонение частоты входного сигнала от средней частоты этих фильтров. Фильтры настраиваем на частоту сигнала, а полосы пропускания делаем различающимися в 2 раза. Соответственно, более узкий фильтр будет менять фазу в 2 раза больше широкого. По разнице показаний мгновенных значений фаз сигналов на выходе этих фильтров корректируем среднюю частоту фильтров.
Если на пальцах, то алгоритм такой: 1. Берем выборку для FFT такого размера, чтобы получить частоту с точностью 3 Гц и назовём её Фц. 2. Инициализируем параметры фильтров: полоса пропускания фильтров - 10 и 20 Гц. Оба фильтра настроены на частоту Фц. 3. Подаем входной сигнал на фильтры в течение 100 мсек. 4. Вычисляем мгновенное значение фазы сигнала на выходе первого и второго фильтра, вычисляем их разность (в градусах).
5. Перестраиваем фильтры на частоту, равную разности в градусах, умноженной на коэффициент, скажем, 0.001 6. Переходим на шаг 3
|