Adaptive IIR filter using fixed point integer arithmetic
IIR low pass filters are simple to implement and require minimal data storage. However floating point IIR filter arithmetic is expensive to compute on microcontrollers without FPU units.
Additionally, its often desirable to able to automatically adapt the level of damping of the IIR filter to the incoming data. Specifically, we would like to minimize step delay response time, while still heavily damping out steady state jitter.
This implementation linearly interpolates the IIR coefficient between a minimum and maximum value based on the rate of change of the sample data. If there is a large step change, the damping is reduced. If there is little change, the damping is increased.
The example code is for calculation of climbrate in a variometer (used by glider pilots), but other than the variable naming, its applicable to any environment where you need fast response to step inputs while heavily filtering steady state data.
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();