latcfilt.c

matlabDigitalSigalProcess内有文件若干

/*
 * latcfilt.c
 *
 * LATCFILT Lattice and lattice-ladder filter implementation.
 *    [F,G] = LATCFILT(K,X) filters X with the FIR lattice coefficients
 *    in vector K.  F is the forward lattice filter result, and G is the
 *    backward filter result.
 * 
 *    If K and X are vectors, the result is a (signal) vector.
 *    Matrix arguments are permitted under the following rules:
 *    - If X is a matrix and K is a vector, each column of X is processed
 *      through the lattice filter specified by K.
 *    - If X is a vector and K is a matrix, each column of K is used to
 *      filter X, and a signal matrix is returned.
 *    - If X and K are both matrices with the same # of columns, then the
 *      i-th column of K is used to filter the i-th column of X.  A
 *      signal matrix is returned.
 * 
 *    [F,G] = LATCFILT(K,V,X) filters X with the IIR lattice
 *    coefficients K and ladder coefficients V.  K and V must be
 *    vectors, while X may be a signal matrix.
 * 
 *    [F,G] = LATCFILT(K,1,X) filters X with the IIR all-pole lattice
 *    specified by K.  K and X may be vectors or matrices according to
 *    the rules given for the FIR lattice.
 *
 *
 *   See also FILTER, TF2LATC, LATC2TF.
 *
 *   D. Orofino, May 1996
 */

/* $Revision: 1.13 $ */
/* Copyright (c) 1988-98 by The MathWorks, Inc. */

#include <math.h>
#include <string.h> /* For memset() */
#include "mex.h"

#define H_IN   prhs[0]
#define V_IN   prhs[1]
#define X_IN   prhs[2]
#define F_OUT  plhs[0]
#define G_OUT  plhs[1]

#ifndef max
#define	max(A, B) ((A)>(B) ? (A) : (B))
#endif

static void Real_FIR_Lattice(
    double F[],  double G[], unsigned int Ly,  unsigned int ky, 
    double              x[], const unsigned int Lx, const unsigned int kx, 
    double              h[], const unsigned int Lh, const unsigned int kh, 
    double D[]
)
{
    double  *orig_x  = x;
    double  *orig_h  = h;
    const unsigned int orig_Ly = Ly;

    /*
     * Compute each column of filter output:
     */
    while (ky-- != 0) {
	double *x_pastend;

	/* Reset for next column: */
	if (kx == 1) x = orig_x;
	if (kh == 1) h = orig_h;
	Ly = orig_Ly;

	/* Determine end of input data column.
	 * Note that this relies on the "new" x pointer, so keep
	 * this after the "x=orig_x" line above.  Also, we do not
	 * want to recompute this in the sample loop below:
	 */
	x_pastend = x + Lx;   /* End of input data column       */

	/* Clear the delay buffers: */

	memset((void *)D, 0, Lh*sizeof(double));

	/* Loop over output samples: */
	while (Ly-- != 0) {
	    /*
	     * Purely-real sample loop:
	     */
	    double fout, gout;
	    double *Dp = D;            /* Reset to start of delay buffer */
	    double *Kp = h;            /* Reset to first lattice coeff   */
	    unsigned int stage_cnt = Lh;

	    /* Get next input sample to first-stage: */
	    double gin;
	    double fin = gin = ((x == x_pastend) ? 0.0 : *x++);

	    while (stage_cnt-- != 0) {
		fout = fin + *Dp * *Kp;
		gout = *Dp + fin * *Kp++;
		*Dp++ = gin;
		fin = fout;       /* Setup for next lattice stage */
		gin = gout;
	    }

	    /* Copy outputs: */
	    *F++ = fout;
	    if (G != NULL) *G++ = gout;
	}

	/* Next filter (we've been incrementing a copy of the pointer: */
	h += Lh;

    } /* k (output column) loop */
}

