Here’s an example that filters an analog sensor reading by taking a weighted average of samples of the sensor. It’s based on this algorithm:
filteredValue = x * rawValue + (1-x)*lastFilteredValue;
Where X is a value between 0 and 1 that indicates how reliable the new raw value is. If it’s 100% reliable, X = 1, and no filtering is done. If it’s totally unreliable, x = 0, and the raw result is filtered out. Examples for Wiring and PicBasic Pro follow:
Written in Wiring, tested on an Arduino board.
/*
Analog smoothing using a weighted average filter
by Tom Igoe
Based on notes by David Schultz, at
http://home.earthlink.net/~david.schultz/rnd/2002/KalmanApogee.pdf
This program reads an analog input and smooths out the result using a
weighted average filter. It works by taking a weighted average of the
current reading and the average of the previous readings.
In this example, a second analog reading,
attached to a trimmer potentiometer, sets the weight. When the trimmer pot
is set high, the average is weighted in favor of the current reading, and
almost no smoothing is done. When the trimmer pot value is low, the average
is weighted in favor of the previous readings, and the current reading
affects the average very little.
n.b. the variable "lastEstimate" needs to be a global, since it's modified
each time a new filtering is done. So if you want to use this for multiple
inouts, you'll need a "lastEstimate" variable for each input.
Created 17 October 2005
Updated 27 March 2006
*/
int currentEstimate = 0; // result of the Kalman filtered reading
int lastEstimate = 0; // previous result
int analogVal = 0; // raw analog input reading
int trimPotValue = 0; // trim pot used to set the weight for averaging
// function prototypes:
int filter(int rawValue, int weight, int lastValue);
void blink(int howManyTimes);
void setup() {
beginSerial(9600);
blink(3);
}
void loop() {
// read the sensor:
analogVal = analogRead(0);
// read the trim pot:
trimPotValue = analogRead(1);
// filter the sensor's result:
currentEstimate = filter(analogVal, trimPotValue, lastEstimate);
// print the result:
printByte(currentEstimate/4);
// save the current result for future use:
lastEstimate = currentEstimate;
// delay before next reading:
delay(10);
}
// Blink the reset LED:
void blink(int howManyTimes) {
int i;
for (i=0; i< howManyTimes; i++) {
digitalWrite(13, HIGH);
delay(200);
digitalWrite(13, LOW);
delay(200);
}
}
// filter the current result using a weighted average filter:
int filter(int rawValue, int weight, int lastValue) {
int fValue = 0;
int x = 0;
// convert the weight number to a value between 0 and 1:
x = weight/102;
// run the filter:
fValue = (x * rawValue + (10-x)*lastValue)/10;
// return the result:
return fValue;
}
Written in PicBasic Pro, tested on a PIC 18F252:
' Analog smoothing using a weighted average filter
' by Tom Igoe
' Based on notes by David Schultz, at
' http://home.earthlink.net/~david.schultz/rnd/2002/KalmanApogee.pdf
' This program reads an analog input and smooths out the result using a
' weighted average filter. It works by taking a weighted average of the
' current reading and the average of the previous readings.
' In this example, a second analog reading,
' attached to a trimmer potentiometer, sets the weight. When the trimmer pot
' is set high, the average is weighted in favor of the current reading, and
' almost no smoothing is done. When the trimmer pot value is low, the average
' is weighted in favor of the previous readings, and the current reading
' affects the average very little.
' n.b. the variable "lastEstimate" needs to be a global, since it's modified
' each time a new filtering is done. So if you want to use this for multiple
' inouts, you'll need a "lastEstimate" variable for each input.
' Created 17 October 2005
' Updated
' Define ADCIN parameters
DEFINE ADC_BITS 10 ' Set number of bits in result
DEFINE ADC_CLOCK 3 ' Set clock source (3=rc)
DEFINE ADC_SAMPLEUS 50 ' Set sampling time in uS
TRISA = %11111111 ' Set PORTA to all input
ADCON1 = %10000010 ' Set PORTA analog and right justify result
currentEstimate var word ' result of the weighted averaged reading
lastEstimate var word ' previous result
analogVal var word ' raw analog input reading
trimPotValue var word ' trim pot used to set the weight for averaging
' serial variables and constants:
tx var portc.6
rx var portc.7
inv9600 con 16468
LEDPin var portb.7
' Variables for subroutines:
byteVar var byte
x var word
i var byte
gosub blink
main:
' read the sensor:
adcin 0, analogVal
' read the trim pot:
adcin 1, trimPotValue
' filter the sensor's result:
gosub filter
' print the result:
byteVar = currentEstimate /4
serout2 tx, inv9600, [byteVar]
' save the current result for future use:
lastEstimate = currentEstimate
' delay before next reading:
pause 10
goto main
' Blink the reset LED:
blink:
for i=0 to 3
high LEDPin
pause 200
low LEDPin
pause 200
next
return
' filter the current result using a weighted average filter:
filter:
x = 0
' convert the weight number to a value between 0 and 10:
x = trimPOtValue/102
' run the filter:
currentEstimate = (x * analogVal + (10-x)*lastEstimate)/10
' return the result:
return
