Problem statement
Suppose we have accelerometer with 3 axis and the raw data is quite noisy. Below is the plots of noisy data (right) and (desired/filtered) data. So we need to write a filter using which we’ll get the required signal.
Left image is filtered from sudden bumps and scaled, so if you notice the left signal is the middle part of right plot from approximately from -240 to 240.
Algorithm
For filtration quite simple algorithm was used. As the accelerometer data on appropriate axes X, Y, Z is the projection of gravity so the square root of their square sum will be the module of gravity vector with some error +/- 100 points.
Code
Below is the bash script which was used for filtration above signal.
#!/bin/bash
function tosigned() {
if [ $1 -gt 32767 ]; then
echo $[$1-65536]
else
echo $1
fi
}
function abs() {
if [ $1 -lt 0 ] ; then
echo `expr 0 - $1`
else
echo $1
fi
}
sleep 1 # Wait for a second then send accelerometer data
tx=0
ty=0
tz=0
while :
do
x= "datax from accel"
y= "datay from accel"
z= "dataz from accel"
x=$(tosigned $x)
y=$(tosigned $y)
z=$(tosigned $z)
if [ $(abs $x) -lt 1000 -a $(abs $y) -lt 1000 -a $(abs $z) -lt 1000 ]; then
g=$[$x * $x + $y * $y + $z * $z]
if [ $g -gt 40000 -a $g -lt 70000 ]; then
max=0
if [ $x -gt $max ]; then
max=$x
fi
if [ $y -gt $max ]; then
max=$y
fi
if [ $z -gt $max ]; then
max=$z
fi
if [ $tx -eq 0 -a $ty -eq 0 -a $tz -eq 0 ]; then
tx=$x
ty=$y
tz=$z
continue
fi
if [ $(abs $[$x - $tx]) -gt 200 -o $(abs $[$y - $ty]) -gt 200 -o $(abs $[$z - $tz]) -gt 200 ]; then
tx=$x
ty=$y
tz=$z
continue
fi
echo "$x $y $z"
break
#sleep 0.01
fi
fi
done
