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; }

## 4 comments:

Hello Adrian,

I am trying to learn more about filters. This “filter” just looks to me like a trailing weighted average. If you have a look at the graph you see how the filtered curve just smoothes the measurement curve with a delay and a loss in amplitude, but does in no way "correct" the measurements.

If you have two independant inputs, you can fuse them with a filter to estimate the true position like a Kalman or a complimentary filter.

Can you explain me why you also "filter" velocity?

As velocity is derived from position it would in my view be sufficient to just smooth position measurements, and you will get a smoother velocity aswell.

sorry, a complementary filter, of course.

hi adrian,do you have the same implementation in C++ as well?? It shall be great help.

sukhi: No, sorry only C, but it shouldn't take much effort to convert to C++.

Post a Comment