Added SetContinuous-command
Make copy and assignment constructor of KalmanFilter private
Get very basic version working

TODO: use continuous mode of 3DM and measure time