minor bugfix: quaternion representation of matlab is not conform with the one in this work. And (c) added
minor imporvement... but still not working as it should