Фильтр Фарроу для целых чисел

Дмитрий К
Сообщения: 29
Зарегистрирован: 02 мар 2011, 09:48

Фильтр Фарроу для целых чисел

Сообщение Дмитрий К »

Взялся переписать подпрограмму фильтрации Фильтра Фарроу 3-го порядка, позаимствованную с этого сайта. До недавнего времени, она успешно функционировала как есть. Но сейчас появилась задача переписать код для использования только с целыми числами.
Сам фильтр прекрано функционирует (проверено в сверке с double-версией: разнится в 1 бит), но с фильтрующей конструкцией видимо что-то не так. Возможно кто-то из опытных увидит где принципиальная ошибка и подскажет.

Исходная частота дискретизации 4096Гц, частота на выходе Фарроу 1706,56 Гц.
частота основной гармоники сигнала 53.33Гц
Многие данные умножены на константу (специфика работы в fixed point)

Код: Выделить всё


#include <stdio.h>
#include <stdlib.h>
#include <math.h>



#define K 32

short FIX_MPY(short a, short b)
{
	/* shift right one less bit (i.e. 15-1) */
	int c = ((int)a * (int)b) >> 14;
	/* last bit shifted out = rounding-bit */
	b = c & 0x01;
	/* last shift + rounding bit */
	a = (c >> 1) + b;
	return a;
}



unsigned short my_sqrt(  unsigned int arg )
{

  unsigned short  res, i, step;
  static unsigned short  aNew[1]; // to use pointer
  unsigned short  * pNew = aNew; // pointer is used to allow 1 cycle mpy
  unsigned int  sq;

  res  = 0;
  step = 0x8000;
    *pNew = res + step;
  for( i = 0; i < 16; i++ )
  {
    sq  = (( unsigned int)(*pNew))*(( unsigned int)(*pNew));
    if ( sq <= arg )
      res += step;
    *pNew = res + (step>>1);
    step >>= 1;
  }

  return res;
}


// Функция сохранения результатов анализа в текстовый файл для
 //построения графиков.
void saveToTXT(short* s, int n, char* fileName){
    int i;
    FILE *file = fopen(fileName, "w");
    if(!file) return;
    for( i = 0; i < n; i++)
        fprintf(file,"%d\n",s[i]);
    fclose(file);
}

// Функция расчета кубического полинома
// на основе модифицированного фильтра Фарроу
short farrow3(short *y, short x){
short tmp;
    short a3 = ((y[3]-y[0])/6)+((y[1]-y[2])/2);
    short a1 = ((y[3]-y[1])/2)-a3;
    short a2 = y[3]-a3-a1-y[2];
    tmp = FIX_MPY(x, a3)+a2;
    tmp = FIX_MPY(x, tmp)+a1;
    tmp = FIX_MPY(x, tmp)+y[2];
    return tmp;
}




