From f618466c25d43f3bae9e40920273bf77de1e1149 Mon Sep 17 00:00:00 2001
From: leochanj105 <leochanj@live.unc.edu>
Date: Mon, 19 Oct 2020 23:09:30 -0400
Subject: initial sd-vbs

initial sd-vbs

add sd-vbs

sd-vbs
---
 .../localization/src/matlab/addEulNoise.m          |  12 ++
 .../localization/src/matlab/calculate3DGaussian.m  |  11 ++
 .../benchmarks/localization/src/matlab/drawLog.m   |  44 +++++
 .../benchmarks/localization/src/matlab/eul2quat.m  |   9 +
 .../localization/src/matlab/generateSample.m       |  25 +++
 .../localization/src/matlab/get3DGaussianProb.m    |  22 +++
 .../localization/src/matlab/getGroundData.m        |   5 +
 SD-VBS/benchmarks/localization/src/matlab/mcl.m    |   9 +
 .../benchmarks/localization/src/matlab/mclWhole.m  |  18 ++
 .../benchmarks/localization/src/matlab/quat2eul.m  |   6 +
 .../benchmarks/localization/src/matlab/quatConj.m  |   5 +
 .../benchmarks/localization/src/matlab/quatMul.m   |  18 ++
 .../benchmarks/localization/src/matlab/quatRot.m   |  24 +++
 .../benchmarks/localization/src/matlab/readLoc.m   |  53 ++++++
 .../localization/src/matlab/readMatrix.m           |  28 +++
 .../localization/src/matlab/readSensorData.m       |  46 +++++
 .../localization/src/matlab/script_run_profile.m   | 209 +++++++++++++++++++++
 .../benchmarks/localization/src/matlab/selfCheck.m |  33 ++++
 SD-VBS/benchmarks/localization/src/matlab/sumCol.m |  14 ++
 .../localization/src/matlab/weightedSample.m       |  20 ++
 20 files changed, 611 insertions(+)
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/drawLog.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/eul2quat.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/generateSample.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/getGroundData.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/mcl.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/mclWhole.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/quat2eul.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/quatConj.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/quatMul.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/quatRot.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/readLoc.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/readMatrix.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/readSensorData.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/selfCheck.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/sumCol.m
 create mode 100644 SD-VBS/benchmarks/localization/src/matlab/weightedSample.m

