arm_iir_lattice_f32.c

00001 /* ----------------------------------------------------------------------    
00002 * Copyright (C) 2010-2014 ARM Limited. All rights reserved.    
00003 *    
00004 * $Date:        19. March 2015
00005 * $Revision:    V.1.4.5
00006 *    
00007 * Project:      CMSIS DSP Library    
00008 * Title:        arm_iir_lattice_f32.c    
00009 *    
00010 * Description:  Floating-point IIR Lattice filter processing function.    
00011 *    
00012 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
00013 *  
00014 * Redistribution and use in source and binary forms, with or without 
00015 * modification, are permitted provided that the following conditions
00016 * are met:
00017 *   - Redistributions of source code must retain the above copyright
00018 *     notice, this list of conditions and the following disclaimer.
00019 *   - Redistributions in binary form must reproduce the above copyright
00020 *     notice, this list of conditions and the following disclaimer in
00021 *     the documentation and/or other materials provided with the 
00022 *     distribution.
00023 *   - Neither the name of ARM LIMITED nor the names of its contributors
00024 *     may be used to endorse or promote products derived from this
00025 *     software without specific prior written permission.
00026 *
00027 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00028 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00029 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00030 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
00031 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00032 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00033 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00034 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00035 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00036 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00037 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00038 * POSSIBILITY OF SUCH DAMAGE.    
00039 * -------------------------------------------------------------------- */
00040 
00041 #include "arm_math.h"
00042 
00043 /**    
00044  * @ingroup groupFilters    
00045  */
00046 
00047 /**    
00048  * @defgroup IIR_Lattice Infinite Impulse Response (IIR) Lattice Filters    
00049  *    
00050  * This set of functions implements lattice filters    
00051  * for Q15, Q31 and floating-point data types.  Lattice filters are used in a     
00052  * variety of adaptive filter applications.  The filter structure has feedforward and    
00053  * feedback components and the net impulse response is infinite length.    
00054  * The functions operate on blocks    
00055  * of input and output data and each call to the function processes    
00056  * <code>blockSize</code> samples through the filter.  <code>pSrc</code> and    
00057  * <code>pDst</code> point to input and output arrays containing <code>blockSize</code> values.    
00058     
00059  * \par Algorithm:    
00060  * \image html IIRLattice.gif "Infinite Impulse Response Lattice filter"    
00061  * <pre>    
00062  *    fN(n)   =  x(n)    
00063  *    fm-1(n) = fm(n) - km * gm-1(n-1)   for m = N, N-1, ...1    
00064  *    gm(n)   = km * fm-1(n) + gm-1(n-1) for m = N, N-1, ...1    
00065  *    y(n)    = vN * gN(n) + vN-1 * gN-1(n) + ...+ v0 * g0(n)    
00066  * </pre>    
00067  * \par    
00068  * <code>pkCoeffs</code> points to array of reflection coefficients of size <code>numStages</code>.     
00069  * Reflection coefficients are stored in time-reversed order.    
00070  * \par    
00071  * <pre>    
00072  *    {kN, kN-1, ....k1}    
00073  * </pre>    
00074  * <code>pvCoeffs</code> points to the array of ladder coefficients of size <code>(numStages+1)</code>.     
00075  * Ladder coefficients are stored in time-reversed order.    
00076  * \par    
00077  * <pre>    
00078  *    {vN, vN-1, ...v0}    
00079  * </pre>    
00080  * <code>pState</code> points to a state array of size <code>numStages + blockSize</code>.    
00081  * The state variables shown in the figure above (the g values) are stored in the <code>pState</code> array.    
00082  * The state variables are updated after each block of data is processed; the coefficients are untouched.    
00083  * \par Instance Structure    
00084  * The coefficients and state variables for a filter are stored together in an instance data structure.    
00085  * A separate instance structure must be defined for each filter.    
00086  * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.    
00087  * There are separate instance structure declarations for each of the 3 supported data types.    
00088   *    
00089  * \par Initialization Functions    
00090  * There is also an associated initialization function for each data type.    
00091  * The initialization function performs the following operations:    
00092  * - Sets the values of the internal structure fields.    
00093  * - Zeros out the values in the state buffer.   
00094  * To do this manually without calling the init function, assign the follow subfields of the instance structure:
00095  * numStages, pkCoeffs, pvCoeffs, pState. Also set all of the values in pState to zero. 
00096  *    
00097  * \par    
00098  * Use of the initialization function is optional.    
00099  * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.    
00100  * To place an instance structure into a const data section, the instance structure must be manually initialized.    
00101  * Set the values in the state buffer to zeros and then manually initialize the instance structure as follows:    
00102  * <pre>    
00103  *arm_iir_lattice_instance_f32 S = {numStages, pState, pkCoeffs, pvCoeffs};    
00104  *arm_iir_lattice_instance_q31 S = {numStages, pState, pkCoeffs, pvCoeffs};    
00105  *arm_iir_lattice_instance_q15 S = {numStages, pState, pkCoeffs, pvCoeffs};    
00106  * </pre>    
00107  * \par    
00108  * where <code>numStages</code> is the number of stages in the filter; <code>pState</code> points to the state buffer array;    
00109  * <code>pkCoeffs</code> points to array of the reflection coefficients; <code>pvCoeffs</code> points to the array of ladder coefficients.    
00110  * \par Fixed-Point Behavior    
00111  * Care must be taken when using the fixed-point versions of the IIR lattice filter functions.    
00112  * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.    
00113  * Refer to the function specific documentation below for usage guidelines.    
00114  */
00115 
00116 /**    
00117  * @addtogroup IIR_Lattice    
00118  * @{    
00119  */
00120 
00121 /**    
00122  * @brief Processing function for the floating-point IIR lattice filter.    
00123  * @param[in] *S points to an instance of the floating-point IIR lattice structure.    
00124  * @param[in] *pSrc points to the block of input data.    
00125  * @param[out] *pDst points to the block of output data.    
00126  * @param[in] blockSize number of samples to process.    
00127  * @return none.    
00128  */
00129 
00130 #ifndef ARM_MATH_CM0_FAMILY
00131 
00132   /* Run the below code for Cortex-M4 and Cortex-M3 */
00133 
00134 void arm_iir_lattice_f32(
00135   const arm_iir_lattice_instance_f32 * S,
00136   float32_t * pSrc,
00137   float32_t * pDst,
00138   uint32_t blockSize)
00139 {
00140   float32_t fnext1, gcurr1, gnext;               /* Temporary variables for lattice stages */
00141   float32_t acc;                                 /* Accumlator */
00142   uint32_t blkCnt, tapCnt;                       /* temporary variables for counts */
00143   float32_t *px1, *px2, *pk, *pv;                /* temporary pointers for state and coef */
00144   uint32_t numStages = S->numStages;             /* number of stages */
00145   float32_t *pState;                             /* State pointer */
00146   float32_t *pStateCurnt;                        /* State current pointer */
00147   float32_t k1, k2;
00148   float32_t v1, v2, v3, v4;
00149   float32_t gcurr2;
00150   float32_t fnext2;
00151 
00152   /* initialise loop count */
00153   blkCnt = blockSize;
00154 
00155   /* initialise state pointer */
00156   pState = &S->pState[0];
00157 
00158   /* Sample processing */
00159   while(blkCnt > 0u)
00160   {
00161     /* Read Sample from input buffer */
00162     /* fN(n) = x(n) */
00163     fnext2 = *pSrc++;
00164 
00165     /* Initialize Ladder coeff pointer */
00166     pv = &S->pvCoeffs[0];
00167     /* Initialize Reflection coeff pointer */
00168     pk = &S->pkCoeffs[0];
00169 
00170     /* Initialize state read pointer */
00171     px1 = pState;
00172     /* Initialize state write pointer */
00173     px2 = pState;
00174 
00175     /* Set accumulator to zero */
00176     acc = 0.0;
00177 
00178     /* Loop unrolling.  Process 4 taps at a time. */
00179     tapCnt = (numStages) >> 2;
00180 
00181     while(tapCnt > 0u)
00182     {
00183       /* Read gN-1(n-1) from state buffer */
00184       gcurr1 = *px1;
00185 
00186       /* read reflection coefficient kN */
00187       k1 = *pk;
00188 
00189       /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
00190       fnext1 = fnext2 - (k1 * gcurr1);
00191 
00192       /* read ladder coefficient vN */
00193       v1 = *pv;
00194 
00195       /* read next reflection coefficient kN-1 */
00196       k2 = *(pk + 1u);
00197 
00198       /* Read gN-2(n-1) from state buffer */
00199       gcurr2 = *(px1 + 1u);
00200 
00201       /* read next ladder coefficient vN-1 */
00202       v2 = *(pv + 1u);
00203 
00204       /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */
00205       fnext2 = fnext1 - (k2 * gcurr2);
00206 
00207       /* gN(n)   = kN * fN-1(n) + gN-1(n-1) */
00208       gnext = gcurr1 + (k1 * fnext1);
00209 
00210       /* read reflection coefficient kN-2 */
00211       k1 = *(pk + 2u);
00212 
00213       /* write gN(n) into state for next sample processing */
00214       *px2++ = gnext;
00215 
00216       /* Read gN-3(n-1) from state buffer */
00217       gcurr1 = *(px1 + 2u);
00218 
00219       /* y(n) += gN(n) * vN  */
00220       acc += (gnext * v1);
00221 
00222       /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */
00223       fnext1 = fnext2 - (k1 * gcurr1);
00224 
00225       /* gN-1(n)   = kN-1 * fN-2(n) + gN-2(n-1) */
00226       gnext = gcurr2 + (k2 * fnext2);
00227 
00228       /* Read gN-4(n-1) from state buffer */
00229       gcurr2 = *(px1 + 3u);
00230 
00231       /* y(n) += gN-1(n) * vN-1  */
00232       acc += (gnext * v2);
00233 
00234       /* read reflection coefficient kN-3 */
00235       k2 = *(pk + 3u);
00236 
00237       /* write gN-1(n) into state for next sample processing */
00238       *px2++ = gnext;
00239 
00240       /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */
00241       fnext2 = fnext1 - (k2 * gcurr2);
00242 
00243       /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */
00244       gnext = gcurr1 + (k1 * fnext1);
00245 
00246       /* read ladder coefficient vN-2 */
00247       v3 = *(pv + 2u);
00248 
00249       /* y(n) += gN-2(n) * vN-2  */
00250       acc += (gnext * v3);
00251 
00252       /* write gN-2(n) into state for next sample processing */
00253       *px2++ = gnext;
00254 
00255       /* update pointer */
00256       pk += 4u;
00257 
00258       /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */
00259       gnext = (fnext2 * k2) + gcurr2;
00260 
00261       /* read next ladder coefficient vN-3 */
00262       v4 = *(pv + 3u);
00263 
00264       /* y(n) += gN-4(n) * vN-4  */
00265       acc += (gnext * v4);
00266 
00267       /* write gN-3(n) into state for next sample processing */
00268       *px2++ = gnext;
00269 
00270       /* update pointers */
00271       px1 += 4u;
00272       pv += 4u;
00273 
00274       tapCnt--;
00275 
00276     }
00277 
00278     /* If the filter length is not a multiple of 4, compute the remaining filter taps */
00279     tapCnt = (numStages) % 0x4u;
00280 
00281     while(tapCnt > 0u)
00282     {
00283       gcurr1 = *px1++;
00284       /* Process sample for last taps */
00285       fnext1 = fnext2 - ((*pk) * gcurr1);
00286       gnext = (fnext1 * (*pk++)) + gcurr1;
00287       /* Output samples for last taps */
00288       acc += (gnext * (*pv++));
00289       *px2++ = gnext;
00290       fnext2 = fnext1;
00291 
00292       tapCnt--;
00293 
00294     }
00295 
00296     /* y(n) += g0(n) * v0 */
00297     acc += (fnext2 * (*pv));
00298 
00299     *px2++ = fnext2;
00300 
00301     /* write out into pDst */
00302     *pDst++ = acc;
00303 
00304     /* Advance the state pointer by 4 to process the next group of 4 samples */
00305     pState = pState + 1u;
00306 
00307     blkCnt--;
00308 
00309   }
00310 
00311   /* Processing is complete. Now copy last S->numStages samples to start of the buffer        
00312      for the preperation of next frame process */
00313 
00314   /* Points to the start of the state buffer */
00315   pStateCurnt = &S->pState[0];
00316   pState = &S->pState[blockSize];
00317 
00318   tapCnt = numStages >> 2u;
00319 
00320   /* copy data */
00321   while(tapCnt > 0u)
00322   {
00323     *pStateCurnt++ = *pState++;
00324     *pStateCurnt++ = *pState++;
00325     *pStateCurnt++ = *pState++;
00326     *pStateCurnt++ = *pState++;
00327 
00328     /* Decrement the loop counter */
00329     tapCnt--;
00330 
00331   }
00332 
00333   /* Calculate remaining number of copies */
00334   tapCnt = (numStages) % 0x4u;
00335 
00336   /* Copy the remaining q31_t data */
00337   while(tapCnt > 0u)
00338   {
00339     *pStateCurnt++ = *pState++;
00340 
00341     /* Decrement the loop counter */
00342     tapCnt--;
00343   }
00344 }
00345 
00346 #else
00347 
00348 void arm_iir_lattice_f32(
00349   const arm_iir_lattice_instance_f32 * S,
00350   float32_t * pSrc,
00351   float32_t * pDst,
00352   uint32_t blockSize)
00353 {
00354   float32_t fcurr, fnext = 0, gcurr, gnext;      /* Temporary variables for lattice stages */
00355   float32_t acc;                                 /* Accumlator */
00356   uint32_t blkCnt, tapCnt;                       /* temporary variables for counts */
00357   float32_t *px1, *px2, *pk, *pv;                /* temporary pointers for state and coef */
00358   uint32_t numStages = S->numStages;             /* number of stages */
00359   float32_t *pState;                             /* State pointer */
00360   float32_t *pStateCurnt;                        /* State current pointer */
00361 
00362 
00363   /* Run the below code for Cortex-M0 */
00364 
00365   blkCnt = blockSize;
00366 
00367   pState = &S->pState[0];
00368 
00369   /* Sample processing */
00370   while(blkCnt > 0u)
00371   {
00372     /* Read Sample from input buffer */
00373     /* fN(n) = x(n) */
00374     fcurr = *pSrc++;
00375 
00376     /* Initialize state read pointer */
00377     px1 = pState;
00378     /* Initialize state write pointer */
00379     px2 = pState;
00380     /* Set accumulator to zero */
00381     acc = 0.0f;
00382     /* Initialize Ladder coeff pointer */
00383     pv = &S->pvCoeffs[0];
00384     /* Initialize Reflection coeff pointer */
00385     pk = &S->pkCoeffs[0];
00386 
00387 
00388     /* Process sample for numStages */
00389     tapCnt = numStages;
00390 
00391     while(tapCnt > 0u)
00392     {
00393       gcurr = *px1++;
00394       /* Process sample for last taps */
00395       fnext = fcurr - ((*pk) * gcurr);
00396       gnext = (fnext * (*pk++)) + gcurr;
00397 
00398       /* Output samples for last taps */
00399       acc += (gnext * (*pv++));
00400       *px2++ = gnext;
00401       fcurr = fnext;
00402 
00403       /* Decrementing loop counter */
00404       tapCnt--;
00405 
00406     }
00407 
00408     /* y(n) += g0(n) * v0 */
00409     acc += (fnext * (*pv));
00410 
00411     *px2++ = fnext;
00412 
00413     /* write out into pDst */
00414     *pDst++ = acc;
00415 
00416     /* Advance the state pointer by 1 to process the next group of samples */
00417     pState = pState + 1u;
00418     blkCnt--;
00419 
00420   }
00421 
00422   /* Processing is complete. Now copy last S->numStages samples to start of the buffer           
00423      for the preperation of next frame process */
00424 
00425   /* Points to the start of the state buffer */
00426   pStateCurnt = &S->pState[0];
00427   pState = &S->pState[blockSize];
00428 
00429   tapCnt = numStages;
00430 
00431   /* Copy the data */
00432   while(tapCnt > 0u)
00433   {
00434     *pStateCurnt++ = *pState++;
00435 
00436     /* Decrement the loop counter */
00437     tapCnt--;
00438   }
00439 
00440 }
00441 
00442 #endif /*   #ifndef ARM_MATH_CM0_FAMILY */
00443 
00444 
00445 /**    
00446  * @} end of IIR_Lattice group    
00447  */