How can I write the Matlab “filter”-function myself?

后端 未结 5 1803
攒了一身酷
攒了一身酷 2021-02-06 19:23

I would like to use a Butterworth filter on a 1D-Signal. In Matlab the script would look like this:

 f=100;
 f_cutoff = 20; 
 fnorm =f_cutoff/(f/2);
 [b,a] = but         


        
相关标签:
5条回答
  • 2021-02-06 19:41

    Check my Answer

    filter

        public static double[] filter(double[] b, double[] a, double[] x) {
          double[] filter = null;
          double[] a1 = getRealArrayScalarDiv(a,a[0]);
          double[] b1 = getRealArrayScalarDiv(b,a[0]);
          int sx = x.length;
          filter = new double[sx];
          filter[0] = b1[0]*x[0];
          for (int i = 1; i < sx; i++) {
            filter[i] = 0.0;
            for (int j = 0; j <= i; j++) {
              int k = i-j;
              if (j > 0) {
                if ((k < b1.length) && (j < x.length)) {
                  filter[i] += b1[k]*x[j];
                }
                if ((k < filter.length) && (j < a1.length)) {
                  filter[i] -= a1[j]*filter[k];
                }
              } else {
                if ((k < b1.length) && (j < x.length)) {
                  filter[i] += (b1[k]*x[j]);
                }
              }
            }
          }
          return filter;
        }
    

    conv

        public static double[] conv(double[] a, double[] b) {
          double[] c = null;
          int na = a.length;
          int nb = b.length;
          if (na > nb) {
            if (nb > 1) {
              c = new double[na+nb-1];
              for (int i = 0; i < c.length; i++) {
                if (i < a.length) {
                  c[i] = a[i];
                } else {
                  c[i] = 0.0;
                }
              }
              a = c;
            }
            c = filter(b, new double [] {1.0} , a);
          } else {
            if (na > 1) {
              c = new double[na+nb-1];
              for (int i = 0; i < c.length; i++) {
                if (i < b.length) {
                  c[i] = b[i];
                } else {
                  c[i] = 0.0;
                }
              }
              b = c;
            }
            c = filter(a, new double [] {1.0}, b);
          }
          return c;
        }
    

    deconv

        public static double[] deconv(double[] b, double[] a) {
          double[] q = null;
          int sb = b.length;
          int sa = a.length;
          if (sa > sb) {
            return q;
          }
          double[] zeros = new double[sb - sa +1];
          for (int i =1; i < zeros.length; i++){
            zeros[i] = 0.0;
          }
          zeros[0] = 1.0;
          q = filter(b,a,zeros);
          return q;
        }
    

    deconvRes

        public static double[] deconvRes(double[] b, double[] a) {
          double[] r = null;
          r = getRealArraySub(b,conv(a,deconv(b,a)));
          return r;
        }
    

    getRealArraySub

        public static double[] getRealArraySub(double[] dSub0, double[] dSub1) {
          double[] dSub = null;
          if ((dSub0 == null) || (dSub1 == null)) { 
            throw new IllegalArgumentException("The array must be defined or diferent to null"); 
          }
          if (dSub0.length != dSub1.length) { 
            throw new IllegalArgumentException("Arrays must be the same size"); 
          }
          dSub = new double[dSub1.length];
          for (int i = 0; i < dSub.length; i++) {
            dSub[i] = dSub0[i] - dSub1[i];
          }
          return dSub;
        }
    

    getRealArrayScalarDiv

        public static double[] getRealArrayScalarDiv(double[] dDividend, double dDivisor) {
          if (dDividend == null) {
            throw new IllegalArgumentException("The array must be defined or diferent to null");
          }
          if (dDividend.length == 0) {
            throw new IllegalArgumentException("The size array must be greater than Zero");
          }
          double[] dQuotient = new double[dDividend.length];
    
          for (int i = 0; i < dDividend.length; i++) {
            if (!(dDivisor == 0.0)) {
              dQuotient[i] = dDividend[i]/dDivisor;
            } else {
              if (dDividend[i] > 0.0) {
                dQuotient[i] = Double.POSITIVE_INFINITY;
              }
              if (dDividend[i] == 0.0) {
                dQuotient[i] = Double.NaN;
              }
              if (dDividend[i] < 0.0) {
                dQuotient[i] = Double.NEGATIVE_INFINITY;
              }
            }
          }
          return dQuotient;
        }
    

    Example Using

    Example Using

      double[] a, b, q, u, v, w, r, z, input, outputVector;
    
    
      u = new double [] {1,1,1};
      v = new double [] {1, 1, 0, 0, 0, 1, 1};
      w = conv(u,v);
      System.out.println("w=\n"+Arrays.toString(w));
    
      a = new double [] {1, 2, 3, 4};
      b = new double [] {10, 40, 100, 160, 170, 120};
      q = deconv(b,a);
      System.out.println("q=\n"+Arrays.toString(q));
    
      r = deconvRes(b,a);
      System.out.println("r=\n"+Arrays.toString(r));
    
      a = new double [] {2, -2.5, 1};
      b = new double [] {0.1, 0.1};
      u = new double[31];
      for (int i = 1; i < u.length; i++) {
        u[i] = 0.0;
      }
      u[0] = 1.0;
      z = filter(b, a, u);
      System.out.println("z=\n"+Arrays.toString(z));
    
      a = new double [] {1.0000,-3.518576748255174,4.687508888099475,-2.809828793526308,0.641351538057564};
      b = new double [] { 0.020083365564211,0,-0.040166731128422,0,0.020083365564211};
      input = new double[]{1,2,3,4,5,6,7,8,9};
    
      outputVector = filter(b, a, input);
      System.out.println("outputVector=\n"+Arrays.toString(outputVector));
    

    OUTPUT

    w=
    [1.0, 2.0, 2.0, 1.0, 0.0, 1.0, 2.0, 2.0, 1.0]
    q=
    [10.0, 20.0, 30.0]
    r=
    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    z=
    [0.05, 0.1125, 0.115625, 0.08828125, 0.0525390625, 0.021533203124999997, 6.469726562499979E-4, -0.009957885742187502, -0.012770843505859377, -0.010984611511230471, -0.007345342636108401, -0.003689372539520266, -9.390443563461318E-4, 6.708808243274683E-4, 0.0013081232085824014, 0.0012997135985642675, 9.705803939141337E-4, 5.633686931105333E-4, 2.189206694310998E-4, -8.033509766391922E-6, -1.195022219235398E-4, -1.453610225212288E-4, -1.219501671897661E-4, -7.975719772659323E-5, -3.8721413563358476E-5, -8.523168090901481E-6, 8.706746668052387E-6, 1.5145017380516224E-5, 1.4577898391619086E-5, 1.0649864299265747E-5, 6.023381178272641E-6]
    outputVector=
    [0.020083365564211, 0.11083159422936348, 0.31591188140651166, 0.648466936215357, 1.0993782391344866, 1.6451284697769106, 2.25463601232057, 2.8947248889603028, 3.534126758562552]
    

    Please give me your feedbacks!!

    0 讨论(0)
  • 2021-02-06 19:46

    I implemented filter function used by Matlab in Java :

    The filter function is implemented as a direct form II transposed structure,

    y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb) - a(2)*y(n-1) - ... - a(na+1)*y(n-na)

    where n-1 is the filter order, which handles both FIR and IIR filters [1], na is the feedback filter order, and nb is the feedforward filter order.

    public void filter(double [] b,double [] a, ArrayList<Double> inputVector,ArrayList<Double> outputVector){
            double rOutputY = 0.0;    
            int j = 0;
            for (int i = 0; i < inputVector.size(); i++) {
                if(j < b.length){
                    rOutputY += b[j]*inputVector.get(inputVector.size() - i - 1);
                }
                j++;
            }
            j = 1;
            for (int i = 0; i < outputVector.size(); i++) {
                if(j < a.length){
                    rOutputY -= a[j]*outputVector.get(outputVector.size() - i - 1);
                }
                j++;   
            }
            outputVector.add(rOutputY);
        }
    

    and Here is an example :

    ArrayList<Double>inputVector = new ArrayList<Double>();
    ArrayList<Double>outputVector = new ArrayList<Double>();
    double [] a = new double [] {1.0000,-3.518576748255174,4.687508888099475,-2.809828793526308,0.641351538057564};
    double [] b = new double [] { 0.020083365564211,0,-0.040166731128422,0,0.020083365564211};
    double  []input = new double[]{1,2,3,4,5,6,7,8,9};
    for (int i = 0; i < input.length; i++) {
        inputVector.add(input[i]);
        filter(b, a, inputVector, outputVector);
    }
    System.out.println(outputVector);
    

    and output was :

    [0.020083365564211, 0.11083159422936348, 0.31591188140651166, 0.6484669362153569, 1.099378239134486, 1.6451284697769086, 2.254636012320566, 2.894724888960297, 3.534126758562545]
    

    as in Matlab output

    That's it

    0 讨论(0)
  • 2021-02-06 19:51

    BlackEagle's solution does not reproduce the same results as MATLAB with other arrays. For example:

    b = [0.1 0.1]
    a = [2 -2.5 1]
    u = [1, zeros(1, 30)];
    z = filter(b, a, u)
    

    Gives you completely other results. Be careful.

    0 讨论(0)
  • 2021-02-06 19:52

    I found my mistake. Here's the working code (as a function):

    function filtered = myFilter(b, a, raw)
    
    filtered = zeros(size(raw));
    for c = 1:3
        for n = 9:size(raw,1)
    
            filtered(n,c) = b(1)* raw(n,c)   + b(2)* raw(n-1,c) + b(3)* raw(n-2,c) ...
                          + b(4)* raw(n-3,c) + b(5)* raw(n-4,c) + b(6)* raw(n-5,c) ...
                          + b(7)* raw(n-6,c) + b(8)* raw(n-7,c) + b(9)* raw(n-8,c) ...
                          - a(1)*filtered(n,c)   - a(2)*filtered(n-1,c) - a(3)*filtered(n-2,c) ...
                          - a(4)*filtered(n-3,c) - a(5)*filtered(n-4,c) - a(6)*filtered(n-5,c) ...
                          - a(7)*filtered(n-6,c) - a(8)*filtered(n-7,c) - a(9)*filtered(n-8,c);
        end
    end
    

    Now the filter works nearly fine, but at the first 40 values i've got divergent results. I'll have to figure that out...

    0 讨论(0)
  • 2021-02-06 19:57

    I have found a text described the Direct Form II Transposed used in the Matlab filter function and it works perfectly. See script below. Other implementations are also available but with error of around 1e-15, you'll see this by running the script yourself.

    %% Specification of the Linear Chebysev filters
    clc;clear all;close all
    ord = 5; %System order (from 1 to 5)
    [bq,aq] = cheby1(ord,2,0.2);theta = [bq aq(2:end)]';
    figure;zplane(bq,aq); % Z-Pole/Zeros
    u = [ones(40,1); zeros(40,1)];
    %% Naive implementation of the basic algorithm
    y0 = filter(bq,aq,u); % Built-in filter
    b = fliplr(bq);a = fliplr(aq);a(end) = [];
    y1 = zeros(40,1);pad = zeros (ord,1);
    yp = [pad; y1(:)];up = [pad; u(:)];
    for i = 1:length(u)
        yp(i+ord) = sum(b(:).*up(i:i+ord))-sum(a(:).*yp(i:i+ord-1));
    end
    y1 = yp(ord+1:end); % Naive implementation
    err = y0(:)-y1(:);
    figure
    plot(y0,'r')
    hold on
    plot(y1,'*g')
    xlabel('Time')
    ylabel('Response')
    legend('My code','Built-in filter')
    figure
    plot(err)
    xlabel('Time')
    ylabel('Error')
    %% Direct Form II Transposed  
    % Direct realization of rational transfer functions
    % trps: 0 for direct realization, 1 for transposed realisation 
    % b,a: Numerator and denominator
    % x:   Input sequence
    % y:   Output sequence
    % u:   Internal states buffer
    
    trps = 1;
    b=theta(1:ord+1);
    a=theta(ord+2:end);
    y2=zeros(size(u));
    x=zeros(ord,1);
    %%
    if trps==1
        for i=1:length(u)
            y2(i)=b(1)*u(i)+x(1);
            x=[x(2:ord);0];
            x=x+b(2:end)*u(i)-a*y2(i);
        end
    else
        for i=1:length(u)
            xnew=u(i)-sum(x(1:ord).*a);
            x=[xnew,x];
            y2(i)=sum(x(1:ord+1).*b);
            x=x(1:ord);
        end
    end
    %%
    err = y2 - filter(bq,aq,u);
    figure
    plot(y0,'r')
    hold on
    plot(y2,'*g')
    xlabel('Time')
    ylabel('Response')
    legend('Form II Transposed','Built-in filter')
    figure
    plot(err)
    xlabel('Time')
    ylabel('Error')
    % end
    
    0 讨论(0)
提交回复
热议问题