1 files changed, 9 insertions, 0 deletions

diff --git a/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m b/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m
new file mode 100644
index 0000000..57001a8
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m
@@ -0,0 +1,9 @@
+function retQuat=eul2quat(eulAngle)
+x=eulAngle(:,1);
+y=eulAngle(:,2);
+z=eulAngle(:,3);
+retQuat=[cos(x/2).*cos(y/2).*cos(z/2)+sin(x/2).*sin(y/2).*sin(z/2) ...
+        sin(x/2).*cos(y/2).*cos(z/2)-cos(x/2).*sin(y/2).*sin(z/2) ...
+        cos(x/2).*sin(y/2).*cos(z/2)+sin(x/2).*cos(y/2).*sin(z/2) ...
+        cos(x/2).*cos(y/2).*sin(z/2)-sin(x/2).*sin(y/2).*cos(z/2)];
+