static void Complex_FIR_Lattice(
    double F[], double Fi[],
    double G[], double Gi[], unsigned int Ly, unsigned int ky, 
    double x[], double xi[], const unsigned int Lx, const unsigned int kx, 
    double h[], double hi[], const unsigned int Lh, const unsigned int kh, 
    double D[]
)
{
    double  *orig_x  = x,  *orig_xi = xi;
    double  *orig_h  = h,  *orig_hi = hi;
    const unsigned int orig_Ly = Ly;

    /*
     * Compute each column of filter output:
     */
    while (ky-- != 0) {
	double *x_pastend;

	/* Reset for next column: */
	if (kx == 1) {x=orig_x; xi=orig_xi;}
	if (kh == 1) {h=orig_h; hi=orig_hi;}
	Ly = orig_Ly;

	/* Determine end of input data column.
	 * Note that this relies on the "new" x pointer, so keep
	 * this after the "x=orig_x" line above.  Also, we do not
	 * want to recompute this in the sample loop below:
	 */
	x_pastend = x + Lx;   /* Beyond end of real input data column */

	/* Clear the delay buffers.
	 * Note that D contains BOTH real and imag buffers
	 * (first D then Di), so that D is of length 2*Lh.
	 */
	memset((void *)D, 0, 2*Lh*sizeof(double));

	/* Loop over output samples: */
	while (Ly-- != 0) {
	    /*
	     * Complex sample loop:
	     */
	    double  foutr, fouti, goutr, gouti;
	    double *Dr = D;       /* Reset to start of real buffer */
	    double *Di = D+Lh;    /* Reset to start of imag buffer */
	    double *Kr = h;       /* Reset to first lattice coeff  */
	    double *Ki = hi;
	    unsigned int stage_cnt = Lh;

	    /*
	     * Get next input samples:
	     * NOTE: Always construct a complex input.
	     *       Get imag sample first, so the x==x_pastend comparison
	     *       is executed BEFORE x is updated for real part...
	     */
	    double fini,finr, gini,ginr;
	    fini = gini = ((x == x_pastend) || (xi == NULL)
			   ? 0.0 : *xi++ );
	    finr = ginr = ((x == x_pastend) ? 0.0 : *x++);

	    if (Ki != NULL) {
		/* Complex coeffs, K: */
		while (stage_cnt-- != 0) {
		    double kkr = *Kr++, kki = *Ki++;
		    double ddr = *Dr, ddi = *Di;

		    /* Update delay buffers: */
    		    *Dr++ = ginr; *Di++ = gini;

		    /*
		     * Compute stage outputs:
		     * Don't forget that fout = fin +   D * K,
		     *           whereas gout =   D + fin * conj(K).
		     */
		    foutr = finr +  ddr * kkr -  ddi *   kki;
		    goutr = ddr  + finr * kkr - fini * (-kki);

		    fouti = fini +  ddr *   kki  +  ddi * kkr;
		    gouti = ddi  + finr * (-kki) + fini * kkr;

		    /* Update for next stage: */
		    finr = foutr; fini = fouti;
		    ginr = goutr; gini = gouti;
		}
	    } else {
		/* Real coeffs, K: */
		while (stage_cnt-- != 0) {
		    double kkr = *Kr++;
		    double ddr = *Dr, ddi = *Di;

		    /* Update delay buffers: */
		    *Dr++ = ginr; *Di++ = gini;

		    /* Compute stage outputs: */
		    foutr = finr + ddr * kkr;
		    goutr = ddr + finr * kkr;

		    fouti = fini + ddi * kkr;
		    gouti = ddi + fini * kkr;

		    /* Update for next stage: */
		    finr = foutr; fini = fouti;
		    ginr = goutr; gini = gouti;
		}
	    }

	    /* Copy outputs: */
	    *F++ = foutr;  *Fi++ = fouti;
	    if (G != NULL) {
		*G++ = goutr; *Gi++ = gouti;
	    }
	}

	/* Next filter (we've been incrementing a copy of the pointer): */
	h += Lh;
	if (hi != NULL) {hi += Lh;}

    } /* k (output column) loop */
}


