summaryrefslogtreecommitdiffstats
path: root/SD-VBS/benchmarks/localization/src
diff options
context:
space:
mode:
Diffstat (limited to 'SD-VBS/benchmarks/localization/src')
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m12
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m11
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/drawLog.m44
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/eul2quat.m9
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/generateSample.m25
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m22
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/getGroundData.m5
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/mcl.m9
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/mclWhole.m18
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/quat2eul.m6
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/quatConj.m5
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/quatMul.m18
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/quatRot.m24
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/readLoc.m53
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/readMatrix.m28
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/readSensorData.m46
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m209
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/selfCheck.m33
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/sumCol.m14
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/weightedSample.m20
20 files changed, 0 insertions, 611 deletions
diff --git a/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m b/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m
deleted file mode 100644
index 3d2ac76..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m
+++ /dev/null
@@ -1,12 +0,0 @@
1function retQuat=addEulNoise(quat, STD)
2n=size(quat,1);
3
4% KVS: replacing randnWrapper(n,3) with rand(n,3)
5gyro=randnWrapper(n,3)*STD;
6
7norm_gyro=sqrt(sum(gyro.^2,2));
8angleAlpha=norm_gyro;
9quatDelta=[cos(angleAlpha/2), gyro./(norm_gyro*ones(1,3)).*(sin(angleAlpha/2)*ones(1,3))];
10
11retQuat=quatMul(quat, quatDelta);
12
diff --git a/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m b/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m
deleted file mode 100644
index 060713a..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m
+++ /dev/null
@@ -1,11 +0,0 @@
1function [meanColor A]=calculate3DGaussian(data)
2n_data=size(data,1);
3n_channel=size(data,2);
4meanColor=mean(data);
5diff=double(data)-ones(n_data,1)*meanColor;
6diifTr = transpose(diff);
7Ainv=(diffTr*diff/n_data);
8AinvTr = transpose(Ainv);
9A=inv(AinvTr);
10
11
diff --git a/SD-VBS/benchmarks/localization/src/matlab/drawLog.m b/SD-VBS/benchmarks/localization/src/matlab/drawLog.m
deleted file mode 100644
index f88e078..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/drawLog.m
+++ /dev/null
@@ -1,44 +0,0 @@
1function drawLog(data, ground_data)
2subplot(1,3,1);
3[nr nc]=size(data);
4for i=1:nr
5 x=data(i,2);
6 y=data(i,3);
7 z=data(i,4);
8 R=[1 0 0; 0 cos(x) sin(x); 0 -sin(x) cos(x)]...
9 *[cos(y) 0 -sin(y); 0 1 0; sin(y) 0 cos(y)]...
10 *[cos(z) sin(z) 0; -sin(z) cos(z) 0; 0 0 1];
11 coord=R*[1 0 0]';
12 axis([-1 1 -1 1 -1 1]);axis on
13 plot3([0 coord(1)], [0 coord(2)], [0 coord(3)], 'b'); hold on
14end
15
16[nr nc]=size(ground_data);
17for i=1:nr
18x=ground_data(i,2);
19y=ground_data(i,3);
20z=ground_data(i,4);
21R=[1 0 0; 0 cos(x) sin(x); 0 -sin(x) cos(x)]...
22 *[cos(y) 0 -sin(y); 0 1 0; sin(y) 0 cos(y)]...
23 *[cos(z) sin(z) 0; -sin(z) cos(z) 0; 0 0 1];
24coord=R*[1 0 0]';
25coord2=R*[0 -1 0]';
26axis([-1 1 -1 1 -1 1]);axis on
27plot3([0 coord(1)], [0 coord(2)], [0 coord(3)], 'r'); hold on
28plot3([0 coord2(1)], [0 coord2(2)], [0 coord2(3)], 'g');
29end
30
31xlabel('x');
32ylabel('y');
33zlabel('z');
34hold off
35drawnow
36subplot(1,3,2);
37Xoffset=4422610;
38Yoffset=483660;
39axis([4422610-Xoffset 4422660-Xoffset 483620-Yoffset 483720-Yoffset]);axis on
40scatter(data(:,5)-Xoffset, data(:,6)-Yoffset, 8, 'b'); hold on
41axis([4422610-Xoffset 4422660-Xoffset 483620-Yoffset 483720-Yoffset]);axis on
42scatter(ground_data(:,5)-Xoffset, ground_data(:,6)-Yoffset, 10, 'r'); hold off
43drawnow
44
diff --git a/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m b/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m
deleted file mode 100644
index 57001a8..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m
+++ /dev/null
@@ -1,9 +0,0 @@
1function retQuat=eul2quat(eulAngle)
2x=eulAngle(:,1);
3y=eulAngle(:,2);
4z=eulAngle(:,3);
5retQuat=[cos(x/2).*cos(y/2).*cos(z/2)+sin(x/2).*sin(y/2).*sin(z/2) ...
6 sin(x/2).*cos(y/2).*cos(z/2)-cos(x/2).*sin(y/2).*sin(z/2) ...
7 cos(x/2).*sin(y/2).*cos(z/2)+sin(x/2).*cos(y/2).*sin(z/2) ...
8 cos(x/2).*cos(y/2).*sin(z/2)-sin(x/2).*sin(y/2).*cos(z/2)];
9
diff --git a/SD-VBS/benchmarks/localization/src/matlab/generateSample.m b/SD-VBS/benchmarks/localization/src/matlab/generateSample.m
deleted file mode 100644
index f3658f8..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/generateSample.m
+++ /dev/null
@@ -1,25 +0,0 @@
1function [retQuat retVel retPos]=generateSample(w, quat, vel, pos, STDDEV_VEL, STDDEV_POS)
2sampledXId=weightedSample(w);
3
4rows = size(sampledXId, 1);
5cols = size(sampledXId, 2);
6
7if(cols > 1)
8 disp(123456);
9end
10
11% retQuat = zeros(rows, 1);
12% retVel = zeros(rows, 1);
13% retPos = zeros(rows, 1);
14
15% for i=1:rows
16% retQuat(i,1) = quat(sampleXId(i,1),1);
17% retVel(i,1) = vel(sampleXId(i,1),1) + randnWrapper(1,1) * STDDEV_VEL;
18% retPos(i,1) = pos(sampleXId(i,1),1) + randnWrapper(1,1) * STDDEV_POS;
19% end
20
21retQuat=quat(sampledXId,:);
22retVel=vel(sampledXId,:);%+randnWrappern(n,3)*STDDEV_VEL;
23retPos=pos(sampledXId,:);%+randnWrappern(n,3)*STDDEV_POS;
24
25
diff --git a/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m b/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m
deleted file mode 100644
index 6a54530..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m
+++ /dev/null
@@ -1,22 +0,0 @@
1function p=get3DGaussianProb(data, mean, A)
2n_data=size(data,1);
3n_channel=size(data,2);
4
5p=zeros(n_data,1);
6diff=(data)-ones(n_data,1)*mean;
7detA = 1; %detA = det(A)
8dotA = randWrapper(size(diff,1),1); %dotA = dot(diff*A, diff, 2)
9p=sqrt(detA/((2*pi)^n_channel))*exp(-0.5*dotA);
10
11%% KVS If the above doesnt work, try uncommenting these lines below
12
13%%temp = (det(A)/((2*pi)^n_channel));
14%temp = (1.0/((2*pi)^n_channel));
15%temp1 = dot(diff*A,diff,2);
16%%temp1 = rand(1000,1);
17%temp2 = exp(-0.5*temp1);
18%p = sqrt(temp) * exp(temp2);
19%
20
21
22
diff --git a/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m b/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m
deleted file mode 100644
index 7cf5f9c..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m
+++ /dev/null
@@ -1,5 +0,0 @@
1function retData=getGroundData(data, tStamp)
2idx=find(data(:,1)==tStamp);
3retData=data(idx,:);
4
5
diff --git a/SD-VBS/benchmarks/localization/src/matlab/mcl.m b/SD-VBS/benchmarks/localization/src/matlab/mcl.m
deleted file mode 100644
index 8ab8aa6..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/mcl.m
+++ /dev/null
@@ -1,9 +0,0 @@
1function [retX, retW]=mcl(x,sData, invCov)
2%instead of using importance resampling, I assumed 3D gaussian for each
3%particle
4%retX=x+randn(nr,nc)*(invCov^-1); %add noise
5retX=x;
6retW=get3DGaussianProb(retX, sData, invCov);
7retW=retW/sum(retW);
8
9
diff --git a/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m b/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m
deleted file mode 100644
index 4e78b8f..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m
+++ /dev/null
@@ -1,18 +0,0 @@
1function retW=mclWhole(quat, Ovel, STDDEV_GPSVel, accl, STDDEV_ACCL)
2n=size(quat,1);
3OvelNorm=norm(Ovel);
4if (OvelNorm>0.5)
5 Ovel=Ovel/norm(Ovel);
6 %trick
7 %quat=addEulNoise(quat, STDDEV_GPSVel);
8 orgWorld=quatRot([1 0 0],quatConj(quat));
9 p1=get3DGaussianProb(orgWorld, Ovel, eye(3)./STDDEV_GPSVel);
10else
11 p1=zeros(n,1);
12end
13
14gravity=quatRot(ones(n,1)*[0 0 -9.8], quat);
15p2=get3DGaussianProb(gravity, accl, eye(3)./(STDDEV_ACCL));
16retW=p1+p2;
17retW=retW/sum(retW);
18 \ 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
deleted file mode 100644
index a6b48ee..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/quat2eul.m
+++ /dev/null
@@ -1,6 +0,0 @@
1function retEul=quat2eul(quat)
2retEul=[atan2(2*(quat(:,3).*quat(:,4)+quat(:,1).*quat(:,2))...
3 ,(quat(:,1).^2 -quat(:,2).^2 -quat(:,3).^2 +quat(:,4).^2)) ...
4 asin(-2*(quat(:,2).*quat(:,4)-quat(:,1).*quat(:,3))) ...
5 atan2(2*(quat(:,2).*quat(:,3)+quat(:,1).*quat(:,4))...
6 ,(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
deleted file mode 100644
index ff494b4..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/quatConj.m
+++ /dev/null
@@ -1,5 +0,0 @@
1function retQuat=quatConj(a)
2rows = size(a,1);
3retQuat = zeros(rows, 4);
4retQuat=[a(:,1), -a(:,2), -a(:,3), -a(:,4)];
5
diff --git a/SD-VBS/benchmarks/localization/src/matlab/quatMul.m b/SD-VBS/benchmarks/localization/src/matlab/quatMul.m
deleted file mode 100644
index b397554..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/quatMul.m
+++ /dev/null
@@ -1,18 +0,0 @@
1function retQuat=quatMul(a, b)
2
3rowsa = size(a,1);
4colsa = size(a,2);
5
6rowsb = size(b,1);
7colsb = size(b,2);
8
9retQuat=[a(:,1).*b(:,1) - a(:,2).*b(:,2) - a(:,3).*b(:,3) - a(:,4).*b(:,4) ...
10 a(:,1).*b(:,2) + a(:,2).*b(:,1) + a(:,3).*b(:,4) - a(:,4).*b(:,3) ...
11 a(:,1).*b(:,3) - a(:,2).*b(:,4) + a(:,3).*b(:,1) + a(:,4).*b(:,2) ...
12 a(:,1).*b(:,4) + a(:,2).*b(:,3) - a(:,3).*b(:,2) + a(:,4).*b(:,1)];
13
14% disp('retQuat');
15% disp(retQuat);
16
17
18
diff --git a/SD-VBS/benchmarks/localization/src/matlab/quatRot.m b/SD-VBS/benchmarks/localization/src/matlab/quatRot.m
deleted file mode 100644
index 3688172..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/quatRot.m
+++ /dev/null
@@ -1,24 +0,0 @@
1function ret=quatRot(vec, rQuat)
2nr=size(vec,1);
3tv = zeros(nr,1);
4vQuat=[tv, vec];
5temp = quatMul(rQuat, vQuat);
6temp1 = quatConj(rQuat);
7retVec = quatMul(temp, temp1);
8%retVec=quatMul(quatMul(rQuat, vQuat),quatConj(rQuat));
9%ret=retVec(:,2:4);
10rows = size(retVec, 1);
11ret = zeros(rows,3);
12
13for i=1:rows
14 k =1;
15 for j=2:4
16 ret(i,k) = retVec(i,j);
17 k = k+1;
18 end
19end
20%size(ret)
21
22
23
24
diff --git a/SD-VBS/benchmarks/localization/src/matlab/readLoc.m b/SD-VBS/benchmarks/localization/src/matlab/readLoc.m
deleted file mode 100644
index fc679d4..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/readLoc.m
+++ /dev/null
@@ -1,53 +0,0 @@
1function out = readLoc
2out = [...
310.512346 3.466955 9.006616 ;...
413.242645 4.280358 14.675757 ;...
512.418589 6.286052 22.721924 ;...
613.598760 5.229711 19.622804 ;...
713.568058 5.229711 19.622804 ;...
813.537888 5.229711 19.622804 ;...
913.508315 5.229711 19.622804 ;...
1013.479378 5.229711 19.622804 ;...
1113.450420 5.229711 19.622804 ;...
1213.345976 7.607621 29.644342 ;...
1313.500197 7.803792 32.248338 ;...
1413.354279 10.248983 42.939018 ;...
1513.500197 10.442337 45.593042 ;...
1613.354279 12.925516 56.404386 ;...
1713.700372 10.538259 47.007521 ;...
1813.669945 10.538259 47.007521 ;...
1913.640058 10.538259 47.007521 ;...
2013.608179 10.538259 47.007521 ;...
2113.576901 10.538259 47.007521 ;...
2213.545771 10.538259 47.007521 ;...
2313.731515 12.682360 55.887526 ;...
241.016192 12.058634 55.442021 ;...
256.263918 15.094210 64.436498 ;...
266.263918 18.208480 70.198939 ;...
276.263918 20.848498 78.618094 ;...
286.263918 23.887747 84.333007 ;...
296.263918 22.065907 78.853015 ;...
306.237774 22.065907 78.853015 ;...
316.211166 22.065907 78.853015 ;...
326.183471 22.065907 78.853015 ;...
336.155114 22.065907 78.853015 ;...
346.126730 22.065907 78.853015 ;...
356.154098 26.480108 89.543387 ;...
366.168298 28.571946 93.679254 ;...
376.157243 32.868572 105.963003 ;...
386.155032 35.245287 110.401650 ;...
396.155032 38.531472 119.557291 ;...
406.155032 36.849296 111.296948 ;...
416.128369 36.849296 111.296948 ;...
426.101609 36.849296 111.296948 ;...
436.073750 36.849296 111.296948 ;...
446.046859 36.849296 111.296948 ;...
456.019815 36.849296 111.296948 ;...
466.046273 40.863016 123.894104 ;...
476.060599 43.054591 127.680881 ;...
486.049203 47.179478 140.858302 ;...
496.046924 49.530293 145.484328 ;...
506.046924 52.526113 153.189673 ;...
516.046924 51.113294 146.241709 ;...
526.021072 51.113294 146.241709 ...
53]; \ 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
deleted file mode 100644
index fe684c5..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/readMatrix.m
+++ /dev/null
@@ -1,28 +0,0 @@
1function readMatrix(srcImage, outName)
2
3write = fopen([outName '.m'], 'w');
4
5count = fwrite(write, 'function out = ');
6count = fwrite(write, outName);
7fprintf(write, '\n');
8count = fwrite(write, 'out = [...');
9fprintf(write, '\n');
10
11height = size(srcImage,1);
12width = size(srcImage,2);
13
14for nI=1:height
15 for nJ=1:width
16 fprintf(write, '%f ', srcImage(nI,nJ));
17 end
18 if(nI < height)
19 fprintf(write, ';...\n');
20 end
21end
22
23fprintf(write, '...\n');
24count = fwrite(write, '];');
25
26fclose(write);
27
28end
diff --git a/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m b/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m
deleted file mode 100644
index e84a6db..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m
+++ /dev/null
@@ -1,46 +0,0 @@
1function [retTStamp, retType, retData, retEOF, index]=readSensorData(index1, fid)
2
3index = index1+1;
4rows = size(fid, 1);
5retTStamp = 0;
6retType = 0;
7retData = zeros(1,8);
8
9if(index > rows)
10 retEOF = 1;
11else
12% for i=index:rows
13% index = i;
14% if(fid(i,2) == 4)
15% break;
16% end
17% end
18 if(index == rows)
19 retEOF = 1;
20 else
21 retEOF = 0;
22 end
23
24 k = index;
25 retTStamp=fid(k,1);
26 retType=fid(k,2);
27 if(fid(k, 2) == 1 || fid(k, 2) == 2 || fid(k, 2) == 3)
28 index = k;
29 for i=1:3
30 retData(1,i)=fid(k,i+2);
31% fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i);
32 end
33 end
34 if(fid(k, 2) == 4)
35 index = k;
36 for i=1:3
37 retData(1,i)=fid(k,i+2);
38% fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i);
39 end
40 for i=4:8
41 retData(1,i) = fid(k+1,i-3);
42% fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i);
43 end
44 index = index + 1;
45 end
46end
diff --git a/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m b/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m
deleted file mode 100644
index 44ce3aa..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m
+++ /dev/null
@@ -1,209 +0,0 @@
1function script_run_profile(dataDir, resultDir, type, common, toolDir)
2
3path(path, common);
4file = fopen([dataDir, '/1.txt'], 'r');
5full = fscanf(file,'%f');
6elapsed = zeros(1,2);
7
8rows = full(2);
9cols = full(1);
10fid = zeros(rows, cols);
11
12k = 3;
13for i=1:rows
14 for j =1:cols
15 fid(i,j) = full(k);
16 k = k+1;
17 end
18end
19
20fclose(file);
21
22n=1000;
23
24gyroTimeInterval=0.01;
25acclTimeInterval=0.01;
26
27STDDEV_GPSVel=0.5;
28
29STDDEV_ODOVel=0.1;
30STDDEV_ACCL=1;
31M_STDDEV_GYRO=0.1;
32M_STDDEV_POS=0.1;
33M_STDDEV_VEL=0.02;
34
35if(strcmp(type,'test'))
36 n = 3;
37 gyroTimeInterval = 0.1;
38 acclTimeInterval = 0.1;
39 M_STDDEV_VEL = 0.2;
40
41elseif(strcmp(type, 'sim_fast'))
42 n = 3;
43elseif(strcmp(type, 'sim'))
44 n = 10;
45elseif(strcmp(type, 'sqcif'))
46 n = 800;
47elseif(strcmp(type, 'qcif'))
48 n = 500;
49elseif(strcmp(type, 'vga'))
50 n = 2000;
51elseif(strcmp(type, 'wuxga'))
52 n = 3000;
53end
54
55fprintf(1,'Input size\t\t- (%dx%dx%d)\n', rows, cols,n);
56pos=zeros(n,3);
57
58vel=zeros(n,3) + randWrapper(n,3)*STDDEV_ODOVel;
59pi = 3.1416;
60
61eul1 = eul2quat([zeros(n,2), randWrapper(n,1)*2*pi]);
62eul2 = eul2quat([pi, 0, 0]);
63quat=quatMul(eul1, eul2);
64
65i=0;
66index = 0;
67resultMat = zeros(3,rows);
68
69while 1
70 i=i+1;
71 [tStamp, sType, sData, isEOF, index] = readSensorData(index, fid);
72
73 start = photonStartTiming;
74
75 if(sType==2)
76
77
78 %Motion model
79 gyro = sData(1, 1:3);
80 randnWrapper_mat = randnWrapper(n,3); % KVS: We do not have function implementation for randnWrapper()
81 gyro=ones(n,1)*gyro+randnWrapper_mat*M_STDDEV_GYRO;
82
83 abc = gyro.^2;
84 abcd = sumCol(abc);
85 norm_gyro = sqrt(abcd);
86% norm_gyro=sqrt(sum(gyro.^2,2));
87 angleAlpha=norm_gyro*gyroTimeInterval;
88 quatDelta=[cos(angleAlpha/2), gyro./(norm_gyro*ones(1,3)+0.00000001).*(sin(angleAlpha/2)*ones(1,3))];
89 quat=quatMul(quat, quatDelta);
90
91 end
92
93 if(sType==4)
94 %Observation
95 STDDEV_GPSPos=[sData(1,7), 0, 0; 0, sData(1,8), 0; 0, 0, 15];
96 Opos=sData(1,1:3);
97
98 %Initialize
99
100 randnWrapper_mat = randnWrapper(n,3);
101 if sum(sum(pos))==0
102 pos=ones(n,1) * Opos + randnWrapper_mat*STDDEV_GPSPos;
103 else
104 rows = size(STDDEV_GPSPos, 1);
105 cols = size(STDDEV_GPSPos, 2);
106
107 temp_STDDEV_GPSPos = ones(rows,cols);
108 for mnrows=1:rows % KVS" Photon rejects this loop becasue of too many nestings ??
109 for mncols=1:cols
110 temp_STDDEV_GPSPos(mnrows, mncols) = power(STDDEV_GPSPos(mnrows,mncols),-1);
111 end
112 end
113 [temp, w]=mcl(pos, Opos , temp_STDDEV_GPSPos);
114 [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
115 end
116
117 %compare direction
118 Ovel=sData(1,4:6);
119
120% KVS: We do not have function for norm() yet! So replacing this operating with OvelNorm
121
122% OvelNorm=norm(Ovel);
123 OvelNorm= 2.0; %1.1169e+09;
124
125 if (OvelNorm>0.5)
126
127 % KVS: Similar here: No impln of norm(), so replacing
128 % norm(Ovel) with OvelNorm value
129
130 Ovel=Ovel/OvelNorm;
131 %trick
132 %quat=addEulNoise(quat, STDDEV_GPSVel);
133 qConj = quatConj(quat);
134 orgWorld=quatRot([1, 0, 0],qConj);
135 eye3 = [1,0,0;0,1,0;0,0,1];
136 [temp, w]=mcl(orgWorld, Ovel, eye3./STDDEV_GPSVel);
137 [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
138 end
139
140 end
141
142 if(sType==1)
143
144 %Observation
145 Ovel=sData(1,1);
146 [temp, w]=mcl(sqrt(vel(:,1).^2+vel(:,2).^2+vel(:,3).^2), Ovel, 1/(STDDEV_ODOVel));
147
148 [quat vel pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
149 end
150
151 if(sType==3)
152 %Observation
153 accl=sData(1,1:3);
154
155 gtemp = ones(n,1) * [0 0 -9.8];
156
157 gravity=quatRot(gtemp, quat);
158 eye3 = [1,0,0;0,1,0;0,0,1];
159 [gravity, w]=mcl(gravity, accl, eye3./(STDDEV_ACCL));
160
161 [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
162
163 %Motion model
164 accl=ones(n,1)*accl;
165 accl=accl-gravity;
166 pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval ...
167 +1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2 ...
168 +randnWrapper(n,3)*M_STDDEV_POS;
169 vel=vel+accl*acclTimeInterval+randnWrapper(n,3)*M_STDDEV_VEL;
170
171 end
172
173 stop = photonEndTiming;
174
175 temp = photonReportTiming(start, stop);
176 elapsed(1) = elapsed(1) + temp(1);
177 elapsed(2) = elapsed(2) + temp(2);
178
179 % Self check
180
181 quatOut = 0;
182 posOut = 0;
183 velOut = 0;
184
185 for ij=1:(size(quat,1)*size(quat,2))
186 quatOut = quatOut + quat(ij);
187 end
188
189 for ij=1:(size(vel,1)*size(vel,2))
190 velOut = velOut + vel(ij);
191 end
192
193 for ij=1:(size(pos,1)*size(pos,2))
194 posOut = posOut + pos(ij);
195 end
196
197 resultMat(:, i) = [quatOut, velOut, posOut];
198
199 if (isEOF == 1)
200 break;
201 end
202end
203
204%% Timing
205photonPrintTiming(elapsed);
206
207%% Self checking %%
208fWriteMatrix(resultMat,dataDir);
209
diff --git a/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m b/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m
deleted file mode 100644
index ac80ebd..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m
+++ /dev/null
@@ -1,33 +0,0 @@
1function ret = selfCheck(in1, in2, tol)
2
3r1 = size(in1, 1);
4c1 = size(in1, 2);
5
6r2 = size(in2, 1);
7c2 = size(in2, 2);
8
9ret = 1;
10
11if r1~=r2
12 disp(1298);
13 ret = 0;
14end
15
16if c1 ~= c2
17 disp(1297);
18 ret = 0;
19end
20
21for i=1:r1
22 if(ret == 0)
23 break;
24 end
25 for j=1:c1
26 if( abs(in1(i,j)-in2(i,j)) > tol)
27 ret = 0;
28 break;
29 end
30 end
31end
32
33
diff --git a/SD-VBS/benchmarks/localization/src/matlab/sumCol.m b/SD-VBS/benchmarks/localization/src/matlab/sumCol.m
deleted file mode 100644
index ad0114e..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/sumCol.m
+++ /dev/null
@@ -1,14 +0,0 @@
1function ret = sumCol(mat)
2
3row = size(mat, 1);
4col = size(mat, 2);
5
6ret = zeros(row, 1);
7
8for i=1:row
9 temp = 0;
10 for j=1:col
11 temp = temp + mat(i, j);
12 end
13 ret(i, 1) = temp;
14end
diff --git a/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m b/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m
deleted file mode 100644
index dc9274b..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m
+++ /dev/null
@@ -1,20 +0,0 @@
1%function x_gen_id=weightedSample(w)
2function bin=weightedSample(w)
3n=size(w,1);
4seed=randWrapper(n,1);
5bin = zeros(n,1);
6%x_gen_id=zeros(n,1);
7 for i=1:n
8 for j=1:n
9 if(seed(j,1) > 0)
10 bin(j,1) = bin(j,1) + 1;
11% x_gen_id(j,1) = x_gen_id(j,1) + bin(j,1);
12 end
13 end
14% bin = (seed>0);
15% x_gen_id=x_gen_id+bin;
16 seed=seed-w(i,1);
17 end
18% x_gen_id = bin;
19%x_gen=x(:,x_gen_id);
20
generated by cgit v1.2.2 (git 2.25.0) at 2025-04-30 17:16:51 -0400