Использование фильтра Фарроу на основе интерполяции Лагранжа

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "dspl.h"

int main(int argc, char* argv[])
{
  double t0[8] = {0.0, 1.0, 2.0, 3.0,  4.0,  5.0,  6.0,  7.0}; /* время             */
  double s0[8] = {1.0, 2.0, 2.0, 1.0, -0.5, -1.0, -2.0, -0.5}; /* исходный сигнал   */
  double *y = NULL;
  int    i, ny;
  
  void* hdspl; /* DSPL handle        */
  
  /* Загружаем libdspl.dll */
  hdspl = dspl_load();
  if(!hdspl)
  {
    printf("Cannot to load libdspl!\n");
    return 0;
  }
  
  
  farrow_lagrange(s0, 8, 1.0, 1.0, 0.25, &y, &ny);
  for(i = 0; i < ny; i++)
    printf("y[%d] = %+-6.3f\n", i, y[i]);


  /* Выгружаем libdspl.dll из памяти */
  dspl_free(hdspl);

  return 0;
}

# -*- coding: utf-8 -*-
"""
Created on Sun Aug 27 23:40:00 2017

@author: sergey
"""

import numpy as np

p = 1
q = 1
x0 = 0.25



s0 = np.array([1., 2., 2., 1., -0.5, -1., -2., -0.5])

tin = np.linspace(0.0, len(s0), len(s0), endpoint=False)

y = np.zeros(int(np.floor( float(len(s0)*q)/float(p))))

s = np.concatenate((np.array([0., 0.]), s0, np.array([0., 0.])))


print('---------------------------------------------------------', end='');
print('-------------------------------------------------------\n', end='');
print('k     x_k    n        d    s(n-3)    s(n-2)   s(n-1)    s(n)     ', end='');
print('a_0       a_1       a_2       a_3       y(k)\n', end='');
print('---------------------------------------------------------', end='');
print('-------------------------------------------------------\n', end='');

tout = np.zeros(int(np.floor( float(len(s0)*q)/float(p))))
for k  in range(len(y)):
    x =  k*q/p - x0       
    n =  int(x) + 1 + 3 
    d =  int(x) + 1 - x 

    tout[k] = x;

    a0 = s[n-1]
    a3 = 1/6 * (s[n] - s[n-3]) + 0.5*(s[n-2] - s[n-1]);
    a1 = 0.5 * (s[n] - s[n-2]) - a3
    a2 = s[n] - s[n-1] - a3 - a1
    
    y[k] = a0 - a1 * d + a2*d**2 - a3*d**3
    
    print('%(d0)d  %(f0)7.2f   %(d1)2d   %(f1)7.2f'  % 
        {'d0': k, 'f0': x, 'd1': n,  'f1': d}, end='')
    print('%(s0)8.1f %(s1)8.1f %(s2)8.1f %(s3)8.1f ' % 
        {'s0': s[n-3],'s1': s[n-2], 's2': s[n-1], 's3':s[n]}, end='')
  
    print('%(a0)9.3f %(a1)9.3f %(a2)9.3f %(a3)9.3f '% 
        {'a0': a0, 'a1': a1, 'a2': a2, 'a3': a3}, end='')
    print('%(y)9.4f\n' % {'y': y[k]}, end='')
 


print('---------------------------------------------------------', end='');
print('-------------------------------------------------------\n', end='');

"""
figure; stem(tin, s0); 
hold on;  
stem(tout, y, 'r'); 
axis([-0.5, 7.5, -2.5, 2.5]);
grid on;

"""

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "dspl.h"

int main(int argc, char* argv[])
{
  double t0[8] = {0.0, 1.0, 2.0, 3.0,  4.0,  5.0,  6.0,  7.0}; /* время             */
  double s0[8] = {1.0, 2.0, 2.0, 1.0, -0.5, -1.0, -2.0, -0.5}; /* исходный сигнал   */
  double *y = NULL;
  int    i, ny;
  
  void* hdspl; /* DSPL handle        */
  
  /* Загружаем libdspl.dll */
  hdspl = dspl_load();
  if(!hdspl)
  {
    printf("Cannot to load libdspl!\n");
    return 0;
  }
  
  
  farrow_lagrange(s0, 8, 1.0, 1.0, 0.25, &y, &ny);
  for(i = 0; i < ny; i++)
    printf("y[%d] = %+-6.3f\n", i, y[i]);


  /* Выгружаем libdspl.dll из памяти */
  dspl_free(hdspl);

  return 0;
}

