diff options
author | leochanj105 <leochanj@live.unc.edu> | 2020-10-20 03:47:33 -0400 |
---|---|---|
committer | leochanj105 <leochanj@live.unc.edu> | 2020-10-20 03:47:33 -0400 |
commit | a32f220f06cc463e5b56e7fa0b1b1334d94d08f3 (patch) | |
tree | 4af4caa60074465d85fc2ef5cc1b23e74c064329 /SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m | |
parent | 79f30887129145e15e5172e36a7d7602859fc932 (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.m | 209 |
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 @@ | |||
1 | function script_run_profile(dataDir, resultDir, type, common, toolDir) | ||
2 | |||
3 | path(path, common); | ||
4 | file = fopen([dataDir, '/1.txt'], 'r'); | ||
5 | full = fscanf(file,'%f'); | ||
6 | elapsed = zeros(1,2); | ||
7 | |||
8 | rows = full(2); | ||
9 | cols = full(1); | ||
10 | fid = zeros(rows, cols); | ||
11 | |||
12 | k = 3; | ||
13 | for i=1:rows | ||
14 | for j =1:cols | ||
15 | fid(i,j) = full(k); | ||
16 | k = k+1; | ||
17 | end | ||
18 | end | ||
19 | |||
20 | fclose(file); | ||
21 | |||
22 | n=1000; | ||
23 | |||
24 | gyroTimeInterval=0.01; | ||
25 | acclTimeInterval=0.01; | ||
26 | |||
27 | STDDEV_GPSVel=0.5; | ||
28 | |||
29 | STDDEV_ODOVel=0.1; | ||
30 | STDDEV_ACCL=1; | ||
31 | M_STDDEV_GYRO=0.1; | ||
32 | M_STDDEV_POS=0.1; | ||
33 | M_STDDEV_VEL=0.02; | ||
34 | |||
35 | if(strcmp(type,'test')) | ||
36 | n = 3; | ||
37 | gyroTimeInterval = 0.1; | ||
38 | acclTimeInterval = 0.1; | ||
39 | M_STDDEV_VEL = 0.2; | ||
40 | |||
41 | elseif(strcmp(type, 'sim_fast')) | ||
42 | n = 3; | ||
43 | elseif(strcmp(type, 'sim')) | ||
44 | n = 10; | ||
45 | elseif(strcmp(type, 'sqcif')) | ||
46 | n = 800; | ||
47 | elseif(strcmp(type, 'qcif')) | ||
48 | n = 500; | ||
49 | elseif(strcmp(type, 'vga')) | ||
50 | n = 2000; | ||
51 | elseif(strcmp(type, 'wuxga')) | ||
52 | n = 3000; | ||
53 | end | ||
54 | |||
55 | fprintf(1,'Input size\t\t- (%dx%dx%d)\n', rows, cols,n); | ||
56 | pos=zeros(n,3); | ||
57 | |||
58 | vel=zeros(n,3) + randWrapper(n,3)*STDDEV_ODOVel; | ||
59 | pi = 3.1416; | ||
60 | |||
61 | eul1 = eul2quat([zeros(n,2), randWrapper(n,1)*2*pi]); | ||
62 | eul2 = eul2quat([pi, 0, 0]); | ||
63 | quat=quatMul(eul1, eul2); | ||
64 | |||
65 | i=0; | ||
66 | index = 0; | ||
67 | resultMat = zeros(3,rows); | ||
68 | |||
69 | while 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 | ||
202 | end | ||
203 | |||
204 | %% Timing | ||
205 | photonPrintTiming(elapsed); | ||
206 | |||
207 | %% Self checking %% | ||
208 | fWriteMatrix(resultMat,dataDir); | ||
209 | |||