summaryrefslogtreecommitdiffstats
path: root/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m
diff options
context:
space:
mode:
authorleochanj105 <leochanj@live.unc.edu>2020-10-20 03:47:33 -0400
committerleochanj105 <leochanj@live.unc.edu>2020-10-20 03:47:33 -0400
commita32f220f06cc463e5b56e7fa0b1b1334d94d08f3 (patch)
tree4af4caa60074465d85fc2ef5cc1b23e74c064329 /SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m
parent79f30887129145e15e5172e36a7d7602859fc932 (diff)
matlab removed
Diffstat (limited to 'SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m')
-rw-r--r--SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m209
1 files changed, 0 insertions, 209 deletions
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