Individual Sensor Filters

Here is the place for all your dsPic or autopilot questions.

Moderator: lukasz

Individual Sensor Filters

Postby Chris » Thu Dec 24, 2009 4:11 pm

Hey everybody!

Some sensors have a really fuzzy or jittery output (looking at you accels). Assuming this fuzz is an error, you can "filter" out the fuzz by using past readings to guess what the current reading should be.
One caution, a filter will also make your system respond slower. How much slower depends on the specifics of the filter.
If you're using a Kalman filter, it already assumes error in your sensor readings, but I've found it helps to have a specific filter on the accel to get rid of the worst jitters. Gyros seem to do okay without pre-filtering. You can use these filters on any data (accelerometer, magnetometer, gyroscope, pressure, etc.).
Here are I couple filters I use, if you have some better ones I'd def like to see them!

1) The Moving Average Filter
This takes the last n readings (including the current one) and averages them. Simple enough. A 5-point moving average filter returns the average of the last 5 readings:
Code: Select all
//written for PIC18F4550
//sensorReading is an array length 5
sensorReading[4] = sensorReading[3]; //Move all the values over one place
sensorReading[3] = sensorReading[2];
sensorReading[2] = sensorReading[1];
sensorReading[1] = sensorReading[0];

ADCON0 = 0b00000011;      //Read AN0
while(ADCON0bits.GO);      //Wait for reading to complete
sensorReading[0] = ADRES;//Update the first value in the array with the new reading

//Average the five values in the array
movAvgFilt = (sensorReading[4] +sensorReading[3] +sensorReading[2] +sensorReading[1] +sensorReading[0]) / 5; 

2)The Low Pass Filter (
Low Pass means the filter allows low frequencies to "pass" through the filter. You set a cutoff frequency and all frequencies above this (i.e. jitters) get cutoff. I used 2Hz on one project, but it really depends on the project. Since this uses a second division in the calculation, it uses more CPU time than the moving average filter, but is a little more elegant.
Cutoff frequency is calculated by:
cutoff frequency = 1/ (2*pi*RC) --> RC is the constant you set in code to set the frequency.
Code: Select all
filteredReading = currentReading*(dt/(RC+dt)) + oldFilteredReading*(RC/(RC+dt));
oldFilteredReading = filteredReading;

OK, that was a long post! Post some more filters, questions, comments. Thanks!
Posts: 19
Joined: Sat Dec 12, 2009 4:49 pm

Re: Individual Sensor Filters

Postby Laurent » Fri Dec 25, 2009 9:26 pm

nice topic !

just my 2 cents from another forum : ... ilter+long

Merry christmas to all !
Posts: 5
Joined: Fri Nov 27, 2009 5:23 pm

Re: Individual Sensor Filters

Postby Tom » Sat Dec 26, 2009 11:44 am

Good thread!

What I currently do in the gluonpilot code: every used input is averaged over 16 samples. Especially for the accelerometers this is important, because motor vibration can add a lot of noise.

An important thing to know when you add a filter is the nature of the noise and your sampling rate, e.g.:
Accelerometer noise from motor vibrations: typically between 10 and 20kHz. If you want to detect and cancel out this frequency, you need to sample at 40kHz or more (Nyquist: In practice this might be less important because our required sample rate is a lot lower.

- Low pass filters:
Can be implemented with few CPU cycles, but requires floating/fixed point values. Be very careful with these, because they have delays! Always calculate your delays and see if it's a problem for not. E.g. if you sample at 1kHz, and you need samples at 500Hz, digital low pass filters will inflict delays in your system!

- Moving average:
Nice filter, but the implementation is not always efficient (moving the values as you did in that code).

Another one:
- To remove jitter:
Take n samples, remove the largest and smallest value, and average the remaining ones.
Costly because you need to sort your values.
User avatar
Site Admin
Posts: 1016
Joined: Fri Nov 13, 2009 6:27 pm
Location: Belgium

Re: Individual Sensor Filters / moving average implementation

Postby Laurent » Sat Dec 26, 2009 10:12 pm

Moving average implementation :

In some circumstances (more than x element to average) it can be faster to implement a circular buffer :

adding a value in the buffer :
Code: Select all
OneEightyDegreeTime[(SpeedPtr&0x000F)] = ThirtyDegreeTime*6;

Then you can calculate the average as alwways :
Code: Select all
ThreeSixtyDegreeAverage = 0;
for (i=0; i<16; i++)
  ThreeSixtyDegreeAverage += OneEightyDegreeTime[i];
ThreeSixtyDegreeAverage = ThreeSixtyDegreeAverage >> 3;

Note that the array indexing takes some instructions, so it's not always more efficient than Chris's implementation (better check assembler file to find out).
Posts: 5
Joined: Fri Nov 27, 2009 5:23 pm

Re: Individual Sensor Filters / low pass filter implementation

Postby Laurent » Sat Dec 26, 2009 10:45 pm

if you just need a "pre" lowpass filter, you can easily avoid float / fixed point by choosing good coeficient :

Code: Select all
FilteredVal = (NewVal * 1 + FilteredVal * 3) >> 2;

you can play with the coeficient, but keep their sum equal to a power of 2

Sidenote :
mul by 3 can be written as (val+val<<1)
Same can be applied to, for example, mul by 15 : (val<<4 - val)

With this you can achieve a very low pass filter that run pretty fast :
Code: Select all
FilteredVal = ( NewVal + (FilteredVal<<4 - FilteredVal) ) >> 4;

Just make sure you take care of overflow (ie avoid them).
Posts: 5
Joined: Fri Nov 27, 2009 5:23 pm

Re: Individual Sensor Filters

Postby Goldfinch » Fri Mar 19, 2010 7:46 pm

It looks like alpha-beta filter! I think that for MEMS data the moving average filter is sufficient and simple in implementation with only one multiply and one sum per iteration. Mathworks Simulink has a great feature to generate c-code for filters (by visual scheme), but it is very expensive.
Posts: 12
Joined: Mon Mar 15, 2010 11:22 am
Location: Russia

Re: Individual Sensor Filters

Postby hibaaryan » Tue Jan 13, 2015 9:12 am

about your roll example: if your aileron isn't moving enough, you should increase the "P" (of the PID-tab) of the roll 2 aileron group. It shouldn't be it's maximum position. This would be too much. If you fly your plane manually, you never give full aileron deflections as well :-)
The "max roll angle", is the maximum angle of your plane. If it's 45°, your plane will never have a roll angle of over 45° (in both stabilized and navigation mode).
As a rule of thumb I can say that the aileron deflection in stabilized mode is usually less than you would expect. On a fast funjet it is only a few millimeters.
Save your time and get on time success in Testking juniper certification and test king to exams itil certification by using our latest and other superb pass resources of Quincy University and Southwestern College
Posts: 1
Joined: Tue Jan 13, 2015 8:23 am

Return to Firmware

Who is online

Users browsing this forum: No registered users and 1 guest