eac44c870b146171d49eb996ae1bbc21fe81b80a
[scilab.git] / scilab / modules / signal_processing / macros / filter.sci
1 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
2 // Copyright (C) INRIA - Serge STEER <serge.steer@inria.fr>
3 //
4 // Copyright (C) 2012 - 2016 - Scilab Enterprises
5 //
6 // This file is hereby licensed under the terms of the GNU GPL v2.0,
7 // pursuant to article 5.3.4 of the CeCILL v.2.1.
8 // This file was originally licensed under the terms of the CeCILL v2.1,
9 // and continues to be available under such terms.
10 // For more information, see the COPYING file which you should have received
11 // along with this program.
12
13 function [y, z] = filter(b, a, x, z)
14     //Implements a direct form II transposed implementation of the standard
15     //difference equation
16
17     fname = "filter"
18     [lhs, rhs] = argn(0)
19
20     if rhs < 3 | rhs > 4
21         error(msprintf(_("%s: Wrong number of input arguments: %d to %d expected.\n"), fname, 3, 4));
22     end
23
24     if rhs == 3
25         z = 0;
26     end
27
28     type_b = typeof(b);
29     type_a = typeof(a);
30     type_x = typeof(x);
31     type_z = typeof(z);
32
33     if type_b <> "constant" & type_b <> "polynomial"
34         error(msprintf(_("%s: Wrong type for input argument #%d: Real matrix or polynomial expected.\n"), fname, 1));
35     end
36
37     if type_a <> "constant" & type_a <> "polynomial"
38         error(msprintf(_("%s: Wrong type for input argument #%d: Real matrix or polynomial expected.\n"), fname, 2));
39     end
40
41     if type_x <> "constant"
42         error(msprintf(_("%s: Wrong type for input argument #%d: Real matrix expected.\n"), fname, 3));
43     end
44
45     if type_z <> "constant"
46         error(msprintf(_("%s: Wrong type for input argument #%d: Real matrix expected.\n"), fname, 4));
47     end
48
49     if ~isreal(b)
50         error(msprintf(_("%s: Wrong type for input argument #%d: Real matrix or polynomial expected.\n"), fname, 1));
51     end
52
53     if ~isreal(a)
54         error(msprintf(_("%s: Wrong type for input argument #%d: Real matrix or polynomial expected.\n"), fname, 2));
55     end
56
57     if ~isreal(x)
58         error(msprintf(_("%s: Wrong type for input argument #%d: Real matrix expected.\n"), fname, 3));
59     end
60
61     if ~isreal(z)
62         error(msprintf(_("%s: Wrong type for input argument #%d: Real matrix expected.\n"), fname, 4));
63     end
64
65     if (size(a, "c") <> 1) & (size(a, "r") <> 1)
66         error(msprintf(_("%s: Wrong size for input argument #%d: Vector expected.\n"), fname, 1));
67     end
68
69     if (size(b, "c") <> 1) & (size(b, "r") <> 1)
70         error(msprintf(_("%s: Wrong size for input argument #%d: Vector expected.\n"), fname, 2));
71     end
72
73     if (size(x, "c") <> 1) & (size(x, "r") <> 1)
74         error(msprintf(_("%s: Wrong size for input argument #%d: Vector expected.\n"), fname, 3));
75     end
76
77     if (size(z, "c") <> 1) & (size(z, "r") <> 1)
78         error(msprintf(_("%s: Wrong size for input argument #%d: Vector expected.\n"), fname, 4));
79     end
80
81     // User mixes polynomial and vector notation
82     if type_b == "polynomial" & size(a, "*") <> 1
83         error(msprintf(_("%s: Incompatible input arguments #%d and #%d: a polynomial and 1-by-1 matrix or two polynomials expected.\n"), fname, 1, 2));
84     end
85
86     // User mixes polynomial and vector notation
87     if type_a == "polynomial" & size(b, "*") <> 1
88         error(msprintf(_("%s: Incompatible input arguments #%d and #%d: a polynomial and 1-by-1 matrix or two polynomials expected.\n"), fname, 1, 2));
89     end
90
91     if type_b == "polynomial" | type_a == "polynomial"
92         c = b/a;
93         b = numer(c);
94         a = denom(c);
95         deg_b = degree(b);
96         deg_a = degree(a);
97         deg = max(deg_b, deg_a);
98         b = coeff(b, deg:-1:0);
99         a = coeff(a, deg:-1:0);
100     end
101
102     ////remove high order coefficients equal to zero
103     //i = 0; while b($ - i) == 0, i = i + 1; end;
104     //b = b(1:$ - i);
105
106     ////remove high order coefficients equal to zero
107     //i = 1; while a(i) == 0, i = i + 1; end
108     //a = a(i:$);
109
110     if a(1) == 0
111         error(msprintf(_("%s: Wrong value for input argument #%d: First element must not be %s.\n"), fname, 2, "0"));
112     end
113
114     //force vector orientation
115     b   = matrix(b, -1, 1);
116     a   = matrix(a, -1, 1);
117     mnx = size(x);
118     x   = matrix(x, 1, -1);
119
120     //normalize
121     b = b / a(1);
122     a = a / a(1);
123
124     n = max(size(b, "*"), size(a, "*"))-1;
125     if n > 0 then
126         if argn(2) < 4 then
127             z = zeros(n, 1);
128         else
129             z = matrix(z, n, 1);
130         end
131
132         //pad the numerator and denominator if necessary
133         a($ + 1:(n + 1)) = 0;
134         b($ + 1:(n + 1)) = 0;
135
136         //form state space representation
137         A     = [-a(2:$), [eye(n - 1, n - 1); zeros(1, n - 1)] ];
138         B     = b(2:$) - a(2:$) * b(1); //C = eye(1, n); D = b(1);
139
140         [z, X] = ltitr(A, B, x, z);
141         y     = X(1, :) + b(1) * x;
142
143     else
144         y     = b(1) * x;
145         z     = [];
146     end
147     //make y orientation similar to the x one
148     y = matrix(y, mnx);
149
150 endfunction
151