I was able to create a s-function for simulink.
The function imports the angel and position data from tracking tools.
Hope this is helpfull for some of you.
i created a Matlab s-function for Motive, very similar to your code.
But i can't build code with Simulink Coder. I need a C mex s-function to do this.
Have you or someone else experience with that?