Initial Kalman filter, 3D with velocity, where seen is 3D points.  Edited from original... not sure where I got it from though