Robotcode
1.0

Kalman filter for efficient and optimal velocity estimation based on (noisy) position data with possibly noisy timing. More...
#include <KalmanFilter.h>
Public Member Functions  
KalmanFilter ()  
void  init (double *y0, double w=30, double v=0.0005, double dt=0.001) 
initialize fresh  
void  update (double *y) 
Update Kalman filter with new measurement and standard dt.  
void  update (double *y, double dt) 
Update Kalman filter with new measurement and dt.  
void  velocity (double *v) 
Return filtered velocity.  
void  position (double *p) 
Return filtered position.  
Public Attributes  
Vector2D  x [N] 
Current state on nth channel.  
Vector2D  xm 
current state after time update  
Vector2D  K 
Current Kalman gain.  
Matrix2D  P 
Current uncertainty matrix for all channels.  
Matrix2D  Pm 
Current uncertainty matrix after time update.  
Matrix2D  A 
Transition matrix.  
Matrix2D  Q 
Conditional uncertainty matrix.  
double  w2 
Variance of state transitions.  
double  v2 
Measurement variance.  
double  dt 
double  dt2 
double  dt3 
delta t and powers of 
Kalman filter for efficient and optimal velocity estimation based on (noisy) position data with possibly noisy timing.
Template class for ndimensional position data. All channels are treated as being independent and equal in variance and dt. Kalman filter assumes that acceleration is a Gaussian White noise process (velocity is brownian motion) . We thank Daniel Wolpert for pointing us to this method.
KalmanFilter< N >::KalmanFilter  (  ) 
N is the number of channels
void KalmanFilter< N >::init  (  double *  x0, 
double  W = 30 , 

double  V = 0.0005 , 

double  DT = 0.001 

) 
initialize fresh
*x0  array of initial states of the the channels 
W  standard devation of the prior on the accelerations 
V  noise standard deviation of the position observations 
DT  standard deltat, in case you want to precompute Kalman gains 