Adding a Function to Tudat#