CalcTf3D: Added documentation
Filter3D stubs
Initial Kalman filter prototype
Kalman: Replaced angle-axis representation in state with quaternion
Updated TODO note for memcpys