static void Real_IIR_Lattice(
    double F[],  double G[], unsigned int Ly,  unsigned int ky,
    double              x[], const unsigned int Lx, const unsigned int kx,
    double              h[], const unsigned int Lh, const unsigned int kh,
    double              v[], const unsigned int Lv, const unsigned int kv,
    double D[]
)
{
    double  *orig_x  = x;
    double  *orig_h  = h;
    double  *orig_v  = v;
    const unsigned int orig_Ly = Ly;
    const unsigned int Ld = Lh+1;

    /*
     * Compute each column of filter output:
     */
    while (ky-- != 0) {
	double *x_pastend;

	/* Reset for next column: */
	if (kx == 1) x = orig_x;
	if (kh == 1) h = orig_h;
	if (kv == 1) v = orig_v;
	Ly = orig_Ly;

	/* Determine end of input data column.
	 * Note that this relies on the "new" x pointer, so keep
	 * this after the "x=orig_x" line above.  Also, we do not
	 * want to recompute this in the sample loop below:
	 */
	x_pastend = x + Lx;   /* End of input data column       */

	/* Clear the delay buffers: */
	memset((void *)D, 0, Ld*sizeof(double));

	/* Loop over output samples: */
	while (Ly-- != 0) {
	    /*
	     * Purely-real sample loop:
	     */
	    /* NOTE: D is of length Ld = Lh+1 */
	    double *Dp = D+Ld-2;       /* Second to last buffer */
	    double *Kp = h+Lh-1;       /* Last lattice coeff    */

	    /* Get next input sample: */
	    double fin = (x == x_pastend) ? 0.0 : *x++;

	    do {
		fin -= *Dp * *Kp;            /* Setup for next stage */
		*(Dp+1) = *Dp + fin * *Kp--; /* Update buffer value  */
	    } while (--Dp >= D);
	    *D = fin;           /* Update first buffer, g0(n) = f0(n) */

	    /* Copy backward lattice output
	     * No ladder coeffs for backward terms:
	     */
	    if (G != NULL) *G++ = *(D+Ld-1); /* Last buffer sample */

	    /*
	     * Implement ladder filter:
	     */
	    if (Lv == 1) {
		/* Scalar ladder coefficient: */
		*F++ = *D * *v;

	    } else {
		/*
		 * Preset to start of delay buffer and ladder coeffs.
		 * NOTE: There are Ld = Lh+1 delays,
		 *             and Lv <= Ld ladder coeffs.
		 */
		unsigned int taps = Lv;
		double acc = 0.0;
		double *Vp = v;  /* Reset to first ladder coeff */
		Dp = D;          /* Reset to first buffer       */
		while (taps-- != 0) {
		    acc += *Dp++ * *Vp++;
		}
		*F++ = acc; /* Store result, bump output position */
	    }
	}
	/* Next filter (we've been incrementing a copy of the pointer): */
	h += Lh;
	v += Lv;

    } /* end of ky (output column) loop */
}


static void Complex_IIR_Lattice(
    double F[], double Fi[],
    double G[], double Gi[], unsigned int Ly, unsigned int ky,
    double x[], double xi[], const unsigned int Lx, const unsigned int kx,
    double h[], double hi[], const unsigned int Lh, const unsigned int kh,
    double v[], double vi[], const unsigned int Lv, const unsigned int kv,
    double DBuf[]
)
{
    double  *orig_x  = x,  *orig_xi = xi;
    double  *orig_h  = h,  *orig_hi = hi;
    double  *orig_v  = v,  *orig_vi = vi;
    const unsigned int orig_Ly = Ly;
    const unsigned int Ld = Lh+1;

    /*
     * DBuf is a contiguous allocation for the real and imag delay buffers.
     * Make "permanent" pointers to start of each individual buffer:
     */
    double *DPr = DBuf, *DPi = DBuf+Ld;

    /*
     * Compute each column of filter output:
     */
    while (ky-- != 0) {
	double *x_pastend;

	/* Reset for next column: */
	if (kx == 1) {x=orig_x; xi=orig_xi;}
	if (kh == 1) {h=orig_h; hi=orig_hi;}
	if (kv == 1) {v=orig_v; vi=orig_vi;}
	Ly = orig_Ly;

	/* Determine end of input data column.