int main()
{
int i, j;
unsigned int x;
    short f = 5333;    //априорная частота основной гармоники
    const unsigned int F0 = 409600; //частота дискретизации исходного сигнала
    unsigned int F1 = f*K;          //частота дискретизации выходного сигнала
    unsigned int N = (K*F0)/F1;     // кол-во исходных отсчётов
   //исходный сигнал с частотой основной гармоники f и содержащий 3-ю гармонику с частотой 3*f
   short y0[76] =                       
    {
    0,5327,10448,15171,19324,22767,25401,27170,28068,28133,27449,26139,24355,
22272,20075,17945,16055,14550,13546,13120,13302,14081,15395,17147,19200,
21391,23543,25468,26989,27941,28190,27635,26218,23928,20801,16919,12407,
7424,2155,-3197,-8425,-13330,-17731,-21474,-24443,-26564,-27810,-28202,
-27807,-26732,-25119,-23135,-20961,-18783,-16777,-15101,-13884,-13219,
-13155,-13699,-14807,-16398,-18347,-20504,-22694,-24732,-26437,-27636,
-28182,-27960,-26894,-24953,-22156,-18565,-14289,-9474
    };
    // выделяю память под результат ресэмплинга
    short* y1 = (short*) malloc(K*sizeof(short));
    short* t1 = (short*) malloc(K*sizeof(short));

    unsigned int t0 = 32768/4096;       //шаг дискретизации входного сигнала
    unsigned int t1 = 32768/(F1/100);   //шаг дискретизации выходного сигнала
 
   
    //фильтрующая конструкция
    // произвожу ресэмплинг
    for(i = 0; i<K; i++){
         j = ((int)i*F0/F1)-1;    // пересчитываю индекс исходного сигнала
        if(j<0) j =0;            // если индекс отрицателььный то приравниваю его к 0
        x = (T1*i - T0*j)/1000; 
        y1[i] = farrow3(y0+j, x);            // фильтр Фарроу
    }

    //сохраняю результат в файлы
    saveToTXT(y0, N, "исходный сигнал.txt");
    saveToTXT(y1, K, "resampling.txt");
    getchar();

return 0;

}
Как можно увидеть, сигнал на выходе получается нескольо искажен. Никак не возму в толк почему...
j пересчитывается с точность как в double-исходнике, помогите правильно x посчитать. Никак не удавалось получить идентичный с double-версией результат для x. Сделал как есть - на выходе что то вразумительное, - но видно, что несовсем(( :roll:
Последний раз редактировалось Дмитрий К 19 авг 2011, 21:22, всего редактировалось 1 раз.

mks
Сообщения: 16
Зарегистрирован: 18 окт 2010, 11:04

Re: Фильтр Фарроу для целых чисел

Сообщение mks »

А как вам вообще удалось откомпилировать эту программу? У вас подряд идут две такие строчки кода:

Код: Выделить всё

    short* t1 = (short*) malloc(K*sizeof(short));
    ...
    unsigned int t1 = 32768/(F1/100);   //шаг дискретизации выходного сигнала
У меня ругается на повторное определение t1.

Дмитрий К
Сообщения: 29
Зарегистрирован: 02 мар 2011, 09:48

Re: Фильтр Фарроу для целых чисел

Сообщение Дмитрий К »

Вот так должно работать

Код: Выделить всё

#include <stdio.h>
#include <stdlib.h>
#include <math.h>



#define K 32

short FIX_MPY(short a, short b)
{
   /* shift right one less bit (i.e. 15-1) */
   int c = ((int)a * (int)b) >> 14;
   /* last bit shifted out = rounding-bit */
   b = c & 0x01;
   /* last shift + rounding bit */
   a = (c >> 1) + b;
   return a;
}



unsigned short my_sqrt(  unsigned int arg )
{

  unsigned short  res, i, step;
  static unsigned short  aNew[1]; // to use pointer
  unsigned short  * pNew = aNew; // pointer is used to allow 1 cycle mpy
  unsigned int  sq;

  res  = 0;
  step = 0x8000;
    *pNew = res + step;
  for( i = 0; i < 16; i++ )
  {
    sq  = (( unsigned int)(*pNew))*(( unsigned int)(*pNew));
    if ( sq <= arg )
      res += step;
    *pNew = res + (step>>1);
    step >>= 1;
  }

  return res;
}


// Функция сохранения результатов анализа в текстовый файл для
 //построения графиков.
void saveToTXT(short* s, int n, char* fileName){
    int i;
    FILE *file = fopen(fileName, "w");
    if(!file) return;
    for( i = 0; i < n; i++)
        fprintf(file,"%d\n",s[i]);
    fclose(file);
}

// Функция расчета кубического полинома
// на основе модифицированного фильтра Фарроу
short farrow3(short *y, short x){
short tmp;
    short a3 = ((y[3]-y[0])/6)+((y[1]-y[2])/2);
    short a1 = ((y[3]-y[1])/2)-a3;
    short a2 = y[3]-a3-a1-y[2];
    tmp = FIX_MPY(x, a3)+a2;
    tmp = FIX_MPY(x, tmp)+a1;
    tmp = FIX_MPY(x, tmp)+y[2];
    return tmp;
}




int main()
{
int i, j;
unsigned int x;
    short f = 5333;    //априорная частота основной гармоники
    const unsigned int F0 = 409600; //частота дискретизации исходного сигнала
    unsigned int F1 = f*K;          //частота дискретизации выходного сигнала
    unsigned int N = (K*F0)/F1;     // кол-во исходных отсчётов
   //исходный сигнал с частотой основной гармоники f и содержащий 3-ю гармонику с частотой 3*f
   short y0[76] =                       
    {
    0,5327,10448,15171,19324,22767,25401,27170,28068,28133,27449,26139,24355,
22272,20075,17945,16055,14550,13546,13120,13302,14081,15395,17147,19200,
21391,23543,25468,26989,27941,28190,27635,26218,23928,20801,16919,12407,
7424,2155,-3197,-8425,-13330,-17731,-21474,-24443,-26564,-27810,-28202,
-27807,-26732,-25119,-23135,-20961,-18783,-16777,-15101,-13884,-13219,
-13155,-13699,-14807,-16398,-18347,-20504,-22694,-24732,-26437,-27636,
-28182,-27960,-26894,-24953,-22156,-18565,-14289,-9474
    };
    // выделяю память под результат ресэмплинга
    short* y1 = (short*) malloc(K*sizeof(short));
    

    unsigned int T0 = 32768/4096;       //шаг дискретизации входного сигнала
    unsigned int T1 = 32768/(F1/100);   //шаг дискретизации выходного сигнала

   
    //фильтрующая конструкция
    // произвожу ресэмплинг
    for(i = 0; i<K; i++){
         j = ((int)i*F0/F1)-1;    // пересчитываю индекс исходного сигнала
        if(j<0) j =0;            // если индекс отрицателььный то приравниваю его к 0
        x = (T1*i - T0*j)/1000; 
        y1[i] = farrow3(y0+j, x);            // фильтр Фарроу
    }

    //сохраняю результат в файлы
    saveToTXT(y0, N, "исходный сигнал.txt");
    saveToTXT(y1, K, "resampling.txt");
    getchar();

return 0;

}
Я исправил ошибку в определении j. По поводу вычисления x все равно не уверен. Но код стал выдавать удобоваримый результат! Правда посравнению с double-версией наблюдается шум на побочных гармоника (незначительный).
Возможно кому-то понадобится сей код для микроконтроллеров без FPU.
Возможно есть шероховатости в коде (конкретно грешу на вычисление x). Буду рад любым коментариям :roll:

ivan219
Сообщения: 61
Зарегистрирован: 09 май 2011, 16:39

Re: Фильтр Фарроу для целых чисел

Сообщение ivan219 »

В вашем коде при определенных условиях X выходит за пределы -1<=X<0 или j ошибочно вычесляется не помню точно.
Вот мой код. Где X всегда в рамках -1<=X<0 и j правильная.

Код: Выделить всё

procedure TForm1.Resempling;
var
  I, J: Integer;
  X, T, Steep: Extended;
begin
 // Размер исходного массива N = Trunc((N1 - 1) * f0 / f1 + 3) + (1)?;
 // Размер выходного массива N1 = Trunc((N - 3) * f1 / f0 + 1);
 Steep := 96000 / 32768;

 for I := 0 to 511 do // N1 - 1
  begin
   T := Steep * I;
   J := Trunc(T);
   X := T - (J + 1);
   SignalOut[I] := Lagrang3(SignalIn, J, X);
   pSignalOut[I] := 50 - Round(SignalOut[I] * 50);
  end;
end;

Дмитрий К
Сообщения: 29
Зарегистрирован: 02 мар 2011, 09:48

Re: Фильтр Фарроу для целых чисел

Сообщение Дмитрий К »

ivan219 писал(а):В вашем коде при определенных условиях X выходит за пределы -1<=X<0 или j ошибочно вычесляется не помню точно.
Вот мой код. Где X всегда в рамках -1<=X<0 и j правильная.

Код: Выделить всё

procedure TForm1.Resempling;
var
  I, J: Integer;
  X, T, Steep: Extended;
begin
 // Размер исходного массива N = Trunc((N1 - 1) * f0 / f1 + 3) + (1)?;
 // Размер выходного массива N1 = Trunc((N - 3) * f1 / f0 + 1);
 Steep := 96000 / 32768;

 for I := 0 to 511 do // N1 - 1
  begin
   T := Steep * I;
   J := Trunc(T);
   X := T - (J + 1);
   SignalOut[I] := Lagrang3(SignalIn, J, X);
   pSignalOut[I] := 50 - Round(SignalOut[I] * 50);
  end;
end;
Здорово! Увидеть бы всё это на чистом С. Всё таки раздел называется С/С++. Но всё же у вас используется плавающая точка.
У меня получается x принимает значения
0
2
4
6
8
.
.
.
64

j принимает значения:
1
3
6
8
11
.
.
.

не при всех состояниях исходного сигнала результат меня удовлетворяет.
Для разных значений к конструкции j = ((int)i*F0/F1) приходится добавлять либо "1" либо "2".
В моём варианте действительно где-то ошибка, но никак не могупонять где.
Дело в том, что при вычислениях с фиксированой точкой x не может попадать в интервал -1..1, так как числа представляются в формате 1.15 (1 бит знака и 15 битов - дробная часть).

ivan219
Сообщения: 61
Зарегистрирован: 09 май 2011, 16:39

Re: Фильтр Фарроу для целых чисел

Сообщение ivan219 »

Чистый С не предоставлю.
Да думаю тут сложного нет.
Цикл и переменные.
Truc отбрасывает дробную часть остаётся целая.
Диапазон Х тут -1...0

Дмитрий К
Сообщения: 29
Зарегистрирован: 02 мар 2011, 09:48

Re: Фильтр Фарроу для целых чисел

Сообщение Дмитрий К »

ivan219 писал(а):Чистый С не предоставлю.
Да думаю тут сложного нет.
Цикл и переменные.
Truc отбрасывает дробную часть остаётся целая.
Диапазон Х тут -1...0
вот, вся загвоздка в том, что как можно представить -1..0 в целых числах?
Напрмиер числа -0,5 прсото нет. Вместо него может быть -16384 (потому что перём число -32768/2). Только вот вопрос: как тогда модифицировать этот цикл (фильтрующий цикл)?

mks
Сообщения: 16
Зарегистрирован: 18 окт 2010, 11:04

Re: Фильтр Фарроу для целых чисел

Сообщение mks »

Пару лет назад делал лагранж 2-го порядка для фиксированной точки. Как я это делал помню плохо :D . Конечный результат приведен на рисунке (схема simulink). Данные 12 бит со знаком, задержка 8 бит. Помню что я умножил delta на 128 и двигаясь по схеме вводил необходимые множители чтобы результат работы фильтра не разваливался, а также округляя промежуточные результаты чтобы не разгонять разрядность до заоблачных высот.
Изображение

Дмитрий К
Сообщения: 29
Зарегистрирован: 02 мар 2011, 09:48

Re: Фильтр Фарроу для целых чисел

Сообщение Дмитрий К »

у меня получается такая петрушка:
функция farrow3 - сам фильтр выдаёт правильный результат (в сравнении с "плавучей" версией). Но сам результат на выходе отличается достаточно сильно (если ошибка в спектре для плавучки составляет 0,00038, то для фиксированой точки 0,011 - это просто пример, чтобы дать понять масштаб проблемы).
Тогда я делаю вывод, что неправильно расчитывается x в этой конструкции

Код: Выделить всё

 //фильтрующая конструкция
    // произвожу ресэмплинг
    for(i = 0; i<K; i++){
        //t1 = T1*i;                // текущий момент дискретизации
        j = ((int)i*F0/F1)-2;    // пересчитываю индекс исходного сигнала
        if(j<0) j =0;                        // если индекс отрицателььный то приравниваю его к 0
        x = (T1*i - T0*j)-2;        // пересчитываю значение x
        //if(x>100) x = 0;
        y1[i] = farrow3(y0+j, x);            // фильтр Фарроу
    
    }

     y1[0] = y1[31]/5;
кстати, строчка y1[0] = y1[1]/100; нужна обязательно, иначе края получаются слишком рваные.

Ответить