! Kalman filter (KalmanFilter);
! We have a system described at time t by a state vector: x(t);
! The state transition formula in matrix form is:
x(t) = F*x(t-1) + B*u(t) + e(t), where
e(t) is a random vector with covariance matrix Q,
Q is a covariance matrix that we re-estimate at each step,
F is a matrix that specifies how the state changes if no controls are applied,
u( t) is a vector of controls that have some effect on the state,
B is a matrix that specifies how controls affect the state;
! We may not have a perfect view of the state.
z(t) is a vector of what we measure/see. It is related to the true state by:
z(t) = H*x(t) + w(t), where
H is a matrix that relates how the true state is relate to what we see,
w(t) is a random vector with covariance matrix R;
!Examples:
1) GPS device: We want to predict vehicle location. When it loses GPS
signal, we can update estimated location only by dead reckoning.
When GPS signal is recovered, it gives noisy estimate of true state.
Combine step of Kalman filter combines the extrapolated and measured estimates.
2) Radar target estimation: Radar signal gives estimate of x & y distance.
True state is (x, y), (dx, dy), & (ddx, ddy). We get a fresh radar estimate
of the location every second. How do we update estimated state of object?
;
! Keywords: Forecasting, GPS, Kalman filter, Radar;
SETS:
state;
control;
measure;
! Make all the vectors, column vectors;
col/1..1/;
statec( state, col): xpt, XPU, XC1, XP1, xc0, xpp;
controlc( control, col): u;
measurec( measure, col): z1, hxz, deltam;
sxs( state, state): F, Q, FT, QC0, QP1, KHQ, QC1;
mxm( measure, measure): R, S, SINV;
sxc( state, control): B;
sxm( measure, state): H, RTMP;
mxs( state, measure): K, HT;
ENDSETS
DATA:
! Example cases.
! For portability and simplicity, we take the input internally.
To interface with spreadsheets, see @OLE(),
To interface with SQL databases, see @ODBC(),
To interface with text files, see
@FILE(), @TEXT(), @READLN(), @READRM(),
To interface with other programs, see @POINTER;
! You can turn off Casex by replacing Casex; ! You can turn on Casex by replacing Casex by Casex;
! Case 1: A vehicle moving in 1 dimensional space;
! State space is its distance and speed (change in distance/second);
!Case1; state = distance speed! We regularly get noisy measures of its distance, call it location;
!Case1; measure = radar!Case1; control = acceleration! Suppose period length is h = 0.5 seconds. The transition matrix is, i.e.,
new distance = 0ld distance + speed*h,
new speed = old speed;
!Case1;
F = 1 0.5
0 1;
! Suppose the only control is an accelerating force, so
change in distance in one step is u*(1/2)*h^2 and change
in speed is u*h;
!Case1; B = 0.125 0.5! A plausible covariance matrix for the state in one transition is,
e.g., speed and distance are positively correlated;
!Case1;
Q = 0.0078 0.0313
0.0313 0.125;
! Covariance matrix for measured values;
!Case1; R = 0.001! Matrix for converting state to measured;
!Case1; H = 1 0
! Now get the process started;
! Estimate of initial state;
! Distance in km, Speed in km/half second;
!Case1;
xc0 = 100 0.25;
! Our first control vector;
!Case1; u = 0
! Use starting covariance matrix in first period;
!Case1;
QC0 = 0.0078 0.0313
0.0313 0.125;
! Our first observation/measurement;
!Case1; z1 = 100.3
! Case 2: A high altitude balloon. We want to estimate its
altitude at any moment.
! State space is its height;
!Case2 state = height;
! We regularly get noisy measures of its height, call it range;
!Case2 measure = range;
!Case2 control = acceleration;
! Suppose period length 1 second. The transition matrix is, i.e.,
new distance = 0ld distance + speed*h,
new speed = old speed;
!Case2
F = 1 ;
! There are no control forces operating on the balloon;
!Case2 B = 0;
! A plausible covariance matrix for the system noise
!Case2 Q = 0.0001;
! Covariance matrix for measured values;
!Case2 R = 0.1;
! Matrix for converting state to measured;
!Case2 H = 1;
! Now get the process started;
! Estimate of initial state. Original location is 0;
! Distance;
!Case2 xc0 = 0;
! Our first control vector;
!Case2 u = 0;
! Starting covariance matrix in first period.
We are not very confident initially;
!Case2
QC0 = 1000;
! Our first observation/measurement;
!Case2 z1 = 0.9;
ENDDATA
procedure kalmanxtrap:
! Extapolate step for Kalman filter;
! Inputs:
xc0 = vector of estimated state of previous period,
QC0 = matrix of estimated covariance matrix of
previous state,
F = matrix of transition rule for getting next state,
Q = matrix of additive error covariance matrix for state transition
B = matrix of effect of control variables on state,
u = vector of control variables
Outputs:
XP1 = vector projected estimate of new state based on extrapolation,
QP1 = matrix estimate of covariance of new state,
based on extrapolation
;
! Extrapolate to new state, essentially, xp(t) = F*xc(t-1) + B*u(t)
effect of transition rule;
xpt = @MTXMUL( F, xc0);
! effect of control variables;
XPU = @MTXMUL( B, u);
! Combine to get extrapolated/Predicted state;
@FOR( statec: xp1 = xpt + xpu);
! Extrapolate covariance matrix of new state, essentially,
QP(t) = F*QC(t-1)*F' + Q;
QP1 = @MTXMUL( F, QC0);
FT = @TRANSPOSE( F); ! Transpose of F;
QP1 = @MTXMUL( QP1, FT);
! Add in system noise: QP1 = QP1 + Q;
@FOR( sxs(s1,s2): QP1(s1,s2) = QP1(s1,s2) + Q(s1,s2));
ENDprocedure
procedure kalmancombine:
! Combine latest measurement with extrapolation.
! The measurement model is:
z(t) = H*x(t) + noise;
! Inputs:
xp1 = vector estimate of current state based just on extrapolation,
QP1 = matrix estimate of covariance of current state,
based just on extrapolation,
z1 = vector of measurements observed this period,
H = matrix describing how state transforms into measurements,
R = matrix of covariances of the measurements,
z1 = vector of observations this period,
Outputs:
xc1 = vector estimate of new state based on combining measurement
information with the forecasted extrapolation.
QC1 = matrix estimate of the covariance of new state,
after combining extrapolation and measurement.
! Compute difference between measured, z, and predicted measured.
Loosely speaking delta = z - H*x;
hxz = @MTXMUL( H, XP1);
@FOR( measurec: deltam = z1 - hxz);
! Covariance matrix of measured values;
! Loosely, S = R + H*Q*H'
! Calculate H*Q;
RTMP = @MTXMUL( H, QP1);
! and H*Q*H';
HT = @TRANSPOSE( H);
S = @MTXMUL( RTMP, HT);
! Add in measurement noise;
@FOR(mxm: S = S + R);
! Kalman gain or adjustment;
! Loosely, K = P*H*Sinverse;
SINV = @INVERSE( S);
K = @MTXMUL( QP1, HT);
K = @MTXMUL( K, SINV);
! Updated estimate of state, combining extrapolation + measurement;
! Loosely, X = X + K*delta;
XC1 = @MTXMUL( K, deltam);
@FOR(statec: xc1 = xp1 + xc1);
! Updated estimate of state covariance matrix;
! Loosely, QC1 = QP1 - K*H*QP1;
KHQ = @MTXMUL( K, H);
KHQ = @MTXMUL( KHQ, QP1);
@FOR( sxs(s1,s2): QC1( s1, s2) = QP1( s1, s2) - KHQ( s1, s2));
ENDprocedure
CALC:
! Do one step of example calculations;
@WRITE(' Initial state:', @NEWLINE(1) );
@FOR( state(s1):
@WRITE(' ', state(s1),'= ', @FORMAT(xc0(s1,1),'9.3f'));
);
@WRITE( @NEWLINE(1));
@WRITE(' Initial state covariances:', @NEWLINE(1) );
@FOR( state(s1):
@FOR( state(s2):@WRITE(' ', @FORMAT(QC0(s1,s2),'9.4f'));
);
@WRITE( @NEWLINE(1));
);
@WRITE( @NEWLINE(1));
@WRITE(' Initial control vector:', @NEWLINE(1) );
@FOR( control(s1):
@WRITE(' ', control(s1),'= ', @FORMAT(u(s1,1),'9.3f'));
);
@WRITE( @NEWLINE(1));
!Do the extrapolation;
kalmanxtrap;
@WRITE(' Extrapolated state:', @NEWLINE(1) );
@FOR( state(s1):
@WRITE(' ', state(s1),'= ', @FORMAT(xp1(s1,1),'9.3f'));
);
@WRITE( @NEWLINE(1));
@WRITE(' Extrapolated state covariances:', @NEWLINE(1) );
@FOR( state(s1):
@FOR( state(s2):@WRITE(' ', @FORMAT(QP1(s1,s2),'9.4f'));
);
@WRITE( @NEWLINE(1));
);
@WRITE( @NEWLINE(1));
@WRITE(' Measured/observation:', @NEWLINE(1) );
@FOR( measure(m1):
@WRITE(' ', measure(m1),'= ', @FORMAT(z1(m1,1),'9.3f'));
);
@WRITE( @NEWLINE(1));
! Combine the extrapolated and measured to get a final forecast;
kalmancombine;
@WRITE(' Difference between measured and extrapolated :', @NEWLINE(1) );
@FOR( measure(m):
@WRITE(' ', measure(m),'= ', @FORMAT( deltam(m,1),'9.3f'));
);
@WRITE( @NEWLINE(1));
@WRITE(' Kalman gain/adjustment matrix:', @NEWLINE(1) );
@FOR( state(s1):
@FOR( measure(m2):@WRITE(' ', @FORMAT(K(s1,m2),'9.4f'));
);
@WRITE( @NEWLINE(1));
);
@WRITE( @NEWLINE(1));
@WRITE(' Combined estimate of state:', @NEWLINE(1) );
@FOR( state(s1):
@WRITE(' ', state(s1),'= ', @FORMAT(xc1(s1,1),'9.4f'));
);
@WRITE( @NEWLINE(1));
@WRITE(' Combined/adjusted state covariances:', @NEWLINE(1) );
@FOR( state(s1):
@FOR( state(s2):@WRITE(' ', @FORMAT(QC1(s1,s2),'9.4f'));
);
@WRITE( @NEWLINE(1));
);
@WRITE( @NEWLINE(1));
! The above illustrates just one iteration/period. To do subsequent interations,
one would have to do the update in each iteration/period:
xc0 by xc1,
QC0 by QC1,
z1 = vector of measurements observed new period;
ENDCALC
|
