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.c534
1 files changed, 0 insertions, 534 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 3392320..0000000
--- a/SD-VBS/benchmarks/localization/src/c/script_localization.c
+++ /dev/null
@@ -1,534 +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 printf("Input txt File: ");
45 scanf("%s", im1);
46
47 fid = readFile(im1);
48
49 n = 1000;
50
51 #ifdef test
52 n = 3;
53 gyroTimeInterval = 0.1;
54 acclTimeInterval = 0.1;
55 M_STDDEV_VEL = 0.2;
56 #endif
57 #ifdef sim_fast
58 n = 3;
59 #endif
60 #ifdef sim
61 n = 10;
62 #endif
63 #ifdef sqcif
64 n = 800;
65 #endif
66 #ifdef qcif
67 n = 500;
68 #endif
69 #ifdef vga
70 n = 2000;
71 #endif
72 #ifdef wuxga
73 n = 3000;
74 #endif
75
76 resultMat = fSetArray(3,fid->height, 0);
77
78 pos = fSetArray(n, 3, 0);
79 vel = fSetArray(n, 3, 0);
80 ones = fSetArray(n,1,1);
81
82
83 F2D *randn;
84
85 randn = randWrapper(n,3);
86 printf("start\n");
87 for_each_job{
88 fResetArray(pos,n, 3, 0);
89 fResetArray(vel,n, 3, 0);
90 fResetArray(ones,n,1,1);
91
92 {
93 int j;
94 for(i=0; i<n; i++)
95 for(j=0; j<3; j++)
96 subsref(vel, i, j) += subsref(randn,i,j) * STDDEV_ODOVel;
97
98 }
99
100
101
102 {
103 F2D *eulAngle, *randn;
104 eulAngle = fSetArray(n, 3, 0);
105 randn = randWrapper(n,1);
106
107 for(i=0; i<n; i++)
108 {
109 subsref(eulAngle, i, 2) = subsref(randn, i, 0) * 2 * pi;
110 }
111
112 eul1 = eul2quat(eulAngle);
113 fFreeHandle(eulAngle);
114
115 eulAngle = fSetArray(1, 3, 0);
116 subsref(eulAngle, 0, 0) = pi;
117 eul2 = eul2quat(eulAngle);
118
119 fFreeHandle(randn);
120 fFreeHandle(eulAngle);
121 }
122
123 quat = quatMul(eul1, eul2);
124 fFreeHandle(eul1);
125 fFreeHandle(eul2);
126
127 i=0;
128 index = iSetArray(1,1,-1);
129 sType = iSetArray(1,1,-1);
130 isEOF = iSetArray(1,1,-1);
131
132
133 rows =0;
134 cols = 5;
135 STDDEV_GPSPos = fSetArray(3,3,0);
136 randW = randnWrapper(n,3);
137 icount=-1;
138 while(1)
139 {
140 icount=icount+1;
141
142 /*
143 S = 4
144 A = 3
145 G = 2
146 V = 1
147 */
148
149 sData = readSensorData(index, fid, sType, isEOF);
150 rows++;
151
152
153
154 if( asubsref(sType,0) ==2)
155 {
156
157 //Motion model
158
159 {
160 int i;
161 F2D *t, *t1;
162 F2D *abc, *abcd;
163 int qD_r=0, qD_c=0;
164 F2D *cosA, *sinA;
165
166 t = fDeepCopyRange(sData, 0, 1, 0, 3);
167 gyro = fMtimes(ones, t);
168 abc = fMallocHandle(gyro->height, gyro->width);
169 t1 = fDeepCopy(randW);
170
171 for(i=0; i<(n*3); i++)
172 {
173 asubsref(t1,i) = asubsref(randW,i) * M_STDDEV_GYRO;
174 asubsref(gyro, i) += asubsref(t1,i);
175 asubsref(abc, i) = pow(asubsref(gyro, i), 2);
176 }
177 fFreeHandle(t1);
178 abcd = fSum2(abc, 2);
179
180 norm_gyro = fMallocHandle(abcd->height,abcd->width);
181 angleAlpha = fMallocHandle(abcd->height, abcd->width);
182
183 for(i=0; i<(abcd->height*abcd->width); i++)
184 {
185 asubsref(norm_gyro, i) = sqrt(asubsref(abcd,i));
186 asubsref(angleAlpha,i) = asubsref(norm_gyro,i) * gyroTimeInterval;
187 }
188
189 qD_r += angleAlpha->height + gyro->height;