| 提交 | 用户 | age | ||
| bfc108 | 1 | /* ---------------------------------------------------------------------- |
| Q | 2 | * Copyright (C) 2010-2014 ARM Limited. All rights reserved. |
| 3 | * | |
| 4 | * $Date: 19. March 2015 | |
| 5 | * $Revision: V.1.4.5 | |
| 6 | * | |
| 7 | * Project: CMSIS DSP Library | |
| 8 | * Title: arm_lms_f32.c | |
| 9 | * | |
| 10 | * Description: Processing function for the floating-point LMS filter. | |
| 11 | * | |
| 12 | * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 | |
| 13 | * | |
| 14 | * Redistribution and use in source and binary forms, with or without | |
| 15 | * modification, are permitted provided that the following conditions | |
| 16 | * are met: | |
| 17 | * - Redistributions of source code must retain the above copyright | |
| 18 | * notice, this list of conditions and the following disclaimer. | |
| 19 | * - Redistributions in binary form must reproduce the above copyright | |
| 20 | * notice, this list of conditions and the following disclaimer in | |
| 21 | * the documentation and/or other materials provided with the | |
| 22 | * distribution. | |
| 23 | * - Neither the name of ARM LIMITED nor the names of its contributors | |
| 24 | * may be used to endorse or promote products derived from this | |
| 25 | * software without specific prior written permission. | |
| 26 | * | |
| 27 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | |
| 28 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | |
| 29 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | |
| 30 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | |
| 31 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | |
| 32 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | |
| 33 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | |
| 34 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | |
| 35 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | |
| 36 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | |
| 37 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | |
| 38 | * POSSIBILITY OF SUCH DAMAGE. | |
| 39 | * -------------------------------------------------------------------- */ | |
| 40 | ||
| 41 | #include "arm_math.h" | |
| 42 | ||
| 43 | /** | |
| 44 | * @ingroup groupFilters | |
| 45 | */ | |
| 46 | ||
| 47 | /** | |
| 48 | * @defgroup LMS Least Mean Square (LMS) Filters | |
| 49 | * | |
| 50 | * LMS filters are a class of adaptive filters that are able to "learn" an unknown transfer functions. | |
| 51 | * LMS filters use a gradient descent method in which the filter coefficients are updated based on the instantaneous error signal. | |
| 52 | * Adaptive filters are often used in communication systems, equalizers, and noise removal. | |
| 53 | * The CMSIS DSP Library contains LMS filter functions that operate on Q15, Q31, and floating-point data types. | |
| 54 | * The library also contains normalized LMS filters in which the filter coefficient adaptation is indepedent of the level of the input signal. | |
| 55 | * | |
| 56 | * An LMS filter consists of two components as shown below. | |
| 57 | * The first component is a standard transversal or FIR filter. | |
| 58 | * The second component is a coefficient update mechanism. | |
| 59 | * The LMS filter has two input signals. | |
| 60 | * The "input" feeds the FIR filter while the "reference input" corresponds to the desired output of the FIR filter. | |
| 61 | * That is, the FIR filter coefficients are updated so that the output of the FIR filter matches the reference input. | |
| 62 | * The filter coefficient update mechanism is based on the difference between the FIR filter output and the reference input. | |
| 63 | * This "error signal" tends towards zero as the filter adapts. | |
| 64 | * The LMS processing functions accept the input and reference input signals and generate the filter output and error signal. | |
| 65 | * \image html LMS.gif "Internal structure of the Least Mean Square filter" | |
| 66 | * | |
| 67 | * The functions operate on blocks of data and each call to the function processes | |
| 68 | * <code>blockSize</code> samples through the filter. | |
| 69 | * <code>pSrc</code> points to input signal, <code>pRef</code> points to reference signal, | |
| 70 | * <code>pOut</code> points to output signal and <code>pErr</code> points to error signal. | |
| 71 | * All arrays contain <code>blockSize</code> values. | |
| 72 | * | |
| 73 | * The functions operate on a block-by-block basis. | |
| 74 | * Internally, the filter coefficients <code>b[n]</code> are updated on a sample-by-sample basis. | |
| 75 | * The convergence of the LMS filter is slower compared to the normalized LMS algorithm. | |
| 76 | * | |
| 77 | * \par Algorithm: | |
| 78 | * The output signal <code>y[n]</code> is computed by a standard FIR filter: | |
| 79 | * <pre> | |
| 80 | * y[n] = b[0] * x[n] + b[1] * x[n-1] + b[2] * x[n-2] + ...+ b[numTaps-1] * x[n-numTaps+1] | |
| 81 | * </pre> | |
| 82 | * | |
| 83 | * \par | |
| 84 | * The error signal equals the difference between the reference signal <code>d[n]</code> and the filter output: | |
| 85 | * <pre> | |
| 86 | * e[n] = d[n] - y[n]. | |
| 87 | * </pre> | |
| 88 | * | |
| 89 | * \par | |
| 90 | * After each sample of the error signal is computed, the filter coefficients <code>b[k]</code> are updated on a sample-by-sample basis: | |
| 91 | * <pre> | |
| 92 | * b[k] = b[k] + e[n] * mu * x[n-k], for k=0, 1, ..., numTaps-1 | |
| 93 | * </pre> | |
| 94 | * where <code>mu</code> is the step size and controls the rate of coefficient convergence. | |
| 95 | *\par | |
| 96 | * In the APIs, <code>pCoeffs</code> points to a coefficient array of size <code>numTaps</code>. | |
| 97 | * Coefficients are stored in time reversed order. | |
| 98 | * \par | |
| 99 | * <pre> | |
| 100 | * {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]} | |
| 101 | * </pre> | |
| 102 | * \par | |
| 103 | * <code>pState</code> points to a state array of size <code>numTaps + blockSize - 1</code>. | |
| 104 | * Samples in the state buffer are stored in the order: | |
| 105 | * \par | |
| 106 | * <pre> | |
| 107 | * {x[n-numTaps+1], x[n-numTaps], x[n-numTaps-1], x[n-numTaps-2]....x[0], x[1], ..., x[blockSize-1]} | |
| 108 | * </pre> | |
| 109 | * \par | |
| 110 | * Note that the length of the state buffer exceeds the length of the coefficient array by <code>blockSize-1</code> samples. | |
| 111 | * The increased state buffer length allows circular addressing, which is traditionally used in FIR filters, | |
| 112 | * to be avoided and yields a significant speed improvement. | |
| 113 | * The state variables are updated after each block of data is processed. | |
| 114 | * \par Instance Structure | |
| 115 | * The coefficients and state variables for a filter are stored together in an instance data structure. | |
| 116 | * A separate instance structure must be defined for each filter and | |
| 117 | * coefficient and state arrays cannot be shared among instances. | |
| 118 | * There are separate instance structure declarations for each of the 3 supported data types. | |
| 119 | * | |
| 120 | * \par Initialization Functions | |
| 121 | * There is also an associated initialization function for each data type. | |
| 122 | * The initialization function performs the following operations: | |
| 123 | * - Sets the values of the internal structure fields. | |
| 124 | * - Zeros out the values in the state buffer. | |
| 125 | * To do this manually without calling the init function, assign the follow subfields of the instance structure: | |
| 126 | * numTaps, pCoeffs, mu, postShift (not for f32), pState. Also set all of the values in pState to zero. | |
| 127 | * | |
| 128 | * \par | |
| 129 | * Use of the initialization function is optional. | |
| 130 | * However, if the initialization function is used, then the instance structure cannot be placed into a const data section. | |
| 131 | * To place an instance structure into a const data section, the instance structure must be manually initialized. | |
| 132 | * Set the values in the state buffer to zeros before static initialization. | |
| 133 | * The code below statically initializes each of the 3 different data type filter instance structures | |
| 134 | * <pre> | |
| 135 | * arm_lms_instance_f32 S = {numTaps, pState, pCoeffs, mu}; | |
| 136 | * arm_lms_instance_q31 S = {numTaps, pState, pCoeffs, mu, postShift}; | |
| 137 | * arm_lms_instance_q15 S = {numTaps, pState, pCoeffs, mu, postShift}; | |
| 138 | * </pre> | |
| 139 | * where <code>numTaps</code> is the number of filter coefficients in the filter; <code>pState</code> is the address of the state buffer; | |
| 140 | * <code>pCoeffs</code> is the address of the coefficient buffer; <code>mu</code> is the step size parameter; and <code>postShift</code> is the shift applied to coefficients. | |
| 141 | * | |
| 142 | * \par Fixed-Point Behavior: | |
| 143 | * Care must be taken when using the Q15 and Q31 versions of the LMS filter. | |
| 144 | * The following issues must be considered: | |
| 145 | * - Scaling of coefficients | |
| 146 | * - Overflow and saturation | |
| 147 | * | |
| 148 | * \par Scaling of Coefficients: | |
| 149 | * Filter coefficients are represented as fractional values and | |
| 150 | * coefficients are restricted to lie in the range <code>[-1 +1)</code>. | |
| 151 | * The fixed-point functions have an additional scaling parameter <code>postShift</code>. | |
| 152 | * At the output of the filter's accumulator is a shift register which shifts the result by <code>postShift</code> bits. | |
| 153 | * This essentially scales the filter coefficients by <code>2^postShift</code> and | |
| 154 | * allows the filter coefficients to exceed the range <code>[+1 -1)</code>. | |
| 155 | * The value of <code>postShift</code> is set by the user based on the expected gain through the system being modeled. | |
| 156 | * | |
| 157 | * \par Overflow and Saturation: | |
| 158 | * Overflow and saturation behavior of the fixed-point Q15 and Q31 versions are | |
| 159 | * described separately as part of the function specific documentation below. | |
| 160 | */ | |
| 161 | ||
| 162 | /** | |
| 163 | * @addtogroup LMS | |
| 164 | * @{ | |
| 165 | */ | |
| 166 | ||
| 167 | /** | |
| 168 | * @details | |
| 169 | * This function operates on floating-point data types. | |
| 170 | * | |
| 171 | * @brief Processing function for floating-point LMS filter. | |
| 172 | * @param[in] *S points to an instance of the floating-point LMS filter structure. | |
| 173 | * @param[in] *pSrc points to the block of input data. | |
| 174 | * @param[in] *pRef points to the block of reference data. | |
| 175 | * @param[out] *pOut points to the block of output data. | |
| 176 | * @param[out] *pErr points to the block of error data. | |
| 177 | * @param[in] blockSize number of samples to process. | |
| 178 | * @return none. | |
| 179 | */ | |
| 180 | ||
| 181 | void arm_lms_f32( | |
| 182 | const arm_lms_instance_f32 * S, | |
| 183 | float32_t * pSrc, | |
| 184 | float32_t * pRef, | |
| 185 | float32_t * pOut, | |
| 186 | float32_t * pErr, | |
| 187 | uint32_t blockSize) | |
| 188 | { | |
| 189 | float32_t *pState = S->pState; /* State pointer */ | |
| 190 | float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ | |
| 191 | float32_t *pStateCurnt; /* Points to the current sample of the state */ | |
| 192 | float32_t *px, *pb; /* Temporary pointers for state and coefficient buffers */ | |
| 193 | float32_t mu = S->mu; /* Adaptive factor */ | |
| 194 | uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */ | |
| 195 | uint32_t tapCnt, blkCnt; /* Loop counters */ | |
| 196 | float32_t sum, e, d; /* accumulator, error, reference data sample */ | |
| 197 | float32_t w = 0.0f; /* weight factor */ | |
| 198 | ||
| 199 | e = 0.0f; | |
| 200 | d = 0.0f; | |
| 201 | ||
| 202 | /* S->pState points to state array which contains previous frame (numTaps - 1) samples */ | |
| 203 | /* pStateCurnt points to the location where the new input data should be written */ | |
| 204 | pStateCurnt = &(S->pState[(numTaps - 1u)]); | |
| 205 | ||
| 206 | blkCnt = blockSize; | |
| 207 | ||
| 208 | ||
| 209 | #ifndef ARM_MATH_CM0_FAMILY | |
| 210 | ||
| 211 | /* Run the below code for Cortex-M4 and Cortex-M3 */ | |
| 212 | ||
| 213 | while(blkCnt > 0u) | |
| 214 | { | |
| 215 | /* Copy the new input sample into the state buffer */ | |
| 216 | *pStateCurnt++ = *pSrc++; | |
| 217 | ||
| 218 | /* Initialize pState pointer */ | |
| 219 | px = pState; | |
| 220 | ||
| 221 | /* Initialize coeff pointer */ | |
| 222 | pb = (pCoeffs); | |
| 223 | ||
| 224 | /* Set the accumulator to zero */ | |
| 225 | sum = 0.0f; | |
| 226 | ||
| 227 | /* Loop unrolling. Process 4 taps at a time. */ | |
| 228 | tapCnt = numTaps >> 2; | |
| 229 | ||
| 230 | while(tapCnt > 0u) | |
| 231 | { | |
| 232 | /* Perform the multiply-accumulate */ | |
| 233 | sum += (*px++) * (*pb++); | |
| 234 | sum += (*px++) * (*pb++); | |
| 235 | sum += (*px++) * (*pb++); | |
| 236 | sum += (*px++) * (*pb++); | |
| 237 | ||
| 238 | /* Decrement the loop counter */ | |
| 239 | tapCnt--; | |
| 240 | } | |
| 241 | ||
| 242 | /* If the filter length is not a multiple of 4, compute the remaining filter taps */ | |
| 243 | tapCnt = numTaps % 0x4u; | |
| 244 | ||
| 245 | while(tapCnt > 0u) | |
| 246 | { | |
| 247 | /* Perform the multiply-accumulate */ | |
| 248 | sum += (*px++) * (*pb++); | |
| 249 | ||
| 250 | /* Decrement the loop counter */ | |
| 251 | tapCnt--; | |
| 252 | } | |
| 253 | ||
| 254 | /* The result in the accumulator, store in the destination buffer. */ | |
| 255 | *pOut++ = sum; | |
| 256 | ||
| 257 | /* Compute and store error */ | |
| 258 | d = (float32_t) (*pRef++); | |
| 259 | e = d - sum; | |
| 260 | *pErr++ = e; | |
| 261 | ||
| 262 | /* Calculation of Weighting factor for the updating filter coefficients */ | |
| 263 | w = e * mu; | |
| 264 | ||
| 265 | /* Initialize pState pointer */ | |
| 266 | px = pState; | |
| 267 | ||
| 268 | /* Initialize coeff pointer */ | |
| 269 | pb = (pCoeffs); | |
| 270 | ||
| 271 | /* Loop unrolling. Process 4 taps at a time. */ | |
| 272 | tapCnt = numTaps >> 2; | |
| 273 | ||
| 274 | /* Update filter coefficients */ | |
| 275 | while(tapCnt > 0u) | |
| 276 | { | |
| 277 | /* Perform the multiply-accumulate */ | |
| 278 | *pb = *pb + (w * (*px++)); | |
| 279 | pb++; | |
| 280 | ||
| 281 | *pb = *pb + (w * (*px++)); | |
| 282 | pb++; | |
| 283 | ||
| 284 | *pb = *pb + (w * (*px++)); | |
| 285 | pb++; | |
| 286 | ||
| 287 | *pb = *pb + (w * (*px++)); | |
| 288 | pb++; | |
| 289 | ||
| 290 | /* Decrement the loop counter */ | |
| 291 | tapCnt--; | |
| 292 | } | |
| 293 | ||
| 294 | /* If the filter length is not a multiple of 4, compute the remaining filter taps */ | |
| 295 | tapCnt = numTaps % 0x4u; | |
| 296 | ||
| 297 | while(tapCnt > 0u) | |
| 298 | { | |
| 299 | /* Perform the multiply-accumulate */ | |
| 300 | *pb = *pb + (w * (*px++)); | |
| 301 | pb++; | |
| 302 | ||
| 303 | /* Decrement the loop counter */ | |
| 304 | tapCnt--; | |
| 305 | } | |
| 306 | ||
| 307 | /* Advance state pointer by 1 for the next sample */ | |
| 308 | pState = pState + 1; | |
| 309 | ||
| 310 | /* Decrement the loop counter */ | |
| 311 | blkCnt--; | |
| 312 | } | |
| 313 | ||
| 314 | ||
| 315 | /* Processing is complete. Now copy the last numTaps - 1 samples to the | |
| 316 | satrt of the state buffer. This prepares the state buffer for the | |
| 317 | next function call. */ | |
| 318 | ||
| 319 | /* Points to the start of the pState buffer */ | |
| 320 | pStateCurnt = S->pState; | |
| 321 | ||
| 322 | /* Loop unrolling for (numTaps - 1u) samples copy */ | |
| 323 | tapCnt = (numTaps - 1u) >> 2u; | |
| 324 | ||
| 325 | /* copy data */ | |
| 326 | while(tapCnt > 0u) | |
| 327 | { | |
| 328 | *pStateCurnt++ = *pState++; | |
| 329 | *pStateCurnt++ = *pState++; | |
| 330 | *pStateCurnt++ = *pState++; | |
| 331 | *pStateCurnt++ = *pState++; | |
| 332 | ||
| 333 | /* Decrement the loop counter */ | |
| 334 | tapCnt--; | |
| 335 | } | |
| 336 | ||
| 337 | /* Calculate remaining number of copies */ | |
| 338 | tapCnt = (numTaps - 1u) % 0x4u; | |
| 339 | ||
| 340 | /* Copy the remaining q31_t data */ | |
| 341 | while(tapCnt > 0u) | |
| 342 | { | |
| 343 | *pStateCurnt++ = *pState++; | |
| 344 | ||
| 345 | /* Decrement the loop counter */ | |
| 346 | tapCnt--; | |
| 347 | } | |
| 348 | ||
| 349 | #else | |
| 350 | ||
| 351 | /* Run the below code for Cortex-M0 */ | |
| 352 | ||
| 353 | while(blkCnt > 0u) | |
| 354 | { | |
| 355 | /* Copy the new input sample into the state buffer */ | |
| 356 | *pStateCurnt++ = *pSrc++; | |
| 357 | ||
| 358 | /* Initialize pState pointer */ | |
| 359 | px = pState; | |
| 360 | ||
| 361 | /* Initialize pCoeffs pointer */ | |
| 362 | pb = pCoeffs; | |
| 363 | ||
| 364 | /* Set the accumulator to zero */ | |
| 365 | sum = 0.0f; | |
| 366 | ||
| 367 | /* Loop over numTaps number of values */ | |
| 368 | tapCnt = numTaps; | |
| 369 | ||
| 370 | while(tapCnt > 0u) | |
| 371 | { | |
| 372 | /* Perform the multiply-accumulate */ | |
| 373 | sum += (*px++) * (*pb++); | |
| 374 | ||
| 375 | /* Decrement the loop counter */ | |
| 376 | tapCnt--; | |
| 377 | } | |
| 378 | ||
| 379 | /* The result is stored in the destination buffer. */ | |
| 380 | *pOut++ = sum; | |
| 381 | ||
| 382 | /* Compute and store error */ | |
| 383 | d = (float32_t) (*pRef++); | |
| 384 | e = d - sum; | |
| 385 | *pErr++ = e; | |
| 386 | ||
| 387 | /* Weighting factor for the LMS version */ | |
| 388 | w = e * mu; | |
| 389 | ||
| 390 | /* Initialize pState pointer */ | |
| 391 | px = pState; | |
| 392 | ||
| 393 | /* Initialize pCoeffs pointer */ | |
| 394 | pb = pCoeffs; | |
| 395 | ||
| 396 | /* Loop over numTaps number of values */ | |
| 397 | tapCnt = numTaps; | |
| 398 | ||
| 399 | while(tapCnt > 0u) | |
| 400 | { | |
| 401 | /* Perform the multiply-accumulate */ | |
| 402 | *pb = *pb + (w * (*px++)); | |
| 403 | pb++; | |
| 404 | ||
| 405 | /* Decrement the loop counter */ | |
| 406 | tapCnt--; | |
| 407 | } | |
| 408 | ||
| 409 | /* Advance state pointer by 1 for the next sample */ | |
| 410 | pState = pState + 1; | |
| 411 | ||
| 412 | /* Decrement the loop counter */ | |
| 413 | blkCnt--; | |
| 414 | } | |
| 415 | ||
| 416 | ||
| 417 | /* Processing is complete. Now copy the last numTaps - 1 samples to the | |
| 418 | * start of the state buffer. This prepares the state buffer for the | |
| 419 | * next function call. */ | |
| 420 | ||
| 421 | /* Points to the start of the pState buffer */ | |
| 422 | pStateCurnt = S->pState; | |
| 423 | ||
| 424 | /* Copy (numTaps - 1u) samples */ | |
| 425 | tapCnt = (numTaps - 1u); | |
| 426 | ||
| 427 | /* Copy the data */ | |
| 428 | while(tapCnt > 0u) | |
| 429 | { | |
| 430 | *pStateCurnt++ = *pState++; | |
| 431 | ||
| 432 | /* Decrement the loop counter */ | |
| 433 | tapCnt--; | |
| 434 | } | |
| 435 | ||
| 436 | #endif /* #ifndef ARM_MATH_CM0_FAMILY */ | |
| 437 | ||
| 438 | } | |
| 439 | ||
| 440 | /** | |
| 441 | * @} end of LMS group | |
| 442 | */ | |
