Hi!
I did a spikefilter wich i find very useful for barodata. It may be of use for GPS as well. Sorry i am no programmer and have no mathematical background so i will describe the function like a kid would do it....
Here is an example array with 5 elements.
5 4 3 2 1
In the first step i sort the numbers low to high.
1 2 3 4 5
The result is in the middle (here "3") and new values are injected on both ends simultaneously.
Example a new value "4" occurs:
4 2 3 4 4
now sort:
2 3 4 4 4
The fliter result (number in the middle) is: 4
Example a new value "9" occurs:
9 3 4 4 9
now sort:
3 4 4 9 9
The result is still "4". The next new data will show the filter wich way to go...
I tested this filter with my spiky barodata and i could reduce the spikes a great deal, even with 5 elements, like in my example. Because this filter produces no/minimal lag, and the following moving average filter can be reduced in size - it reduces the over all lag significantly. It uses minimal cpu time.
Here i did the filter for 32bit barodata on multiwii/arduino. It takes around 20us.
....
static int32_t BaroTab[5];
....
//IDIOT BUBBLESORT FOR 5 ELEMENTS BY CRASHPILOT
int32_t extmp;
uint8_t rdy,sortidx,maxsortidx;
extmp = BaroAlt-GroundAlt; // "extmp" contains new data for filter
BaroTab[4] = extmp; // put new data on both ends of the field
BaroTab[0] = extmp;
debug[0] = extmp*10; // SHOW RAW DATA IN GUI
rdy = 0; maxsortidx=4;
while(rdy == 0){
rdy = 1;
for (sortidx = 0; sortidx<maxsortidx;sortidx++){
extmp = BaroTab[sortidx];
if (extmp > BaroTab[sortidx+1]) {BaroTab[sortidx] = BaroTab[sortidx+1]; BaroTab[sortidx+1] = extmp; rdy = 0;}
}
maxsortidx --;
}
int32_t EstAltBaro = BaroTab[2];
debug[1] = EstAltBaro*10; // SHOW FILTERED DATA IN GUI
Perhaps it is useful for someone here!
So long
Kraut Rob (aka Crashpilot:) )
EDIT: I have now tested it on the ACC values as well of my Baro/ACC integration. Now i can land in Baromode, because it was to bumpy with acc on touchdown without increasing the acc-lpf. This method seems to me of practical use for filtering other sensor data or outputdata (motor/servo signals) the easy way. The only drawback is the memoryusage. My simple bubble sort could be def. faster (sorting from 2 sides etc) but currently i am just sorting 5 numbers....
Replies
Nice experiment. A traditional moving average filter has some lag, but is much used since it is very fast to compute.
Here is an eample of standard moving average with your example data.
Buffer [ 5 4 3 2 1 ] calculate sum and devide to get average (5+4+3+2+1) = 15 / 5 = 3. Use average (3) as filter output.
New data 4. Remove oldest data from buffer (1) and insert new data. buffer is now [4 5 4 3 2], calculate new sum and average 18/5=3 (nearest integer). Do the same with next data (9) input. [ 9 4 5 4 3 ] = 25/5 = 5 and so on.
And this procedure can be greatly optimized. If you store the sum from the previous calculation you only need to subtract and oldest data entry from the sum and add the new data to get the next sum.
5 + 4 +3 +2 +1 = 15 / 5 = 3
[5 4 3 2 1] + 4:
15 - 1 + 4 = 18 / 5 = 3 (integer)
[4 5 4 3 2] + 9:
18 - 2 + 9 = 25 / 5 = 5
and so on.
And if you are smart about the size of the buffer and use a binary size (4, 8, 16..) you can use bitshift instead of divide to get the new average. Making it so that you only need to perform a single subtraction, addition and a bitshift each time there is new data.
Update:
The Spikefilter with 5 elements does a good job with 5Hz - GPS lat/lon data in combination with the LeadFilter. Tested on multiwii and eos bandi code (APM port) and mtk (3329) gps in binary mode.
So long
Kraut Rob
Here is a little video of the spikefilter in action. Sorry for choppy video but my old centrino laptop can not do more...
http://youtu.be/QrI1IYaWQz8