save result to mat file
ignore txt
save trajectory to text file
convert from z-y'-x'' to quaternion