DSPRelated.com
Code
   Code Snippets    Submitted by wingnut

Adaptive IIR filter using fixed point integer arithmetic

● March 23, 2013 ● Coded in C 
typedef signed long s32;

#define ABS(x)                 ((x) < 0 ? -(x) : (x))
#define CLAMP(x,min,max)       {if ((x) <= (min)) (x) = (min); else if ((x) >= (max)) (x) = (max);}

#define KLPF_MAX_ADAPT			232L
#define KLPF_MIN_ADAPT			 0L
#define KLPF_DEFAULT_ADAPT		160L

#define ACCEL_MIN_ADAPT     5
#define ACCEL_MAX_ADAPT     15

s32 gnKLpf          = KLPF_DEFAULT_ADAPT;   // IIR filter coefficient, 0 to 255
s32 gnKLpfMax       = KLPF_MAX_ADAPT;  
s32 gnAccel = 0;   // rate of change of climbrate
s32 gnCpsx256 = 0; // climbrate in centimeters per second, with 8 bit fractional precision
s32 gnCps; // climbrate in centimeters per second

// IIR low pass filter using fixed point integer arithmetic. 8bits of fractional precision for IIR coefficient K.
// So the actual floating point coefficient is k = K/256, 0.0 <= k < 1.0. The filter computes
// climbRateFiltered = climbRateFiltered*k + newClimbRate*(1.0 - k). So K values close to 256 will result in 
// heavy damping, K values close to 0 will result in almost no damping.  The IIR low pass filtered output gnCps 
// is the rounded up integer value, the 8bit fraction precision value gnCpsx256 is a global variable, so is
// maintained between calls to the filter. No integer divides required. 

void sns_LPFClimbRate(void)   {
   	s32 tmp;
   	tmp = gnCpsx256*gnKLpf + (gnCps<<8)*(256L-gnKLpf);
   	gnCpsx256 = (tmp >= 0 ? ((tmp + 128L)>>8) : ((tmp - 128L)>>8));
   	gnCps = (gnCpsx256>>8);
   	}  

// Adapt the IIR filter coeffient to the rate of change. If samples are not changing much, increase the damping.
// If the samples are changing by a large amount, reduce the damping. This is done to reduce the response delay to a
// a step change, while still being able to heavily damp out jitter in steady state noisy samples.
// This function basically linearly interpolates KLpf between KLPF_MAX and KLPF_MIN based on the acceleration.
// Values of acceleration outside experimentally determined limits are clamped to the limits, depends on the 
// application. A variable is used for KLpfMax to allow user to adjust the maximum allowed damping.

s32 sns_AdaptKLpf(void) {
	s32 klpf,nAccel;
	nAccel = ABS(gnAccel);
	CLAMP(nAccel, ACCEL_MIN_ADAPT, ACCEL_MAX_ADAPT);
	klpf = gnKLpfMax + ((KLPF_MIN_ADAPT-gnKLpfMax)*(nAccel - ACCEL_MIN_ADAPT))/(ACCEL_MAX_ADAPT-ACCEL_MIN_ADAPT);
	CLAMP(klpf, KLPF_MIN_ADAPT, KLPF_MAX_ADAPT);
	return klpf;
	}

// usage : 
// For each new data sample that arrives
//   1. calculate new unfiltered climbrate nCps from sample data buffer	
//	 2. gnAccel = (nCps - gnCps);
//	 3. (optionally low pass filter gnAccel using same type of IIR filter, if required)
//   4. gnCps = nCps;
//	 5. gnKLpf = sns_AdaptKLpf();
//	 6. sns_LPFClimbRate();