(EDIT: See also
Kalman filters)
Alpha-Beta filters are fairly well explained on wikipedia, and are generally a very easy first-stop solution before heading to Kalman or particle-filter alternatives. (The next obvious step is to extend this to include acceleration (Alpha-Beta-Gamma), and to limit the error/estimates to a sensible range for your application).
Here is a small example program showing how they work, it generates output similar to this:
Ideal position: -0.897 -0.443
Mesaured position: -0.890 -0.421 [diff:0.029]
AlphaBeta position: -0.898 -0.442 [diff:0.001]
Total error if using raw measured: 1.522438
Total error if using a-b filter: 1.059981
Reduction in error: 69%
C source code follows:
/** A simple alpha-beta filter example by Adrian Boeing
www.adrianboeing.com
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
typedef struct {
float alpha; //alpha value (effects x, eg pos)
float beta; //beta value (effects v, eg vel)
float xk_1; //current x-estimate
float vk_1; //current v-estimate
} AlphaBeta;
void InitializeAlphaBeta(float x_measured, float alpha, float beta, AlphaBeta* pab) {
pab->xk_1 = x_measured;
pab->vk_1 = 0;
pab->alpha = alpha;
pab->beta = beta;
}
void AlphaBetaFilter(float x_measured, float dt, AlphaBeta* pab) {
float xk_1 = pab->xk_1;
float vk_1 = pab->vk_1;
float alpha = pab->alpha;
float beta = pab->beta;
float xk; //current system state (ie: position)
float vk; //derivative of system state (ie: velocity)
float rk; //residual error
//update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dt)
xk = xk_1 + dt * vk_1;
//update (estimated) velocity
vk = vk_1;
//what is our residual error (mesured - estimated)
rk = x_measured - xk;
//update our estimates given the residual error.
xk = xk + alpha * rk;
vk = vk + beta/dt * rk;
//finished!
//now all our "currents" become our "olds" for next time
pab->vk_1 = vk;
pab->xk_1 = xk;
}
double frand() {
return 2*((rand()/(double)RAND_MAX) - 0.5);
}
int main(int argc, char *argv[]) {
AlphaBeta ab_x;
AlphaBeta ab_y;
double t; //time
double x,y; //ideal x-y coordinates
double xm,ym; //measured x-y coordinates
double xnoise = 0; //noise we are inserting into our system
double ynoise = 0;
double m_error = 0; //total error (measured vs ideal)
double ab_error = 0; //total error (ab filter vs ideal)
#define DT 0.1
//intialize the AB filters
InitializeAlphaBeta(1,0.85,0.001,&ab_x); //x position
InitializeAlphaBeta(0,1.27,0.009,&ab_y); //y position
srand(0);
for (t = 0; t < 4; t+=DT) {
//our 'true' position (A circle)
x = cos(t);
y = sin(t);
//update our simulated noise & drift
xnoise += frand()*0.01;
ynoise += frand()*0.01;
//our 'measured' position (has some noise)
xm = x + xnoise;
ym = y + ynoise;
//our 'filtered' position (removes some noise)
AlphaBetaFilter(xm,DT, &ab_x);
AlphaBetaFilter(ym,DT, &ab_y);
//print
printf("Ideal position: %6.3f %6.3f\n",x,y);
printf("Mesaured position: %6.3f %6.3f [diff:%.3f]\n",xm,ym,fabs(x-xm) + fabs(y-ym));
printf("AlphaBeta position: %6.3f %6.3f [diff:%.3f]\n",ab_x.xk_1,ab_y.xk_1,fabs(x-ab_x.xk_1) + fabs(y-ab_y.xk_1));
//update error sum (for statistics only)
m_error += fabs(x-xm) + fabs(y-ym);
ab_error += fabs(x-ab_x.xk_1) + fabs(y-ab_y.xk_1);
}
printf("Total error if using raw measured: %f\n",m_error);
printf("Total error if using a-b filter: %f\n",ab_error);
printf("Reduction in error: %d%% \n",(int)((ab_error/m_error)*100));
return 0;
}
Posts
