July 1993/The Alpha-Beta Filter/Listing 1
/* ALPHABET.C
Demonstrate the alpha-beta filter with a particular
data profile, with and without gaussian noise, and
with different alpha-beta values.
The graphic features here assume the presence of a
VGA monitor.
This code was written for Borland C++, Version 3.1,
coded for MS-DOS.
*/
#include <graphics.h>
#include <stdlib.h>
#include <stdio.h>
#include <conio.h>
#include <alloc.h>
#include <time.h>
#include <math.h>
#define PLOT1 120 /* range of x for first
part of plot */
#define PLOT2 310 /* range of x for second
part of plot */
#define PLOT3 430 /* range of x for third
part of plot */
#define PLOTEND 640 /* end of x range */
#define STEADY_STATE1 120.0 /* first steady state y
value */
#define STEADY_STATE2 400.0 /* second steady state y
value */
#define STEADY_STATE3 70.0 /* third steady state y
value */
#define STEADY_STATEBOT 480.0 /* bottom limit for
steady state
values */
#define ALPHA 0.25 /* arbitrary alpha
value */
void alpha_beta(double *y,double *y_smoothed,
double alpha,double beta);
void profile(double *y);
void gauss_noise(double *noise);
void plot_it(double *y);
void message(char *string1,char *string2,
char *string3);
void main(void)
{
int x,graphdriver=DETECT,graphmode,errorcode;
double *y_pure,*y_noisy,*y_smoothed,*noise;
double alpha,beta;
/**********************************/
/* allocate memory for the arrays */
/**********************************/
if((y_pure =
(double *)malloc(PLOTEND*sizeof(double)))==
NULL) {
printf("\a");
cprintf("Unable to allocate space for array ");
cprintf("y_pure[]\n\r");
exit(1);
}
if((y_noisy =
(double *)malloc(PLOTEND*sizeof(double)))==
NULL) {
printf("\a");
cprintf("Unable to allocate space for array ");
cprintf("y_noisy[]\n\r");
exit(1);
}
if((y_smoothed =
(double *)malloc(PLOTEND*sizeof(double)))==
NULL) {
printf("\a");
cprintf("Unable to allocate space for array ");
cprintf("y_smoothed[]\n\r");
exit(1);
}
if((noise =
(double *)malloc(PLOTEND*sizeof(double)))==NULL) {
printf("\a");
cprintf("Unable to allocate space for array ");
cprintf("noise[]\n\r");
exit(1);
}
/****************************/
/* initialize graphics mode */
/****************************/
registerbgidriver(EGAVGA_driver);
initgraph(&graphdriver,&graphmode,"");
errorcode=graphresult();
if(errorcode != grOk) {
printf("\a");
cprintf("Graphics error: %s\n\r",
grapherrormsg(errorcode));
exit(1);
}
/*************************************************/
/* generate the pure and corrupted data profiles */
/*************************************************/
/* generate the clean profile */
profile(y_pure);
/* generate the noise */
gauss_noise(noise);
/* corrupt the clean data with noise */
for(x=0;x<PLOTEND;++x)
y_noisy[x] = y_pure[x] + noise[x];
/****************************************/
/* plot a pure profile with underdamped */
/* alpha-beta smoothing */
/****************************************/
/* plot the clean profile */
message("Clean data profile","","");
plot_it(y_pure);
/* compute and plot the smoothed result
over the clean profile */
alpha = ALPHA; /* arbitrary alpha value */
beta = alpha*alpha / (2.0 - alpha); /* standard,
underdamped
beta value */
alpha_beta(y_pure,y_smoothed,alpha,beta);
message("","with",
"Underdamped alpha-beta smoothing");
plot_it(y_smoothed);
/* clear the screen and replot
the smoothed result */
cleardevice();
message("Underdamped alpha-beta smoothing of clean \
profile","","");
plot_it(y_smoothed);
/*****************************************/
/* plot a noisy profile with underdamped */
/* alpha-beta smoothing */
/*****************************************/
/* clear the screen and plot the profile */
cleardevice();
message("Noisy profile","","");
plot_it(y_noisy);
/* compute and plot the smoothed result over the
noisy profile */
alpha_beta(y_noisy,y_smoothed,alpha,beta);
message("","with",
"Underdamped alpha-beta smoothing");
plot_it(y_smoothed);
/* clear the screen and replot the
smoothed result */
cleardevice();
message("Underdamped alpha-beta smoothing of noisy \
profile","","");
plot_it(y_smoothed);
/* plot the pure profile over the estimate based on
the noisy version */
message("","with","Clean data profile");
plot_it(y_pure);
/********************************************/
/* plot a pure profile with near-critically */
/* damped alpha-beta smoothing */
/********************************************/
/* clear the screen and plot the clean profile */
cleardevice();
message("Clean data profile","","");
plot_it(y_pure);
/* compute and plot the smoothed result over the
clean profile */
alpha = ALPHA; /* arbitrary alpha value */
beta = 0.8*(2.0 - alpha*alpha -
2.0*sqrt(1 - alpha*alpha))/(alpha*alpha);
alpha_beta(y_pure,y_smoothed,alpha,beta);
message("","with","Near-critically damped \
alpha-beta smoothing");
plot_it(y_smoothed);
/*********************************************/
/* plot a noisy profile with near-critically */
/* damped alpha-beta smoothing */
/*********************************************/
/* clear the screen and plot the noisy profile */
cleardevice();
message("Noisy profile","","");
plot_it(y_noisy);
/* compute and plot the smoothed response over the
noisy profile */
beta = 0.8*(2.0 - alpha*alpha -
2.0*sqrt(1 - alpha*alpha))/(alpha*alpha);
alpha_beta(y_noisy,y_smoothed,alpha,beta);
message("Noisy profile","with",
"Near-critically damped alpha-beta \
smoothing");
plot_it(y_smoothed);
/* clear the screen and replot the smoothed result */
cleardevice();
message("Near-critically damped alpha-beta \
smoothing of noisy profile","","");
plot_it(y_smoothed);
/* plot the pure profile over the estimate based on
the noisy version */
message(""," with","Clean data profile");
plot_it(y_pure);
/* prepare to exit */
closegraph();
}
void alpha_beta(double *y,double *y_smoothed,
double alpha,double beta)
/* demonstrate the alpha-beta algorithm */
{
int i;
double s_est,v_est,s_pred,error;
/* initialize the filter */
s_est = y[0];
v_est = 0.0;
/* loop through all of the position measurements */
for(i=0;i<PLOTEND;++i) {
/***********************************/
/* the alpha-beta filter algorithm */
/* (with T absorbed into v) */
/***********************************/
/* predict the new position */
s_pred = s_est + v_est;
/* compute the measurement error */
error = y[i] - s_pred;
/* estimate the true position and velocity */
s_est = s_pred + alpha*error;
v_est = v_est + beta*error;
/*****************************/
/* update the smoothed array */
/*****************************/
y_smoothed[i] = s_est;
}
}
void profile(double *y)
/* generate the profile to be studied */
{
int i;
/* first steady-state portion */
for(i=0;i<PLOT1;++i) y[i] = STEADY_STATE1;
/* ramp from first steady state level to second
steady state level */
for(i=PLOT1;i<PLOT2;++i)
y[i] = (i-PLOT1) * (STEADY_STATE2-STEADY_STATE1) /
(PLOT2-PLOT1) + STEADY_STATE1;
/* second steady-state portion */
for(i-PLOT2;i<PLOT3;++i) y[i] = STEADY_STATE2;
/* step to third steady-state portion */
for(i-PLOT3;i<PLOTEND;++i) y[i] = STEADY_STATE3;
}
void gauss_noise(double *noise)
/* generate mean-zero gaussian noise using Box-Muller
method */
{
int i;
double drand1,drand2,pi,sigma=10.0,mean=0.0;
/* define pi */
pi = 4.0 * atan(1.0);
/* seed the random number generator */
randomize();
/* generate gaussian noise using the Box-Muller
method */
for(i=0;i<PLOTEND;++i) {
/* compute two double random numbers */
drand1 = (double)rand)()/RAND_MAX;
drand2 = (double)rand()/RAND_MAX;
/* compute the noise term */
if(drand1 < 1e-10) drand1 = 1e-10; /* avoid
division
by zero */
noise[i] = sqrt(2.0*log(1.0/drand1)) *
cos(2.0 * pi * drand2) * sigma + mean;
}
}
void plot_it(double *y)
/* plot one member of array y for each horizontal
pixel position */
{
int x,y_old;
/* plot the profile */
y_old = (int)(y[0]+0.5);
for(x=0;x<PLOTEND;++x) {
line(x,y_old,x,(int)(y[x]+0.5));
y_old = (int)(y[x]+0.5);
}
/* wait for key press */
getch();
}
void message(char *string1,char *string2,
char *string3)
/* print a message of three lines centered at bottom of
screen */
{
int midx,maxy;
/* get screen parameters */
midx=getmaxx()/2;
maxy=getmaxy();
/* print pieces of message, centered */
settextjustify(CENTER_TEXT,TOP_TEXT);
outtextxy(midx,maxy-30,string1);
outtextxy(midx,maxy-20,string2);
outtextxy(midx,maxy-10,string3);
}
/* End of File */