(limited to 'SD-VBS/benchmarks/localization/src/matlab')

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);
+
diff --git a/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m b/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m
new file mode 100644
index 0000000..060713a
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m
@@ -0,0 +1,11 @@
+function [meanColor A]=calculate3DGaussian(data)
+n_data=size(data,1);
+n_channel=size(data,2);
+meanColor=mean(data);
+diff=double(data)-ones(n_data,1)*meanColor;
+diifTr = transpose(diff);
+Ainv=(diffTr*diff/n_data);
+AinvTr = transpose(Ainv);
+A=inv(AinvTr);
+
+
diff --git a/SD-VBS/benchmarks/localization/src/matlab/drawLog.m b/SD-VBS/benchmarks/localization/src/matlab/drawLog.m
new file mode 100644
index 0000000..f88e078
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/drawLog.m
@@ -0,0 +1,44 @@
+function drawLog(data, ground_data)
+subplot(1,3,1);
+[nr nc]=size(data);
+for i=1:nr
+    x=data(i,2);
+    y=data(i,3);
+    z=data(i,4);
+    R=[1 0 0; 0 cos(x) sin(x); 0 -sin(x) cos(x)]...
+        *[cos(y) 0 -sin(y); 0 1 0; sin(y) 0 cos(y)]...
+        *[cos(z) sin(z) 0; -sin(z) cos(z) 0; 0 0 1];
+    coord=R*[1 0 0]';
+    axis([-1 1 -1 1 -1 1]);axis on
+    plot3([0 coord(1)], [0 coord(2)], [0 coord(3)], 'b'); hold on
+end
+
+[nr nc]=size(ground_data);
+for i=1:nr
+x=ground_data(i,2);
+y=ground_data(i,3);
+z=ground_data(i,4);
+R=[1 0 0; 0 cos(x) sin(x); 0 -sin(x) cos(x)]...
+        *[cos(y) 0 -sin(y); 0 1 0; sin(y) 0 cos(y)]...
+        *[cos(z) sin(z) 0; -sin(z) cos(z) 0; 0 0 1];
+coord=R*[1 0 0]';
+coord2=R*[0 -1 0]';
+axis([-1 1 -1 1 -1 1]);axis on
+plot3([0 coord(1)], [0 coord(2)], [0 coord(3)], 'r'); hold on
+plot3([0 coord2(1)], [0 coord2(2)], [0 coord2(3)], 'g');
+end
+
+xlabel('x');
+ylabel('y');
+zlabel('z');
+hold off
+drawnow
+subplot(1,3,2);
+Xoffset=4422610;
+Yoffset=483660;
+axis([4422610-Xoffset 4422660-Xoffset 483620-Yoffset 483720-Yoffset]);axis on
+scatter(data(:,5)-Xoffset, data(:,6)-Yoffset, 8, 'b'); hold on
+axis([4422610-Xoffset 4422660-Xoffset 483620-Yoffset 483720-Yoffset]);axis on
+scatter(ground_data(:,5)-Xoffset, ground_data(:,6)-Yoffset, 10, 'r'); hold off
+drawnow
+
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)];
+    
diff --git a/SD-VBS/benchmarks/localization/src/matlab/generateSample.m b/SD-VBS/benchmarks/localization/src/matlab/generateSample.m
new file mode 100644
index 0000000..f3658f8
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/generateSample.m
@@ -0,0 +1,25 @@
+function [retQuat retVel retPos]=generateSample(w, quat, vel, pos, STDDEV_VEL, STDDEV_POS)
+sampledXId=weightedSample(w);
+
+rows = size(sampledXId, 1);
+cols = size(sampledXId, 2);
+
+if(cols > 1)
+    disp(123456);
+end
+
+% retQuat = zeros(rows, 1);
+% retVel = zeros(rows, 1);
+% retPos = zeros(rows, 1);
+
+% for i=1:rows
+%     retQuat(i,1) = quat(sampleXId(i,1),1);
+%     retVel(i,1) = vel(sampleXId(i,1),1) + randnWrapper(1,1) * STDDEV_VEL;
+%     retPos(i,1) = pos(sampleXId(i,1),1) + randnWrapper(1,1) * STDDEV_POS;    
+% end
+
+retQuat=quat(sampledXId,:);
+retVel=vel(sampledXId,:);%+randnWrappern(n,3)*STDDEV_VEL;
+retPos=pos(sampledXId,:);%+randnWrappern(n,3)*STDDEV_POS;
+
+
diff --git a/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m b/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m
new file mode 100644
index 0000000..6a54530
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m
@@ -0,0 +1,22 @@
+function p=get3DGaussianProb(data, mean, A)
+n_data=size(data,1);
+n_channel=size(data,2);
+
+p=zeros(n_data,1);
+diff=(data)-ones(n_data,1)*mean;
+detA = 1; %detA = det(A)
+dotA = randWrapper(size(diff,1),1); %dotA = dot(diff*A, diff, 2)
+p=sqrt(detA/((2*pi)^n_channel))*exp(-0.5*dotA);
+
+%% KVS If the above doesnt work, try uncommenting these lines below
+
+%%temp = (det(A)/((2*pi)^n_channel));
+%temp = (1.0/((2*pi)^n_channel));
+%temp1 = dot(diff*A,diff,2);
+%%temp1 = rand(1000,1); 
+%temp2 = exp(-0.5*temp1);
+%p = sqrt(temp) * exp(temp2);
+%
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m b/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m
new file mode 100644
index 0000000..7cf5f9c
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m
@@ -0,0 +1,5 @@
+function retData=getGroundData(data, tStamp)
+idx=find(data(:,1)==tStamp);
+retData=data(idx,:);
+
+
diff --git a/SD-VBS/benchmarks/localization/src/matlab/mcl.m b/SD-VBS/benchmarks/localization/src/matlab/mcl.m
new file mode 100644
index 0000000..8ab8aa6
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/mcl.m
@@ -0,0 +1,9 @@
+function [retX, retW]=mcl(x,sData, invCov)
+%instead of using importance resampling, I assumed 3D gaussian for each
+%particle
+%retX=x+randn(nr,nc)*(invCov^-1); %add noise
+retX=x;
+retW=get3DGaussianProb(retX, sData, invCov);
+retW=retW/sum(retW);
+
+
diff --git a/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m b/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m
new file mode 100644
index 0000000..4e78b8f
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m
@@ -0,0 +1,18 @@
+function retW=mclWhole(quat, Ovel, STDDEV_GPSVel, accl, STDDEV_ACCL)
+n=size(quat,1);
+OvelNorm=norm(Ovel);
+if (OvelNorm>0.5)
+    Ovel=Ovel/norm(Ovel);
+    %trick
+    %quat=addEulNoise(quat, STDDEV_GPSVel); 
+    orgWorld=quatRot([1 0 0],quatConj(quat));
+    p1=get3DGaussianProb(orgWorld, Ovel, eye(3)./STDDEV_GPSVel);
+else
+    p1=zeros(n,1);
+end
+
+gravity=quatRot(ones(n,1)*[0 0 -9.8], quat);
+p2=get3DGaussianProb(gravity, accl, eye(3)./(STDDEV_ACCL));
+retW=p1+p2;
+retW=retW/sum(retW);
+            
\ No newline at end of file
diff --git a/SD-VBS/benchmarks/localization/src/matlab/quat2eul.m b/SD-VBS/benchmarks/localization/src/matlab/quat2eul.m
new file mode 100644
index 0000000..a6b48ee
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/quat2eul.m
@@ -0,0 +1,6 @@
+function retEul=quat2eul(quat)
+retEul=[atan2(2*(quat(:,3).*quat(:,4)+quat(:,1).*quat(:,2))...
+               ,(quat(:,1).^2 -quat(:,2).^2 -quat(:,3).^2 +quat(:,4).^2)) ...
+        asin(-2*(quat(:,2).*quat(:,4)-quat(:,1).*quat(:,3))) ...
+        atan2(2*(quat(:,2).*quat(:,3)+quat(:,1).*quat(:,4))...
+               ,(quat(:,1).^2 +quat(:,2).^2 -quat(:,3).^2 -quat(:,4).^2))];
diff --git a/SD-VBS/benchmarks/localization/src/matlab/quatConj.m b/SD-VBS/benchmarks/localization/src/matlab/quatConj.m
new file mode 100644
index 0000000..ff494b4
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/quatConj.m
@@ -0,0 +1,5 @@
+function retQuat=quatConj(a)
+rows = size(a,1);
+retQuat = zeros(rows, 4);
+retQuat=[a(:,1), -a(:,2), -a(:,3), -a(:,4)];
+         
diff --git a/SD-VBS/benchmarks/localization/src/matlab/quatMul.m b/SD-VBS/benchmarks/localization/src/matlab/quatMul.m
new file mode 100644
index 0000000..b397554
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/quatMul.m
@@ -0,0 +1,18 @@
+function retQuat=quatMul(a, b)
+
+rowsa = size(a,1);
+colsa = size(a,2);
+
+rowsb = size(b,1);
+colsb = size(b,2);
+
+retQuat=[a(:,1).*b(:,1) - a(:,2).*b(:,2) - a(:,3).*b(:,3) - a(:,4).*b(:,4) ...
+    a(:,1).*b(:,2) + a(:,2).*b(:,1) + a(:,3).*b(:,4) - a(:,4).*b(:,3) ...
+    a(:,1).*b(:,3) - a(:,2).*b(:,4) + a(:,3).*b(:,1) + a(:,4).*b(:,2) ...
+    a(:,1).*b(:,4) + a(:,2).*b(:,3) - a(:,3).*b(:,2) + a(:,4).*b(:,1)];
+
+% disp('retQuat');
+% disp(retQuat);
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/matlab/quatRot.m b/SD-VBS/benchmarks/localization/src/matlab/quatRot.m
new file mode 100644
index 0000000..3688172
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/quatRot.m
@@ -0,0 +1,24 @@
+function ret=quatRot(vec, rQuat)
+nr=size(vec,1);
+tv = zeros(nr,1);
+vQuat=[tv, vec];
+temp = quatMul(rQuat, vQuat);
+temp1 = quatConj(rQuat);
+retVec = quatMul(temp, temp1);
+%retVec=quatMul(quatMul(rQuat, vQuat),quatConj(rQuat));
+%ret=retVec(:,2:4);
+rows = size(retVec, 1);
+ret = zeros(rows,3);
+
+for i=1:rows
+    k =1;
+    for j=2:4
+        ret(i,k) = retVec(i,j);
+        k = k+1;
+    end
+end
+%size(ret)
+
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/matlab/readLoc.m b/SD-VBS/benchmarks/localization/src/matlab/readLoc.m
new file mode 100644
index 0000000..fc679d4
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/readLoc.m
@@ -0,0 +1,53 @@
+function out = readLoc
+out = [...
+10.512346 3.466955 9.006616 ;...
+13.242645 4.280358 14.675757 ;...
+12.418589 6.286052 22.721924 ;...
+13.598760 5.229711 19.622804 ;...
+13.568058 5.229711 19.622804 ;...
+13.537888 5.229711 19.622804 ;...
+13.508315 5.229711 19.622804 ;...
+13.479378 5.229711 19.622804 ;...
+13.450420 5.229711 19.622804 ;...
+13.345976 7.607621 29.644342 ;...
+13.500197 7.803792 32.248338 ;...
+13.354279 10.248983 42.939018 ;...
+13.500197 10.442337 45.593042 ;...
+13.354279 12.925516 56.404386 ;...
+13.700372 10.538259 47.007521 ;...
+13.669945 10.538259 47.007521 ;...
+13.640058 10.538259 47.007521 ;...
+13.608179 10.538259 47.007521 ;...
+13.576901 10.538259 47.007521 ;...
+13.545771 10.538259 47.007521 ;...
+13.731515 12.682360 55.887526 ;...
+1.016192 12.058634 55.442021 ;...
+6.263918 15.094210 64.436498 ;...
+6.263918 18.208480 70.198939 ;...
+6.263918 20.848498 78.618094 ;...
+6.263918 23.887747 84.333007 ;...
+6.263918 22.065907 78.853015 ;...
+6.237774 22.065907 78.853015 ;...
+6.211166 22.065907 78.853015 ;...
+6.183471 22.065907 78.853015 ;...
+6.155114 22.065907 78.853015 ;...
+6.126730 22.065907 78.853015 ;...
+6.154098 26.480108 89.543387 ;...
+6.168298 28.571946 93.679254 ;...
+6.157243 32.868572 105.963003 ;...
+6.155032 35.245287 110.401650 ;...
+6.155032 38.531472 119.557291 ;...
+6.155032 36.849296 111.296948 ;...
+6.128369 36.849296 111.296948 ;...
+6.101609 36.849296 111.296948 ;...
+6.073750 36.849296 111.296948 ;...
+6.046859 36.849296 111.296948 ;...
+6.019815 36.849296 111.296948 ;...
+6.046273 40.863016 123.894104 ;...
+6.060599 43.054591 127.680881 ;...
+6.049203 47.179478 140.858302 ;...
+6.046924 49.530293 145.484328 ;...
+6.046924 52.526113 153.189673 ;...
+6.046924 51.113294 146.241709 ;...
+6.021072 51.113294 146.241709 ...
+];
\ No newline at end of file
diff --git a/SD-VBS/benchmarks/localization/src/matlab/readMatrix.m b/SD-VBS/benchmarks/localization/src/matlab/readMatrix.m
new file mode 100644
index 0000000..fe684c5
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/readMatrix.m
@@ -0,0 +1,28 @@
+function readMatrix(srcImage, outName)
+
+write = fopen([outName '.m'], 'w');
+
+count = fwrite(write, 'function out = ');
+count = fwrite(write, outName);
+fprintf(write, '\n');
+count = fwrite(write, 'out = [...');
+fprintf(write, '\n');    
+
+height = size(srcImage,1);
+width = size(srcImage,2);
+
+for nI=1:height
+    for nJ=1:width
+        fprintf(write, '%f ', srcImage(nI,nJ));
+    end
+    if(nI < height)
+        fprintf(write, ';...\n');
+    end
+end
+
+fprintf(write, '...\n');
+count = fwrite(write, '];');
+
+fclose(write);
+
+end
diff --git a/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m b/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m
new file mode 100644
index 0000000..e84a6db
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m
@@ -0,0 +1,46 @@
+function [retTStamp, retType, retData, retEOF, index]=readSensorData(index1, fid)
+
+index = index1+1;
+rows = size(fid, 1);
+retTStamp = 0;
+retType = 0;
+retData = zeros(1,8);
+
+if(index > rows)
+    retEOF = 1;
+else
+%     for i=index:rows
+%         index = i;
+%         if(fid(i,2) == 4)
+%             break;
+%         end
+%     end
+    if(index == rows)
+        retEOF = 1;
+    else
+        retEOF = 0;
+    end
+
+    k = index;
+    retTStamp=fid(k,1);
+    retType=fid(k,2);
+    if(fid(k, 2) == 1 || fid(k, 2) == 2 || fid(k, 2) == 3)
+        index = k;
+        for i=1:3
+            retData(1,i)=fid(k,i+2);
+%            fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i);
+        end
+    end
+    if(fid(k, 2) == 4)
+        index = k;
+        for i=1:3
+            retData(1,i)=fid(k,i+2);
+%            fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i);
+        end
+        for i=4:8
+            retData(1,i) = fid(k+1,i-3);
+%            fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i);
+        end
+        index = index + 1;        
+    end    
+end
diff --git a/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m b/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m
new file mode 100644
index 0000000..44ce3aa
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m
@@ -0,0 +1,209 @@
+function script_run_profile(dataDir, resultDir, type, common, toolDir)
+
+path(path, common);
+file = fopen([dataDir, '/1.txt'], 'r');
+full = fscanf(file,'%f');
+elapsed = zeros(1,2);
+
+rows = full(2);
+cols = full(1);
+fid = zeros(rows, cols);
+
+k = 3;
+for i=1:rows
+    for j =1:cols
+        fid(i,j) = full(k);
+        k = k+1;
+    end
+end
+
+fclose(file);
+
+n=1000;
+
+gyroTimeInterval=0.01;
+acclTimeInterval=0.01;
+
+STDDEV_GPSVel=0.5;
+
+STDDEV_ODOVel=0.1;
+STDDEV_ACCL=1;
+M_STDDEV_GYRO=0.1;
+M_STDDEV_POS=0.1;
+M_STDDEV_VEL=0.02;
+
+if(strcmp(type,'test'))
+        n = 3;
+        gyroTimeInterval = 0.1;
+        acclTimeInterval = 0.1;
+        M_STDDEV_VEL = 0.2;
+    
+elseif(strcmp(type, 'sim_fast'))
+        n = 3;
+elseif(strcmp(type, 'sim'))
+        n = 10;
+elseif(strcmp(type, 'sqcif'))
+        n = 800;
+elseif(strcmp(type, 'qcif'))
+        n = 500;
+elseif(strcmp(type, 'vga'))
+        n = 2000;
+elseif(strcmp(type, 'wuxga'))
+        n = 3000;
+end
+
+fprintf(1,'Input size\t\t- (%dx%dx%d)\n', rows, cols,n);
+pos=zeros(n,3);
+
+vel=zeros(n,3) + randWrapper(n,3)*STDDEV_ODOVel;
+pi = 3.1416;
+
+eul1 = eul2quat([zeros(n,2), randWrapper(n,1)*2*pi]);
+eul2 = eul2quat([pi, 0, 0]);
+quat=quatMul(eul1, eul2);
+
+i=0;
+index = 0;
+resultMat = zeros(3,rows);
+
+while 1
+    i=i+1;
+    [tStamp, sType, sData, isEOF, index] = readSensorData(index, fid);
+
+    start = photonStartTiming;
+
+    if(sType==2)
+
+
+        %Motion model
+        gyro = sData(1, 1:3);
+        randnWrapper_mat = randnWrapper(n,3);    % KVS: We do not have function implementation for randnWrapper()
+        gyro=ones(n,1)*gyro+randnWrapper_mat*M_STDDEV_GYRO;
+
+        abc = gyro.^2;
+        abcd = sumCol(abc);
+        norm_gyro = sqrt(abcd);
+%        norm_gyro=sqrt(sum(gyro.^2,2));
+        angleAlpha=norm_gyro*gyroTimeInterval;
+        quatDelta=[cos(angleAlpha/2), gyro./(norm_gyro*ones(1,3)+0.00000001).*(sin(angleAlpha/2)*ones(1,3))];
+        quat=quatMul(quat, quatDelta);
+
+    end
+
+    if(sType==4)
+        %Observation
+        STDDEV_GPSPos=[sData(1,7), 0, 0; 0, sData(1,8), 0; 0, 0, 15];
+        Opos=sData(1,1:3);
+
+        %Initialize
+        
+        randnWrapper_mat = randnWrapper(n,3);
+        if sum(sum(pos))==0
+            pos=ones(n,1) * Opos + randnWrapper_mat*STDDEV_GPSPos;
+        else 
+           rows = size(STDDEV_GPSPos, 1);
+           cols = size(STDDEV_GPSPos, 2);
+
+            temp_STDDEV_GPSPos = ones(rows,cols);
+            for mnrows=1:rows                                 % KVS" Photon rejects this loop becasue of too many nestings ??
+                for mncols=1:cols
+                    temp_STDDEV_GPSPos(mnrows, mncols) = power(STDDEV_GPSPos(mnrows,mncols),-1);
+                end
+            end
+           [temp, w]=mcl(pos, Opos , temp_STDDEV_GPSPos);
+           [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
+       end
+
+       %compare direction
+       Ovel=sData(1,4:6);
+
+%        KVS: We do not have function for norm() yet! So replacing this operating with OvelNorm
+  
+%        OvelNorm=norm(Ovel);                               
+       OvelNorm= 2.0;  %1.1169e+09;
+       
+       if (OvelNorm>0.5)
+       
+           % KVS: Similar here: No impln of norm(), so replacing
+           % norm(Ovel) with OvelNorm value
+           
+           Ovel=Ovel/OvelNorm;
+           %trick
+           %quat=addEulNoise(quat, STDDEV_GPSVel);
+           qConj = quatConj(quat);
+            orgWorld=quatRot([1, 0, 0],qConj);
+            eye3 = [1,0,0;0,1,0;0,0,1];
+            [temp, w]=mcl(orgWorld, Ovel, eye3./STDDEV_GPSVel);
+            [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
+       end
+ 
+   end
+   
+   if(sType==1)
+
+       %Observation
+       Ovel=sData(1,1);
+       [temp, w]=mcl(sqrt(vel(:,1).^2+vel(:,2).^2+vel(:,3).^2), Ovel, 1/(STDDEV_ODOVel));
+
+       [quat vel pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
+   end
+   
+   if(sType==3)
+       %Observation
+       accl=sData(1,1:3);
+       
+       gtemp = ones(n,1) * [0 0 -9.8];
+       
+       gravity=quatRot(gtemp, quat);
+        eye3 = [1,0,0;0,1,0;0,0,1];
+        [gravity, w]=mcl(gravity, accl, eye3./(STDDEV_ACCL));
+
+        [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
+
+        %Motion model
+        accl=ones(n,1)*accl;
+        accl=accl-gravity;
+        pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval ...
+            +1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2 ...
+            +randnWrapper(n,3)*M_STDDEV_POS;
+        vel=vel+accl*acclTimeInterval+randnWrapper(n,3)*M_STDDEV_VEL;
+
+   end
+   
+    stop = photonEndTiming;
+    
+    temp = photonReportTiming(start, stop);
+    elapsed(1) = elapsed(1) + temp(1);
+    elapsed(2) = elapsed(2) + temp(2);
+
+    % Self check
+
+    quatOut = 0;
+    posOut = 0;
+    velOut = 0;
+
+    for ij=1:(size(quat,1)*size(quat,2))
+        quatOut = quatOut + quat(ij);
+    end
+
+    for ij=1:(size(vel,1)*size(vel,2))
+        velOut = velOut + vel(ij);
+    end
+
+    for ij=1:(size(pos,1)*size(pos,2))
+        posOut = posOut + pos(ij);
+    end
+
+    resultMat(:, i) = [quatOut, velOut, posOut];
+
+    if (isEOF == 1)
+        break;
+    end
+end
+
+%% Timing
+photonPrintTiming(elapsed);
+
+%% Self checking %%
+fWriteMatrix(resultMat,dataDir);
+
diff --git a/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m b/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m
new file mode 100644
index 0000000..ac80ebd
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m
@@ -0,0 +1,33 @@
+function ret = selfCheck(in1, in2, tol)
+
+r1 = size(in1, 1);
+c1 = size(in1, 2);
+
+r2 = size(in2, 1);
+c2 = size(in2, 2);
+
+ret = 1;
+
+if r1~=r2 
+    disp(1298);
+    ret = 0;
+end
+
+if c1 ~= c2
+	disp(1297);
+	ret = 0;
+end
+
+for i=1:r1
+    if(ret == 0)
+        break;
+    end
+    for j=1:c1
+        if( abs(in1(i,j)-in2(i,j)) > tol)
+            ret = 0;
+            break;
+        end
+    end
+end
+
+
diff --git a/SD-VBS/benchmarks/localization/src/matlab/sumCol.m b/SD-VBS/benchmarks/localization/src/matlab/sumCol.m
new file mode 100644
index 0000000..ad0114e
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/sumCol.m
@@ -0,0 +1,14 @@
+function ret = sumCol(mat)
+
+row = size(mat, 1);
+col = size(mat, 2);
+
+ret = zeros(row, 1);
+
+for i=1:row
+	temp = 0;
+	for j=1:col
+		temp = temp + mat(i, j);
+	end
+	ret(i, 1) = temp;
+end
diff --git a/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m b/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m
new file mode 100644
index 0000000..dc9274b
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m
@@ -0,0 +1,20 @@
+%function x_gen_id=weightedSample(w)
+function bin=weightedSample(w)
+n=size(w,1);
+seed=randWrapper(n,1);
+bin = zeros(n,1);
+%x_gen_id=zeros(n,1);
+ for i=1:n
+     for j=1:n
+         if(seed(j,1) > 0)
+             bin(j,1) = bin(j,1) + 1;
+%             x_gen_id(j,1) = x_gen_id(j,1) + bin(j,1);
+         end
+     end
+%     bin = (seed>0);
+%     x_gen_id=x_gen_id+bin;
+     seed=seed-w(i,1);
+ end
+% x_gen_id = bin;
+%x_gen=x(:,x_gen_id);
+
-- 
cgit v1.2.2