# -*- coding: utf-8 -*-
"""
Created on Sun Aug 27 23:40:00 2017

@author: sergey
"""

import numpy as np

p = 1
q = 1
x0 = 0.25



s0 = np.array([1., 2., 2., 1., -0.5, -1., -2., -0.5])

tin = np.linspace(0.0, len(s0), len(s0), endpoint=False)

y = np.zeros(int(np.floor( float(len(s0)*q)/float(p))))

s = np.concatenate((np.array([0., 0.]), s0, np.array([0., 0.])))


print('---------------------------------------------------------', end='');
print('-------------------------------------------------------\n', end='');
print('k     x_k    n        d    s(n-3)    s(n-2)   s(n-1)    s(n)     ', end='');
print('a_0       a_1       a_2       a_3       y(k)\n', end='');
print('---------------------------------------------------------', end='');
print('-------------------------------------------------------\n', end='');

tout = np.zeros(int(np.floor( float(len(s0)*q)/float(p))))
for k  in range(len(y)):
    x =  k*q/p - x0       
    n =  int(x) + 1 + 3 
    d =  int(x) + 1 - x 

    tout[k] = x;

    a0 = s[n-1]
    a3 = 1/6 * (s[n] - s[n-3]) + 0.5*(s[n-2] - s[n-1]);
    a1 = 0.5 * (s[n] - s[n-2]) - a3
    a2 = s[n] - s[n-1] - a3 - a1
    
    y[k] = a0 - a1 * d + a2*d**2 - a3*d**3
    
    print('%(d0)d  %(f0)7.2f   %(d1)2d   %(f1)7.2f'  % 
        {'d0': k, 'f0': x, 'd1': n,  'f1': d}, end='')
    print('%(s0)8.1f %(s1)8.1f %(s2)8.1f %(s3)8.1f ' % 
        {'s0': s[n-3],'s1': s[n-2], 's2': s[n-1], 's3':s[n]}, end='')
  
    print('%(a0)9.3f %(a1)9.3f %(a2)9.3f %(a3)9.3f '% 
        {'a0': a0, 'a1': a1, 'a2': a2, 'a3': a3}, end='')
    print('%(y)9.4f\n' % {'y': y[k]}, end='')
 


print('---------------------------------------------------------', end='');
print('-------------------------------------------------------\n', end='');

"""
figure; stem(tin, s0); 
hold on;  
stem(tout, y, 'r'); 
axis([-0.5, 7.5, -2.5, 2.5]);
grid on;

"""

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "dspl.h"

#define N 200

#define P 10
#define Q 1




int main(int argc, char* argv[])
{
  double t0[8] = {0.0, 1.0, 2.0, 3.0,  4.0,  5.0,  6.0,  7.0}; /* время             */
  double s0[8] = {1.0, 2.0, 2.0, 1.0, -0.5, -1.0, -2.0, -0.5}; /* исходный сигнал   */
  double *ti = NULL;
  double *y  = NULL;
  int    i, ny, ni;
  
  double *hi = NULL;
  double  h[5] = {0.0, 0.0, 1.0, 0.0, 0.0};
  
  double w[N], H[N];
  
  
  void* hdspl; /* DSPL handle        */
  void* hplot; /* GNUPLOT handle     */
  
  /* Загружаем libdspl.dll */
  hdspl = dspl_load();
  if(!hdspl)
  {
    printf("Cannot to load libdspl!\n");
    return 0;
  }
  
  for(i = 0; i < 8; i++)
    t0[i] *= (double)P;
  
  writetxt(t0, s0, 8, "dat/resample_lagrange_interp_s.txt");
  farrow_lagrange(s0, 8, P, Q, 0, &y, &ny);
  
  printf("ny = %d\n", ny);
  
  ti = (double*)malloc(ny*sizeof(double));
  linspace(0,ny,ny,DSPL_PERIODIC, ti);
  
  array_scale_lin(ti,   ny,  0, ny-1, 0, 14,  ti);
  array_scale_lin(y,    ny, -2,  2, -2, 4,   y);
  writetxt(ti, y, ny-1, "dat/resample_lagrange_interp_y.cm");
  
 
  farrow_lagrange(h, 5, P, Q, 0, &hi, &ni);
  ti = (double*)realloc(ti, ni*sizeof(double));
  linspace(0,ni,ni,DSPL_PERIODIC, ti);
  writetxt(ti, hi, ni, "dat/resample_lagrange_interp_filter_time.txt");
  
  
  linspace(0, M_PI, N, DSPL_PERIODIC, w);
  filter_freq_resp(hi, NULL, ni-1, w, N, DSPL_FLAG_LOGMAG, H, NULL, NULL);
  writetxt(w, H, N, "dat/resample_lagrange_interp_filter_freq.txt");
  
  
  /* Выгружаем libdspl.dll из памяти */
  dspl_free(hdspl);

  return 0;
}

