diff options
| -rw-r--r-- | macros/zp2sos.sci | 287 |
1 files changed, 259 insertions, 28 deletions
diff --git a/macros/zp2sos.sci b/macros/zp2sos.sci index 6bb9426..f299099 100644 --- a/macros/zp2sos.sci +++ b/macros/zp2sos.sci @@ -1,4 +1,135 @@ -function [sos,g] = zp2sos(z,p,k) +function B = ipermute(A, perm) + funcprot(0); + // ipermute : Inverse permute the dimensions of a matrix A. + // B = ipermute(A, perm) returns the array A with dimensions inverted + // according to the permutation vector `perm`. + // Validate the permutation vector + if max(size(perm)) ~= ndims(A) || or(gsort(perm, "g", "i") ~= 1:ndims(A)) + error('Permutation vector must contain unique integers from 1 to ndims(A).'); + end + // Compute the inverse permutation vector + invPerm = zeros(size(perm,1),size(perm , 2)); + for i = 1:max(size(perm)) + invPerm(perm(i)) = i; + end + // Use the permute function with the inverse permutation + B = permute(A, invPerm); +endfunction + +function zsort = cplxpair (z, tol, dim) + funcprot(0); + if (nargin < 1) + error("Invalid inputs"); + end + // default double + realmin = 2.2251e-308 + if (isempty (z)) + zsort = zeros (size (z,1) , size (z,2)); + return; + end + if (nargin < 2 || isempty (tol)) + tol = 100* %eps; + elseif (~ isscalar (tol) || tol < 0 || tol >= 1) + error ("cplxpair: TOL must be a scalar number in the range 0 <= TOL < 1"); + end + nd = ndims (z); + if (nargin < 3) + // Find the first singleton dimension. + sz = size (z); + dim = find (sz > 1, 1); + if isempty(dim) + dim = 1; + end + else + dim = floor (dim); + if (dim < 1 || dim > nd) + error ("cplxpair: invalid dimension DIM"); + end + end + // Move dimension to analyze to first position, and convert to a 2-D matrix. + perm = [dim:nd, 1:dim-1]; + z = permute (z, perm); + sz = size (z); + n = sz(1); + m = prod (sz) / n; + z = matrix (z, n, m); + // Sort the sequence in terms of increasing real values. + [temp, idx] = gsort (real (z), 1 , "i"); + z = z(idx + n * ones (n, 1) * [0:m-1]); + // Put the purely real values at the end of the returned list. + [idxi, idxj] = find (abs (imag (z)) ./ (abs (z) + realmin) <= tol); + // Force values detected to be real within tolerance to actually be real. + z(idxi + n*(idxj-1)) = real (z(idxi + n*(idxj-1))); + //if idxi and idxj are not scalers + if ~isscalar(idxi) then + v = ones(size(idxi,1),size(idxi,2)); + else + v = 1 ; + end + q = sparse ([idxi' idxj'], v, [n m]); + nr = sum (q, 1); + [temp, idx] = gsort (q, 'r','i'); + midx = idx + size (idx,1) * ones (size (idx,1), 1) * [0:size(idx,2)-1]; + z = z(midx); + zsort = z; + // For each remaining z, place the value and its conjugate at the start of + // the returned list, and remove them from further consideration. + for j = 1:m + p = n - nr(j); + for i = 1:2:p + if (i+1 > p) + error ("cplxpair: could not pair all complex numbers"); + end + [v, idx] = min (abs (z(i+1:p,j) - conj (z(i,j)))); + if (v >= tol * abs (z(i,j))) + error ("cplxpair: could not pair all complex numbers"); + end + // For pairs, select the one with positive imaginary part and use it and + // it's conjugate, but list the negative imaginary pair first. + if (imag (z(i,j)) > 0) + zsort([i, i+1],j) = [conj(z(i,j)), z(i,j)]; + else + zsort([i, i+1],j) = [conj(z(idx+i,j)), z(idx+i,j)]; + end + z(idx+i,j) = z(i+1,j); + end + end + // Reshape the output matrix. + zsort = ipermute (matrix (zsort, sz), perm); +endfunction + +function [zc, zr] = cplxreal (z, tol, dim) + funcprot(0); + if (nargin < 1 || nargin > 3) + error("invalid inputs"); + end + if (isempty (z)) + zc = zeros (size (z,1),size(z,2)); + zr = zeros (size (z,1),size(z,2)); + return; + end + if (nargin < 2 || isempty (tol)) + tol = 100 * %eps ; + end + if (nargin >= 3) + zcp = cplxpair(z,tol,dim); + else + zcp = cplxpair (z , tol); + end + nz = max(size (z) ); + idx = nz; + while ((idx > 0) && (zcp(idx) == 0 || (abs (imag (zcp(idx))) ./ abs (zcp(idx))) <= tol)) + zcp(idx) = real (zcp(idx)); + idx = idx - 1; + end + if (pmodulo (idx, 2) ~= 0) + error ("cplxreal: odd number of complex values was returned from cplxpair"); + end + zc = zcp(2:2:idx); + zr = zcp(idx+1:nz); +endfunction + +function [SOS, G] = zp2sos(z, p, k, DoNotCombineReal) //This function converts filter poles and zeros to second-order sections. //Calling Sequence //[sos] = zp2sos(z) @@ -10,7 +141,6 @@ function [sos,g] = zp2sos(z,p,k) //p: column vector //k: real or complex value, default value is 1 //Description -//This is an Octave function. //This function converts filter poles and zeros to second-order sections. //The first and second parameters are column vectors containing zeros and poles. The third parameter is the overall filter gain, the default value of which is 1. //The output is the sos matrix and the overall gain. @@ -20,32 +150,133 @@ function [sos,g] = zp2sos(z,p,k) //ans = // 6 -18 12 1 -2 0 // 1 -3 0 1 0 0 + + if argn(2) < 3 then + k = 1; + end + if argn(2) < 2 then + p = []; + end + + DoNotCombineReal = 0; + + [zc, zr] = cplxreal(z(:)); + [pc, pr] = cplxreal(p(:)); + + nzc = length(zc); + npc = length(pc); + + nzr = length(zr); + npr = length(pr); + + if DoNotCombineReal then + + // Handling complex conjugate poles + for count = 1:npc + SOS(count, 4:6) = [1, -2 * real(pc(count)), abs(pc(count))^2]; + end + + // Handling real poles + for count = 1:npr + SOS(count + npc, 4:6) = [0, 1, -pr(count)]; + end + // Handling complex conjugate zeros + for count = 1:nzc + SOS(count, 1:3) = [1, -2 * real(zc(count)), abs(zc(count))^2]; + end -funcprot(0); -rhs = argn(2) -lhs = argn(1) -if(rhs<1 | rhs>3) -error("Wrong number of input arguments.") -end - select(rhs) - case 1 then - if (lhs<2) - sos=callOctave("zp2sos",z) - else - [sos,g]=callOctave("zp2sos",z) - end - case 2 then - if(lhs<2) - [sos]=callOctave("zp2sos",z,p) - else - [sos,g]=callOctave("zp2sos",z,p) - end - case 3 then - if(lhs<2) - sos=callOctave("zp2sos",z,p,k) - else - [sos,g]=callOctave("zp2sos",z,p,k) - end - end + // Handling real zeros + for count = 1:nzr + SOS(count + nzc, 1:3) = [0, 1, -zr(count)]; + end + + // Completing SOS if needed (sections without pole or zero) + if npc + npr > nzc + nzr then + for count = nzc + nzr + 1 : npc + npr // sections without zero + SOS(count, 1:3) = [0, 0, 1]; + end + else + for count = npc + npr + 1 : nzc + nzr // sections without pole + SOS(count, 4:6) = [0, 0, 1]; + end + end + + else + + // Handling complex conjugate poles + for count = 1:npc + SOS(count, 4:6) = [1, -2 * real(pc(count)), abs(pc(count))^2]; + end + + // Handling pair of real poles + for count = 1:floor(npr / 2) + SOS(count + npc, 4:6) = [1, -pr(2 * count - 1) - pr(2 * count), pr(2 * count - 1) * pr(2 * count)]; + end + + // Handling last real pole (if any) + if pmodulo(npr, 2) == 1 then + SOS(npc + floor(npr / 2) + 1, 4:6) = [0, 1, -pr($)]; + end + + // Handling complex conjugate zeros + for count = 1:nzc + SOS(count, 1:3) = [1, -2 * real(zc(count)), abs(zc(count))^2]; + end + + // Handling pair of real zeros + for count = 1:floor(nzr / 2) + SOS(count + nzc, 1:3) = [1, -zr(2 * count - 1) - zr(2 * count), zr(2 * count - 1) * zr(2 * count)]; + end + + // Handling last real zero (if any) + if pmodulo(nzr, 2) == 1 then + SOS(nzc + floor(nzr / 2) + 1, 1:3) = [0, 1, -zr($)]; + end + + // Completing SOS if needed (sections without pole or zero) + if npc + ceil(npr / 2) > nzc + ceil(nzr / 2) then + for count = nzc + ceil(nzr / 2) + 1 : npc + ceil(npr / 2) // sections without zero + SOS(count, 1:3) = [0, 0, 1]; + end + else + for count = npc + ceil(npr / 2) + 1 : nzc + ceil(nzr / 2) // sections without pole + SOS(count, 4:6) = [0, 0, 1]; + end + end + end + + if ~exists('SOS') then + SOS = [0, 0, 1, 0, 0, 1]; // leading zeros will be removed + end + + // Removing leading zeros if present in numerator and denominator + for count = 1:size(SOS, 1) + B = SOS(count, 1:3); + A = SOS(count, 4:6); + while B(1) == 0 & A(1) == 0 do + A(1) = []; + A($ + 1) = 0; + B(1) = []; + B($ + 1) = 0; + end + SOS(count, :) = [B, A]; + end + + // If no output argument for the overall gain, combine it into the first section. + if argn(1) < 2 then + SOS(1, 1:3) = k * SOS(1, 1:3); + else + G = k; + end endfunction + +//tests +//sos = zp2sos ([]); +//sos = zp2sos ([], []); +//sos = zp2sos ([], [], 2); +//[sos, g] = zp2sos ([], [], 2); +//sos = zp2sos([], [0], 1); +//sos = zp2sos([0], [], 1); +//sos = zp2sos([1,2,3,4,5,6], 2); +//sos = zp2sos([-1-%i, -1+%i], [-1-2*%i, -1+2*%i], 10); |