diff --git a/util/quaternionDistance.m b/util/quaternionDistance.m
new file mode 100644
index 000000000000..15f59a819e55
--- /dev/null
+++ b/util/quaternionDistance.m
@@ -0,0 +1,4 @@
+function quat_norm = quaternionDistance(quat1,quat2)
+lambda = sum(bsxfun(@times,quat1,quat2),1);
+quat_norm = 1-abs(lambda);
+end