1 files changed, 12 insertions, 0 deletions

diff --git a/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m b/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m
new file mode 100644
index 0000000..3d2ac76
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m
@@ -0,0 +1,12 @@
+function retQuat=addEulNoise(quat, STD)
+n=size(quat,1);
+
+% KVS: replacing randnWrapper(n,3) with rand(n,3)
+gyro=randnWrapper(n,3)*STD;
+
+norm_gyro=sqrt(sum(gyro.^2,2));
+angleAlpha=norm_gyro;
+quatDelta=[cos(angleAlpha/2), gyro./(norm_gyro*ones(1,3)).*(sin(angleAlpha/2)*ones(1,3))];
+
+retQuat=quatMul(quat, quatDelta);
+