# -*- coding: utf-8 -*-
"""
Created on Mon Aug 28 23:34:59 2017

@author: sergey
"""

import numpy as np
import scipy.signal as signal

def resampling_lagrange(s, p, q, x0):
    """
    % y = resample_lagrange(s, p, q, x0)
    % Digital resampling by polynomial Lagrange interpolation.
    % Function changes input signal s samplerate to p/q times and adds fractional
    % delay.
    %
    % Input parameters
    %  s   - input signal vector [N x 1];
    %  p   - p paramter of samplarate conversion
    %  q   - q paramter of samplarate conversion
    %  x0  - fractional delay
    %
    % Ouptut parameters
    %  y   - Resampled signal
    % 
    % Author: Sergey Bakhurin (dsplib.org)
    """
    if(p>1):
        if(q==1):
            y = np.zeros(int(float((len(s)-1) * p) / float(q)) + 1)
        else:
           y = np.zeros(int( float(len(s) * p ) / float(q))) 
    else:
        y = np.zeros(int( float(len(s) * p ) / float(q)))
        
        

    t = np.zeros(len(y))
    s = np.concatenate((np.array([0., 0.]), s, np.array([0., 0.])))

    for k in range(len(y)):
        x = k*q/p - x0
        t[k] = x  
        n = int(np.floor(x)) + 4
        d = np.floor(x) + 1 - x  
        a0 = s[n-1]
        a3 = 1/6 * (s[n] - s[n-3]) + 0.5*(s[n-2] - s[n-1])
        a1 = 0.5 * (s[n] - s[n-2]) - a3
        a2 = s[n] - s[n-1] - a3 - a1  
        y[k] = a0 - a1 * d + a2*d**2 - a3*d**3  
    return y



p = 10
q = 1
x0 = 0

"""
Пример интерполяции сигнала с использованием фильтра Фарроу
"""
s = np.array([1., 2., 2., 1., -0.5, -1., -2., -0.5])

t = np.linspace(0, len(s)*p, len(s), endpoint = False, dtype = 'float64')
np.savetxt("dat/resample_lagrange_interp_s.txt", np.transpose([t, s]), fmt="%+.9e")

y = resampling_lagrange(s, p, q, x0)

t = np.linspace(0, len(y), len(y), endpoint = False, dtype = 'float64')
np.savetxt("dat/resample_lagrange_interp_y.txt", np.transpose([t, y]), fmt="%+.9e") 







"""
Расчет импульсной характеристики h[k] и частотной характеристики H(w)
"""

s = np.array([0., 0., 1., 0., 0.])

h = resampling_lagrange(s, p, q, x0)

t = np.linspace(0, len(h), len(h), endpoint = False, dtype = 'float64')
np.savetxt("dat/resample_lagrange_interp_filter_time.txt", np.transpose([t, h]), fmt="%+.9e")

w, H = signal.freqz(h)
H = 20.0 * np.log10(np.abs(H))
np.savetxt("dat/resample_lagrange_interp_filter_freq.txt", np.transpose([w, H]), fmt="%+.9e")









    
    
    

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "dspl.h"

#define N 54
#define P 20
#define Q 11
#define FS 26.4
#define L 10
#define F0 6.0

