Some small changes for first test in optitrack. This version is flying in the optitrack!