first implementation of estimateTwWithCovFromTrajectory, not working still buggy