Implement zp2sos.sci in Scilab

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);