Working version
simple initialisation working
Further initialisation cleaned up
Initialisation divided on functions, main function the same
Further simplificaiton, just quaternions sending