summaryrefslogtreecommitdiffstats
path: root/SD-VBS/benchmarks/localization/src/c/script_localization.c
diff options
context:
space:
mode:
Diffstat (limited to 'SD-VBS/benchmarks/localization/src/c/script_localization.c')
-rw-r--r--SD-VBS/benchmarks/localization/src/c/script_localization.c525
1 files changed, 0 insertions, 525 deletions
diff --git a/SD-VBS/benchmarks/localization/src/c/script_localization.c b/SD-VBS/benchmarks/localization/src/c/script_localization.c
deleted file mode 100644
index ddd2215..0000000
--- a/SD-VBS/benchmarks/localization/src/c/script_localization.c
+++ /dev/null
@@ -1,525 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include <malloc.h>
8#include "localization.h"
9#include "extra.h"
10#define LOCOLIZATION_MEM 1<<24
11int main(int argc, char* argv[])
12{
13 SET_UP
14 mallopt(M_TOP_PAD, LOCOLIZATION_MEM);
15 mallopt(M_MMAP_MAX, 0);
16 int n, i, j, k, icount=-1;
17 F2D* fid;
18 float gyroTimeInterval=0.01;
19 float acclTimeInterval=0.01;
20
21 float STDDEV_GPSVel=0.5;
22 float STDDEV_ODOVel=0.1;
23 float STDDEV_ACCL=1;
24 float M_STDDEV_GYRO=0.1;
25 float M_STDDEV_POS=0.1;
26 float M_STDDEV_VEL=0.02;
27
28 F2D *pos, *vel;
29 float pi = 3.1416;
30 F2D *eul1, *eul2, *quat;
31 F2D *sData, *gyro, *norm_gyro, *angleAlpha;
32 F2D *quatDelta, *Opos, *temp_STDDEV_GPSPos, *w;
33 F2D *qConj, *orgWorld, *accl, *gtemp;
34 F2D *gravity, *t1;
35 I2D *tStamp, *sType, *isEOF;
36 I2D *index;
37 int rows, cols;
38 F2D *resultMat;
39 F2D *STDDEV_GPSPos;
40 F2D *ones, *randW;
41
42 char im1[100];
43
44 scanf("%s", im1);
45
46 fid = readFile(im1);
47
48 n = 1000;
49
50 #ifdef test
51 n = 3;
52 gyroTimeInterval = 0.1;
53 acclTimeInterval = 0.1;
54 M_STDDEV_VEL = 0.2;
55 #endif
56 #ifdef sim_fast
57 n = 3;
58 #endif
59 #ifdef sim
60 n = 10;
61 #endif
62 #ifdef sqcif
63 n = 800;
64 #endif
65 #ifdef qcif
66 n = 500;
67 #endif
68 #ifdef vga
69 n = 2000;
70 #endif
71 #ifdef wuxga
72 n = 3000;
73 #endif
74
75 resultMat = fSetArray(3,fid->height, 0);
76
77 pos = fSetArray(n, 3, 0);
78 vel = fSetArray(n, 3, 0);
79 ones = fSetArray(n,1,1);
80
81
82 F2D *randn;
83
84 randn = randWrapper(n,3);
85 for_each_job {
86 fResetArray(pos,n, 3, 0);
87 fResetArray(vel,n, 3, 0);
88 fResetArray(ones,n,1,1);
89
90 {
91 int j;
92 for(i=0; i<n; i++)
93 for(j=0; j<3; j++)
94 subsref(vel, i, j) += subsref(randn,i,j) * STDDEV_ODOVel;
95
96 }
97
98
99
100 {
101 F2D *eulAngle, *randn;
102 eulAngle = fSetArray(n, 3, 0);
103 randn = randWrapper(n,1);
104
105 for(i=0; i<n; i++)
106 {
107 subsref(eulAngle, i, 2) = subsref(randn, i, 0) * 2 * pi;
108 }
109
110 eul1 = eul2quat(eulAngle);
111 fFreeHandle(eulAngle);
112
113 eulAngle = fSetArray(1, 3, 0);
114 subsref(eulAngle, 0, 0) = pi;
115 eul2 = eul2quat(eulAngle);
116
117 fFreeHandle(randn);
118 fFreeHandle(eulAngle);
119 }
120
121 quat = quatMul(eul1, eul2);
122 fFreeHandle(eul1);
123 fFreeHandle(eul2);
124
125 i=0;
126 index = iSetArray(1,1,-1);
127 sType = iSetArray(1,1,-1);
128 isEOF = iSetArray(1,1,-1);
129
130
131 rows =0;
132 cols = 5;
133 STDDEV_GPSPos = fSetArray(3,3,0);
134 randW = randnWrapper(n,3);
135 icount=-1;
136 while(1)
137 {
138 icount=icount+1;
139
140 /*
141 S = 4
142 A = 3
143 G = 2
144 V = 1
145 */
146
147 sData = readSensorData(index, fid, sType, isEOF);
148 rows++;
149
150
151
152 if( asubsref(sType,0) ==2)
153 {
154
155 //Motion model
156
157 {
158 int i;
159 F2D *t, *t1;
160 F2D *abc, *abcd;
161 int qD_r=0, qD_c=0;
162 F2D *cosA, *sinA;
163
164 t = fDeepCopyRange(sData, 0, 1, 0, 3);
165 gyro = fMtimes(ones, t);
166 abc = fMallocHandle(gyro->height, gyro->width);
167 t1 = fDeepCopy(randW);
168
169 for(i=0; i<(n*3); i++)
170 {
171 asubsref(t1,i) = asubsref(randW,i) * M_STDDEV_GYRO;
172 asubsref(gyro, i) += asubsref(t1,i);
173 asubsref(abc, i) = pow(asubsref(gyro, i), 2);
174 }
175 fFreeHandle(t1);
176 abcd = fSum2(abc, 2);
177
178 norm_gyro = fMallocHandle(abcd->height,abcd->width);
179 angleAlpha = fMallocHandle(abcd->height, abcd->width);
180
181 for(i=0; i<(abcd->height*abcd->width); i++)
182 {
183 asubsref(norm_gyro, i) = sqrt(asubsref(abcd,i));
184 asubsref(angleAlpha,i) = asubsref(norm_gyro,i) * gyroTimeInterval;
185 }
186
187 qD_r += angleAlpha->height + gyro->height;
188 qD_c += angleAlpha->width + 3;
189
190 f