Alpha-Beta-Gamma Filter

The ABG filter is a simplified version of a steady state Kalman filter.
Often it is practical to update the Kalman gains every iteration and
often one doesn't know the covarience of the system and measurement noise
so they are 'fudged' to get the desired results. It that is so then on can
chose the ABG filter were the one simply choses the bandwidth that yields
satisfactory results.

The prime use for the ABG filter for motion control is to filter reference
inputs like joy sticks or line speed encoders.

The ABG system matrix for 10 millisecond intervals.
The state x(n)=[[p(n)],[v(n)],[a(n)]]
The matrix is generate by
p(n+1)=p(n)+v(n)*δ+a(n)*δ^2/2
v(n+1)=v(n)+a(n)*δ
a(n+1)=a(n)
Basically the A matrix provides inertia for the filter. The A matrix
will not try to change state. Any corrections is done using the gains.

(%i48) numer: false$
'%delta=%delta: 1/100;
'A=A: matrix([1,%delta,%delta^2/2],[0,1,%delta],[0,0,1]);

The ABG system matrix for 1 millisecond intervals.
This matrix is used to estimate the state between samples.

(%i51) '%delta=%delta: 1/1000;
'A=A: matrix([1,%delta,%delta^2/2],[0,1,%delta],[0,0,1]);

10 iterations of the 1 millisecond system matrix is the same as
one iteration of the 10 millisecond system matrix.

(%i53) A^^10;

Calculate the matrix coefficients for the transition matrix and the ABG gains.
'K=K: matrix([3*%omega],[3*%omega^2],[%omega^3])*%delta; /* For 3 Pole LP ABG or SS Kalman gains */
'K=K: matrix([2*%omega],[2*%omega^2],[%omega^3])*%delta; /* For Butterworth ABG or SS Kalman gains */
'K=K: matrix([2.090001*%omega],[1.479343*%omega^2],[%omega^3])*%delta; /* For IAE */

(%i54) numer: true$ /* Do floating point calcuclations below here */
load(distrib)$ /* Load the random_normal function */
'delta=%delta: 0.01; /* ABG sampling time in seconds */
'resolution=resolution: 3/16384; /* Resolution 14 bit = 3 meters/16384 counts */
'jitter=jitter: 0.0005; /* Jitter value for Weibull distribution, seconds */
'%omega=%omega: float(2*%pi*2); /* ABG filter bandwidth, radians per second */
'A=A: matrix([1,%delta,%delta^2/2],[0,1,%delta],[0,0,1]);
'K=K: matrix([3*%omega],[3*%omega^2],[%omega^3])*%delta; /* ABG or SS Kalman gains */
'K=K: matrix([2.090001*%omega],[1.479343*%omega^2],[%omega^3])*%delta; /* For IAE */
'Amp=Amp: 1.0; /* Wave amplitude */
'Tw=Tw: 10; /* Wave period */
r(t):=Amp*matrix([sin(2*%pi*t/Tw)],
                [cos(2*%pi*t/Tw)*(2*%pi/Tw)],
                [-sin(2*%pi*t/Tw)*(2*%pi/Tw)^2]);
'x=x: r(-%delta);

Calculate the meassured positions
The time is varied to simulate sample jitter and the result is truncate to the feed back resolution

(%i67) ylist: makelist(floor(r(i*%delta+quantile_weibull(random(1.0),2,jitter))[1,1]/resolution)*resolution,i,0,2000)$

Calculate the estimated position, velocity and acceleration

(%i68) xt: transpose(x)$ /* Initialize xt, the transposed state */
for i:1 thru 2000 do
(
    x: A.x, /* Update estimated state */
    x: x+K*(ylist[i]-x[1,1]), /* Correct for errors between actual and estimated */
    xt: addrow(xt,transpose(x)) /* Append to end of array */
)$

Make lists for plotting

(%i70) '[Rows,Cols]=[Rows,Cols]: matrix_size(xt);
tlist: makelist((i-1)*%delta,i,1,Rows)$

Plot the positions

(%i72) xlist: makelist(xt[i,1],i,1,Rows)$
rlist: makelist(r((i-1)*%delta)[1][1],i,1,Rows)$
wxplot2d([[discrete,tlist,xlist],
          [discrete,tlist,ylist],
          [discrete,tlist,rlist]],
    [legend,"Estimated Position","Measured Position","Actual Position"],
    [xlabel,"time, sec"],[ylabel,"Position, m"]);

Plot the position error. There is a slight bias to the high side
because the measured position is rounded down.

(%i75) elist: makelist(r((i-1)*%delta)[1][1]-xt[i,1],i,1,Rows)$
wxplot2d([[discrete,tlist,elist]],
    [legend,"Estimated Position Error"],
    [xlabel,"time, sec"],[ylabel,"Estimated Position Error, m"]);

Plot the velocities

(%i77) xxlist: makelist(xt[i,2],i,1,Rows)$
rrlist: makelist(r((i-1)*%delta)[2][1],i,1,Rows)$
wxplot2d([[discrete,tlist,xxlist],[discrete,tlist,rrlist]],
    [legend,"Estimated Velocity","Actual Velocity"],
    [xlabel,"time, sec"],[ylabel,"Velocity, m/s"]);

(%i80) eelist: makelist(r((i-1)*%delta)[2][1]-xt[i,2],i,1,Rows)$
wxplot2d([[discrete,tlist,eelist]],
    [legend,"Estimated Velocity Error"],
    [xlabel,"time, sec"],[ylabel,"Estimated Velocity Error, m/s"]);

Plot the accelerations

(%i82) xxxlist: makelist(xt[i,3],i,1,Rows)$
rrrlist: makelist(r((i-1)*%delta)[3][1],i,1,Rows)$
wxplot2d([[discrete,tlist,xxxlist],[discrete,tlist,rrrlist]],
    [legend,"Estimated Acceleration","Actual Acceleration"],
    [xlabel,"time, sec"],[ylabel,"Acceleration, m/s^2"]);

(%i85) eeelist: makelist(r((i-1)*%delta)[3][1]-xt[i,3],i,1,Rows)$
wxplot2d([[discrete,tlist,eeelist]],
    [legend,"Estimated Accleration Error"],
    [xlabel,"time, sec"],[ylabel,"Estimated Acceleration Error, m/s^2"]);

Calculate standard deviation of estimated position, velocity and
acceleration from actual position, velocity and accelreation.

(%i87) 'PositionStdDev=sqrt(lsum(i^2,i,elist)/Rows);
'VelocityStdDev=sqrt(lsum(i^2,i,eelist)/Rows);
'AccelerationStdDev=sqrt(lsum(i^2,i,eeelist)/Rows);

Quantizing due to resolution.
Units are in meters and seconds

(%i90) 'PositionResolution=resolution;
'VelocityResolution=resolution/%delta;
'AccelerationResolution=resolution/%delta^2;

Weibull distribution for simulating sample jitter.
The probabilty distribution function
The inverse cumulative distribution function.

(%i93) wxplot2d(pdf_weibull(i,2,jitter),[i,0.0,%delta]);
wxplot2d(quantile_weibull(i,2,jitter),[i,0.0,1.0]);