int main(int argc, char* argv[])
{
  double tc[N*L], c[N*L], ts[N], s[N];
  double *y=NULL;
  double *ty=NULL;
  int i, ny;
  void* hdspl; /* DSPL handle        */
  
  /* Загружаем libdspl.dll */
  hdspl = dspl_load();
  if(!hdspl)
  {
    printf("Cannot to load libdspl!\n");
    return 0;
  }
  
  
  linspace(0, (double)N/FS, N*L, DSPL_PERIODIC, tc);
  linspace(0, (double)N/FS, N,   DSPL_PERIODIC, ts);
  
  
  for(i = 0; i < N; i++)
    s[i] = sin(M_2PI * F0 * ts[i]);

  for(i = 0; i < N*L; i++)
    c[i] = sin(M_2PI * F0 * tc[i]);
  
  farrow_lagrange(s, N, P, Q, 0, &y, &ny);
  
  ty = (double*)malloc(ny*sizeof(double));
  for(i = 0; i < ny; i++)
    ty[i] = (double)(Q * i) / FS / (double)P;
  
  
  array_scale_lin(ts,   N,  0, 2,  0, 16,  ts);
  array_scale_lin( s,   N, -1, 1, -2,  4,   s); 
  writetxt(ts, s,       N, "dat/resample_lagrange_ex_fs_s.cm"); 

  array_scale_lin(tc,   N*L,  0, 2,  0, 16,  tc);
  array_scale_lin(c,    N*L, -1, 1, -2,  4,   c);
  writetxt(tc, c, (N-1)*L, "dat/resample_lagrange_ex_fs_c.cm");
  
  array_scale_lin(ty,   ny,  0, 2,  0, 16,  ty);
  array_scale_lin(y,    ny, -1, 1, -2,  4,   y);
  writetxt(ty, y,      ny, "dat/resample_lagrange_ex_fs_y.cm");
  
  
  if(y)
    free(y);
  if(ty)
    free(ty);
  
  /* Выгружаем libdspl.dll из памяти */
  dspl_free(hdspl);

  return 0;
}

# -*- coding: utf-8 -*-
"""
Created on Sat Sep  2 12:18:52 2017

@author: sergey
"""

# -*- coding: utf-8 -*-
"""
Created on Mon Aug 28 23:34:59 2017

@author: sergey
"""

import numpy as np
import scipy.signal as signal

def resampling_lagrange(s, p, q, x0):
    """
    % y = resample_lagrange(s, p, q, x0)
    % Digital resampling by polynomial Lagrange interpolation.
    % Function changes input signal s samplerate to p/q times and adds fractional
    % delay.
    %
    % Input parameters
    %  s   - input signal vector [N x 1];
    %  p   - p paramter of samplarate conversion
    %  q   - q paramter of samplarate conversion
    %  x0  - fractional delay
    %
    % Ouptut parameters
    %  y   - Resampled signal
    % 
    % Author: Sergey Bakhurin (dsplib.org)
    """
    if(p>1):
        if(q==1):
            y = np.zeros(int(float((len(s)-1) * p) / float(q)) + 1)
        else:
           y = np.zeros(int( float(len(s) * p ) / float(q))) 
    else:
        y = np.zeros(int( float(len(s) * p ) / float(q)))
        
        

    t = np.zeros(len(y))
    s = np.concatenate((np.array([0., 0.]), s, np.array([0., 0.])))

    for k in range(len(y)):
        x = k*q/p - x0
        t[k] = x  
        n = int(np.floor(x)) + 4
        d = np.floor(x) + 1 - x  
        a0 = s[n-1]
        a3 = 1/6 * (s[n] - s[n-3]) + 0.5*(s[n-2] - s[n-1])
        a1 = 0.5 * (s[n] - s[n-2]) - a3
        a2 = s[n] - s[n-1] - a3 - a1  
        y[k] = a0 - a1 * d + a2*d**2 - a3*d**3  
    return y



P = 20
Q = 11
N = 54
L = 10
Fs = 26.4
f0 = 6.0

tc = np.linspace(0, N*L, N*L, endpoint = False, dtype = 'float64')/(Fs * float(L))
c = np.sin(np.pi*2.0*f0*tc)

ts = np.linspace(0, N, N, endpoint = False, dtype = 'float64')/Fs
s = np.sin(np.pi*2.0*f0*ts)

y = resampling_lagrange(s, P, Q, 0.0)
ty = float(Q) * np.linspace(0, len(y), len(y), endpoint = False, dtype = 'float64') / (Fs * float(P))

np.savetxt("dat/resample_lagrange_ex_fs_c.txt", np.transpose([tc[0:(N-1)*L], c[0:(N-1)*L] ]), fmt="%+.9e")
np.savetxt("dat/resample_lagrange_ex_fs_s.txt", np.transpose([ts, s]), fmt="%+.9e")
np.savetxt("dat/resample_lagrange_ex_fs_y.txt", np.transpose([ty, y]), fmt="%+.9e")