## Tuesday, March 02, 2010

### Alpha Beta 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
*/

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

Andreas said...

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.

Andreas said...

sorry, a complementary filter, of course.

sukhi said...

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