summaryrefslogtreecommitdiffstats
path: root/SD-VBS/benchmarks/localization/src/c
diff options
context:
space:
mode:
Diffstat (limited to 'SD-VBS/benchmarks/localization/src/c')
-rw-r--r--SD-VBS/benchmarks/localization/src/c/eul2quat.c50
-rw-r--r--SD-VBS/benchmarks/localization/src/c/generateSample.c88
-rw-r--r--SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c49
-rw-r--r--SD-VBS/benchmarks/localization/src/c/input.txt8
-rw-r--r--SD-VBS/benchmarks/localization/src/c/localization.h24
-rw-r--r--SD-VBS/benchmarks/localization/src/c/mcl.c34
-rw-r--r--SD-VBS/benchmarks/localization/src/c/quat2eul.c47
-rw-r--r--SD-VBS/benchmarks/localization/src/c/quatConj.c32
-rw-r--r--SD-VBS/benchmarks/localization/src/c/quatMul.c55
-rw-r--r--SD-VBS/benchmarks/localization/src/c/quatRot.c47
-rw-r--r--SD-VBS/benchmarks/localization/src/c/readSensorData.c63
-rw-r--r--SD-VBS/benchmarks/localization/src/c/script_localization.c525
-rw-r--r--SD-VBS/benchmarks/localization/src/c/weightedSample.c37
13 files changed, 0 insertions, 1059 deletions
diff --git a/SD-VBS/benchmarks/localization/src/c/eul2quat.c b/SD-VBS/benchmarks/localization/src/c/eul2quat.c
deleted file mode 100644
index bea75cf..0000000
--- a/SD-VBS/benchmarks/localization/src/c/eul2quat.c
+++ /dev/null
@@ -1,50 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* eul2quat(F2D* angle)
10{
11 F2D *ret;
12 F2D *x, *y, *z;
13 int k, i, j;
14 int rows, cols;
15
16 rows = angle->height;
17 cols = angle->width;
18
19 x = fDeepCopyRange(angle, 0, angle->height, 0, 1);
20 y = fDeepCopyRange(angle, 0, angle->height, 1, 1);
21 z = fDeepCopyRange(angle, 0, angle->height, 2, 1);
22
23 ret = fSetArray(x->height, 4, 0);
24
25 for(i=0; i<rows; i++)
26 {
27 float xi, yi, zi;
28 k = 0;
29 xi = asubsref(x,i);
30 yi = asubsref(y,i);
31 zi = asubsref(z,i);
32
33 subsref(ret,i,k) = cos(xi/2)*cos(yi/2)*cos(zi/2)+sin(xi/2)*sin(yi/2)*sin(zi/2);
34 k++;
35 subsref(ret,i,k) = sin(xi/2)*cos(yi/2)*cos(zi/2)-cos(xi/2)*sin(yi/2)*sin(zi/2);
36 k++;
37 subsref(ret,i,k) = cos(xi/2)*sin(yi/2)*cos(zi/2)+sin(xi/2)*cos(yi/2)*sin(zi/2);
38 k++;
39 subsref(ret,i,k) = cos(xi/2)*cos(yi/2)*sin(zi/2)-sin(xi/2)*sin(yi/2)*cos(zi/2);
40 }
41
42 fFreeHandle(x);
43 fFreeHandle(y);
44 fFreeHandle(z);
45
46 return ret;
47}
48
49
50
diff --git a/SD-VBS/benchmarks/localization/src/c/generateSample.c b/SD-VBS/benchmarks/localization/src/c/generateSample.c
deleted file mode 100644
index 3acb4d7..0000000
--- a/SD-VBS/benchmarks/localization/src/c/generateSample.c
+++ /dev/null
@@ -1,88 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9void generateSample(F2D *w, F2D *quat, F2D *vel, F2D *pos)
10{
11 int rows, cols, i, j, index;
12 I2D *sampleXId;
13 F2D *retQuat, *retVel, *retPos;
14
15 sampleXId = weightedSample(w);
16
17 rows = sampleXId->height;
18 cols = sampleXId->width;
19
20 if(cols > 1)
21 printf("ERROR: Cols more than 1.. Handle this case \n");
22
23 retQuat = fSetArray(quat->height, quat->width, 0);
24 retVel = fSetArray(vel->height, vel->width, 0);
25 retPos = fSetArray(pos->height, pos->width, 0);
26
27 for(i=0; i<rows; i++)
28 {
29 index = asubsref(sampleXId, i) - 1;
30 for(j=0; j<quat->width; j++)
31 {
32 subsref(retQuat,i,j) = subsref(quat,index,j);
33 }
34 }
35
36 for(i=0; i<rows; i++)
37 {
38 index = asubsref(sampleXId, i) - 1;
39 for(j=0; j<vel->width; j++)
40 {
41 subsref(retVel,i,j) = subsref(vel,index,j);
42 }
43 }
44
45 for(i=0; i<rows; i++)
46 {
47 index = asubsref(sampleXId, i) - 1;
48 for(j=0; j<pos->width; j++)
49 {
50 subsref(retPos,i,j) = subsref(pos,index,j);
51 }
52 }
53
54 for(i=0; i<quat->height; i++)
55 {
56 for(j=0; j<quat->width; j++)
57 {
58 subsref(quat,i,j) = subsref(retQuat,i,j);
59 }
60 }
61
62 for(i=0; i<vel->height; i++)
63 {
64 for(j=0; j<vel->width; j++)
65 {
66 subsref(vel,i,j) = subsref(retVel,i,j);
67 }
68 }
69
70 for(i=0; i<pos->height; i++)
71 {
72 for(j=0; j<pos->width; j++)
73 {
74 subsref(pos,i,j) = subsref(retPos,i,j);
75 }
76 }
77
78 fFreeHandle(retQuat);
79 fFreeHandle(retVel);
80 fFreeHandle(retPos);
81 iFreeHandle(sampleXId);
82
83 return;
84}
85
86
87
88
diff --git a/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c b/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c
deleted file mode 100644
index 58c4475..0000000
--- a/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c
+++ /dev/null
@@ -1,49 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* get3DGaussianProb( F2D* data, F2D* mean, F2D* A)
10{
11 F2D *p, *diff, *temp1, *temp2, *mt;
12 float temp;
13 int n_data, n_channel;
14 int i, j, k;
15 F2D* t;
16 float pi = 3.1412;
17
18 n_data = data->height;
19 n_channel = data->width;
20
21 t = fSetArray(n_data, 1, 1);
22
23 mt = fMtimes(t, mean);
24 diff = fMinus( data, mt);
25 p = fSetArray(diff->height, 1, 0);
26
27 temp = sqrt(1.0/(pow(2*pi, n_channel)));
28 temp2 = randWrapper(diff->height,1);
29
30 j = (temp2->height*temp2->width);
31 for(i=0; i<j; i++)
32 {
33 float temp2i;
34 temp2i = asubsref(temp2,i);
35
36 temp2i = exp(-0.5*temp2i);
37 asubsref(p,i) = temp2i*temp;
38 }
39
40 fFreeHandle(t);
41 fFreeHandle(temp2);
42 fFreeHandle(mt);
43 fFreeHandle(diff);
44
45 return p;
46}
47
48
49
diff --git a/SD-VBS/benchmarks/localization/src/c/input.txt b/SD-VBS/benchmarks/localization/src/c/input.txt
deleted file mode 100644
index dd6d1cf..0000000
--- a/SD-VBS/benchmarks/localization/src/c/input.txt
+++ /dev/null
@@ -1,8 +0,0 @@
15 6
211168.4 3 -0.098 0.247 9.59
311168.175 1 -0.000000 0.000000 0.000000
411168.04 4 4422.01 4836.36 33.99
5-0.0006 -0.0007 0.021 9.463 21.130
611168.4 3 -0.098 0.247 9.59
711168.4 3 -0.098 0.247 9.59
8
diff --git a/SD-VBS/benchmarks/localization/src/c/localization.h b/SD-VBS/benchmarks/localization/src/c/localization.h
deleted file mode 100644
index b2c03bb..0000000
--- a/SD-VBS/benchmarks/localization/src/c/localization.h
+++ /dev/null
@@ -1,24 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#ifndef _LOCALIZATION_
6#define _LOCALIZATION_
7
8#include "sdvbs_common.h"
9
10int script_localization();
11F2D* eul2quat(F2D* angle);
12void generateSample(F2D *w, F2D *quat, F2D *vel, F2D *pos);
13F2D* get3DGaussianProb( F2D* data, F2D* mean, F2D* A);
14F2D* mcl(F2D* x, F2D* sData, F2D* invConv);
15F2D* quat2eul(F2D* quat);
16F2D* quatConj(F2D* a);
17F2D* quatMul(F2D* a, F2D* b);
18F2D* quatRot(F2D* vec, F2D* rQuat);
19F2D* readSensorData(I2D* index, F2D* fid, I2D* type, I2D* eof);
20I2D* weightedSample(F2D* w);
21
22#endif
23
24
diff --git a/SD-VBS/benchmarks/localization/src/c/mcl.c b/SD-VBS/benchmarks/localization/src/c/mcl.c
deleted file mode 100644
index a3c56b6..0000000
--- a/SD-VBS/benchmarks/localization/src/c/mcl.c
+++ /dev/null
@@ -1,34 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* mcl(F2D* x, F2D* sData, F2D* invConv)
10{
11 int i, j;
12 F2D *retW, *retX, *sum;
13 float sumVal;
14
15 retX = fDeepCopy(x);
16 retW = get3DGaussianProb(retX, sData, invConv);
17 sum = fSum(retW);
18 if(sum->height == 1 && sum->width ==1)
19 {
20 sumVal = asubsref(sum,0);
21 for(i=0; i<retW->height; i++)
22 for(j=0; j<retW->width; j++)
23 subsref(retW,i,j) = subsref(retW,i,j)/sumVal;
24 }
25 else
26 retW = fMdivide(retW, sum);
27
28 fFreeHandle(retX);
29 fFreeHandle(sum);
30
31 return retW;
32}
33
34
diff --git a/SD-VBS/benchmarks/localization/src/c/quat2eul.c b/SD-VBS/benchmarks/localization/src/c/quat2eul.c
deleted file mode 100644
index bc581c9..0000000
--- a/SD-VBS/benchmarks/localization/src/c/quat2eul.c
+++ /dev/null
@@ -1,47 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8#include <math.h>
9
10F2D* quat2eul(F2D* quat)
11{
12 F2D *retEul;
13 int i, j, k;
14 int rows, cols;
15
16 rows = quat->height;
17 cols = quat->width;
18
19 retEul = fSetArray(rows, 3, 0);
20
21 for(i=0; i<rows; i++)
22 {
23 float temp, temp1, temp2, temp3, temp4;
24 float quati2, quati3, quati1, quati0;
25
26 quati0 = subsref(quat,i,0);
27 quati1 = subsref(quat,i,1);
28 quati2 = subsref(quat,i,2);
29 quati3 = subsref(quat,i,3);
30
31 temp = 2 *quati2 * quati3 + quati0 * quati1;
32 temp1 = pow(quati0,2) - pow(quati1,2) - pow(quati2,2) + pow(quati3,2);
33 temp2 = -2*quati1 * quati2 + quati0 * quati3;
34 temp3 = 2*quati1 * quati2 + quati0 * quati3;
35 temp4 = pow(quati0,2) + pow(quati1,2) - pow(quati2,2) - pow(quati3,2);
36
37 asubsref(retEul,k++) = atan2(temp, temp1);
38 asubsref(retEul,k++) = asin(temp2);
39 asubsref(retEul,k++) = atan2(temp3, temp4);
40 }
41
42 return retEul;
43}
44
45
46
47
diff --git a/SD-VBS/benchmarks/localization/src/c/quatConj.c b/SD-VBS/benchmarks/localization/src/c/quatConj.c
deleted file mode 100644
index 6607186..0000000
--- a/SD-VBS/benchmarks/localization/src/c/quatConj.c
+++ /dev/null
@@ -1,32 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* quatConj(F2D* a)
10{
11 F2D* retQuat;
12 int rows, cols;
13 int i, j, k;
14
15 rows = a->height;
16 cols = a->width;
17 retQuat = fSetArray(rows, 4, 0);
18
19 for(i=0; i<rows; i++)
20 {
21 k=0;
22 subsref(retQuat,i,k++) = subsref(a,i,0);
23 subsref(retQuat,i,k++) = -subsref(a,i,1);
24 subsref(retQuat,i,k++) = -subsref(a,i,2);
25 subsref(retQuat,i,k) = -subsref(a,i,3);
26 }
27
28 return retQuat;
29}
30
31
32
diff --git a/SD-VBS/benchmarks/localization/src/c/quatMul.c b/SD-VBS/benchmarks/localization/src/c/quatMul.c
deleted file mode 100644
index 8b8dafe..0000000
--- a/SD-VBS/benchmarks/localization/src/c/quatMul.c
+++ /dev/null
@@ -1,55 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* quatMul(F2D* a, F2D* b)
10{
11 int ra, ca, rb, cb;
12 F2D *ret;
13 int i, j, k=0;
14
15 ra = a->height;
16 ca = a->width;
17
18 rb = b->height;
19 cb = b->width;
20
21 ret = fSetArray(ra, 4, 0);
22
23 j = 0;
24 for(i=0; i<ra; i++)
25 {
26 k = 0;
27 float ai0, ai1, ai2, ai3;
28 float bj0, bj1, bj2, bj3;
29
30 ai0 = subsref(a,i,0);
31 ai1 = subsref(a,i,1);
32 ai2 = subsref(a,i,2);
33 ai3 = subsref(a,i,3);
34
35 bj0 = subsref(b,j,0);
36 bj1 = subsref(b,j,1);
37 bj2 = subsref(b,j,2);
38 bj3 = subsref(b,j,3);
39
40 subsref(ret,i,k++) = ai0*bj0 - ai1*bj1 - ai2*bj2 - ai3*bj3;
41 subsref(ret,i,k++) = ai0*bj1 + ai1*bj0 + ai2*bj3 - ai3*bj2;
42 subsref(ret,i,k++) = ai0*bj2 - ai1*bj3 + ai2*bj0 + ai3*bj1;
43 subsref(ret,i,k++) = ai0*bj3 + ai1*bj2 - ai2*bj1 + ai3*bj0;
44
45 if(rb == ra)
46 j++;
47 }
48
49 return ret;
50}
51
52
53
54
55
diff --git a/SD-VBS/benchmarks/localization/src/c/quatRot.c b/SD-VBS/benchmarks/localization/src/c/quatRot.c
deleted file mode 100644
index 4962b55..0000000
--- a/SD-VBS/benchmarks/localization/src/c/quatRot.c
+++ /dev/null
@@ -1,47 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* quatRot(F2D* vec, F2D* rQuat)
10{
11 F2D *ret;
12 int nr, i, j, k, rows, cols;
13 F2D *tv, *vQuat, *temp, *temp1;
14 F2D *retVec;
15
16 nr = vec->height;
17 tv = fSetArray(nr, 1, 0);
18 vQuat = fHorzcat(tv, vec);
19 temp = quatMul(rQuat, vQuat);
20 temp1 = quatConj(rQuat);
21 retVec = quatMul(temp, temp1);
22
23 rows = retVec->height;
24 cols = retVec->width;
25
26 ret = fSetArray(rows, 3, 0);
27
28 for(i=0; i<rows; i++)
29 {
30 k = 0;
31 for(j=1; j<4; j++)
32 {
33 subsref(ret,i,k) = subsref(retVec,i,j);
34 k++;
35 }
36 }
37
38 fFreeHandle(tv);
39 fFreeHandle(vQuat);
40 fFreeHandle(temp);
41 fFreeHandle(temp1);
42 fFreeHandle(retVec);
43
44 return ret;
45}
46
47
diff --git a/SD-VBS/benchmarks/localization/src/c/readSensorData.c b/SD-VBS/benchmarks/localization/src/c/readSensorData.c
deleted file mode 100644
index 12c0dc0..0000000
--- a/SD-VBS/benchmarks/localization/src/c/readSensorData.c
+++ /dev/null
@@ -1,63 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* readSensorData(I2D* index, F2D* fid, I2D* type, I2D* eof)
10{
11 F2D *retData;
12 int rows, i, j, k;
13 int atype=-1, aindex;
14
15 aindex = asubsref(index, 0);
16
17 asubsref(index,0) = asubsref(index,0) + 1;
18 rows = fid->height;
19 asubsref(type,0) = 0;
20 retData = fSetArray(1, 8, 0);
21
22 if( asubsref(index,0) > (rows-1) )
23 asubsref(eof,0) = 1;
24 else
25 {
26 if( asubsref(index,0) == rows)
27 asubsref(eof,0) = 1;
28 else
29 asubsref(eof,0) = 0;
30
31 k = asubsref(index,0);
32 atype = subsref(fid, k, 1);
33 if( (atype == 1) || (atype == 2) || (atype == 3) )
34 {
35 for(i=0; i<3; i++)
36 {
37 asubsref(retData,i) = subsref(fid,k,(i+2));
38 }
39 }
40 if( atype == 4 )
41 {
42 for(i=0; i<3; i++)
43 {
44 asubsref(retData,i) = subsref(fid,k,(i+2));
45 }
46 for(i=3; i<8; i++)
47 {
48 asubsref(retData,i) = subsref(fid,k+1,(i-3));
49 }
50 aindex = aindex + 1;
51 }
52 aindex = aindex + 1;
53 }
54
55 asubsref(index,0) = aindex;
56 asubsref(type, 0) = atype;
57
58 return retData;
59}
60
61
62
63
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 fFreeHandle(t);
191 fFreeHandle(abcd);
192
193 cosA = fSetArray(angleAlpha->height, angleAlpha->width, 0);
194 sinA = fSetArray(angleAlpha->height, angleAlpha->width, 0);
195
196 for(i=0; i<(cosA->height*cosA->width); i++)
197 asubsref(cosA,i) = cos( asubsref(angleAlpha,i) /2 );
198
199 for(i=0; i<(sinA->height*sinA->width); i++)
200 asubsref(sinA,i) = sin( asubsref(angleAlpha,i) /2 );
201
202
203 fFreeHandle(abc);
204 abc = fSetArray(1,3,1);
205 t1 = fMtimes(norm_gyro, abc);
206 t = ffDivide(gyro, t1);
207 fFreeHandle(t1);
208
209 abcd = fMtimes(sinA, abc);
210 t1 = fTimes(t, abcd);
211 quatDelta = fHorzcat(cosA, t1);
212
213 fFreeHandle(abcd);
214 fFreeHandle(t);
215 fFreeHandle(t1);
216 fFreeHandle(abc);
217
218 t = quatMul(quat, quatDelta);
219 fFreeHandle(quat);
220 fFreeHandle(quatDelta);
221 quat = fDeepCopy(t);
222
223 fFreeHandle(t);
224 fFreeHandle(norm_gyro);
225 fFreeHandle(gyro);
226 fFreeHandle(angleAlpha);
227 fFreeHandle(cosA);
228 fFreeHandle(sinA);
229 }
230 }
231
232 if( asubsref(sType,0) ==4)
233 {
234 //Observation
235 float tempSum=0;
236 F2D *Ovel;
237 float OvelNorm;
238 int i;
239
240 asubsref(STDDEV_GPSPos, 0) = asubsref(sData, 6);
241 asubsref(STDDEV_GPSPos, 4) = asubsref(sData,7);
242 asubsref(STDDEV_GPSPos, 8) = 15;
243
244 Opos = fDeepCopyRange(sData, 0, 1, 0, 3);
245
246 //Initialize
247 for(i=0; i<(pos->height*pos->width); i++)
248 tempSum += asubsref(pos,i);
249
250 if(tempSum == 0)
251 {
252 F2D *t, *t1;
253 t = fMtimes( randW, STDDEV_GPSPos);
254 t1 = fMtimes(ones, Opos);
255
256 for(i=0; i<(pos->height*pos->width); i++)
257 asubsref(pos,i) = asubsref(t,i) + asubsref(t1,i);
258
259 fFreeHandle(t);
260 fFreeHandle(t1);
261 }
262 else
263 {
264 int rows, cols;
265 int mnrows, mncols;
266
267 rows = STDDEV_GPSPos->height;
268 cols = STDDEV_GPSPos->width;
269
270 temp_STDDEV_GPSPos = fSetArray(rows,cols,1);
271 for( mnrows=0; mnrows<rows; mnrows++)
272 {
273 for( mncols=0; mncols<cols; mncols++)
274 {
275 subsref(temp_STDDEV_GPSPos,mnrows,mncols) = pow(subsref(STDDEV_GPSPos,mnrows,mncols),-1);
276 }
277 }
278
279 w = mcl(pos, Opos , temp_STDDEV_GPSPos);
280 generateSample(w, quat, vel, pos);
281 }
282 fFreeHandle(Opos);
283
284 //compare direction
285 Ovel = fDeepCopyRange(sData, 0, 1, 3, 3);
286 OvelNorm=2;//1.1169e+09;
287
288 if (OvelNorm>0.5)
289 {
290 F2D *t;
291 t = fDeepCopy(Ovel);
292 fFreeHandle(Ovel);
293 /* This is a double precision division */
294 Ovel = fDivide(t, OvelNorm);
295 qConj = quatConj(quat);
296 fFreeHandle(t);
297
298 {
299 t = fSetArray(1,3,0);
300 subsref(t,0,0) = 1;
301 orgWorld = quatRot(t, qConj);
302 fFreeHandle(t);
303 fFreeHandle(qConj);
304 t = fSetArray(3,3,0);
305 asubsref(t,0) = 1;
306 asubsref(t,4) = 1;
307 asubsref(t,8) = 1;
308
309 {
310 int i;
311 for(i=0; i<(t->height*t->width); i++)
312 asubsref(t, i) = asubsref(t,i)/STDDEV_GPSVel;
313 w = mcl( orgWorld, Ovel, t);
314 generateSample(w, quat, vel, pos);
315 }
316
317 fFreeHandle(t);
318 fFreeHandle(w);
319 fFreeHandle(orgWorld);
320 }
321 }
322 fFreeHandle(Ovel);
323 }
324
325 if( asubsref(sType,0) ==1)
326 {
327 //Observation
328 F2D *Ovel;
329 F2D *t, *t1, *t2;
330 float valVel;
331
332 t = fSetArray(vel->height, 1, 0);
333
334 for(i=0; i<vel->height; i++)
335 {
336 subsref(t,i,0) = sqrt(pow(subsref(vel,i,0),2) + pow(subsref(vel,i,1),2) + pow(subsref(vel,i,2),2));
337 }
338
339 Ovel = fSetArray(1, 1, asubsref(sData,0));
340 valVel = 1.0/STDDEV_ODOVel;
341
342 t1 = fSetArray(1,1,(1.0/STDDEV_ODOVel));
343 w = mcl (t, Ovel, t1);
344 generateSample(w, quat, vel, pos);
345
346
347 fFreeHandle(w);
348 fFreeHandle(t);
349 fFreeHandle(t1);
350 fFreeHandle(Ovel);
351 }
352
353 if( asubsref(sType,0) ==3)
354 {
355 //Observation
356 F2D *t;
357 t = fSetArray(1, 3, 0);
358 asubsref(t,2) = -9.8;
359
360 accl = fDeepCopyRange(sData, 0, 1, 0, 3);
361 gtemp = fMtimes( ones, t);
362 gravity = quatRot(gtemp, quat);
363
364 fFreeHandle(gtemp);
365 fFreeHandle(t);
366 t = fSetArray(3,3,0);
367 asubsref(t,0) = 1;
368 asubsref(t,4) = 1;
369 asubsref(t,8) = 1;
370
371 {
372 int i;
373 for(i=0; i<(t->height*t->width); i++)
374 asubsref(t,i) = asubsref(t,i)/STDDEV_ACCL;
375 w = mcl( gravity, accl, t);
376 }
377
378 generateSample(w, quat, vel, pos);
379 fFreeHandle(t);
380 //Motion model
381 t = fMtimes(ones, accl);
382 fFreeHandle(accl);
383
384 accl = fMinus(t, gravity);
385
386 fFreeHandle(w);
387 fFreeHandle(gravity);
388 fFreeHandle(t);
389
390 {
391 //pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval+1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2+randn(n,3)*M_STDDEV_POS;
392
393 F2D *s, *is;
394 int i;
395 is = quatConj(quat);
396 s = quatRot(vel, is);
397 fFreeHandle(is);
398
399 for(i=0; i<(s->height*s->width); i++)
400 {
401 asubsref(s,i) = asubsref(s,i)*acclTimeInterval; //+(1/2);
402 }
403 is = fPlus(pos, s);
404 fFreeHandle(pos);
405 pos = fDeepCopy(is);
406 fFreeHandle(is);
407 fFreeHandle(s);
408
409 /** pos_ above stores: pos+quatRot(vel,quatConj(quat))*acclTimeInterval **/
410
411 is = quatConj(quat);
412 s = quatRot(accl, is);
413 t = fDeepCopy(s);
414 for(i=0; i<(s->height*s->width); i++)
415 {
416 asubsref(t,i) = 1/2*asubsref(s,i)*acclTimeInterval*acclTimeInterval;
417 }
418
419 /** t_ above stores: 1/2*quatRot(accl,quatCong(quat))*acclTimeInterval^2 **/
420
421 fFreeHandle(s);
422 fFreeHandle(is);
423 s = randnWrapper(n,3);
424
425 for(i=0; i<(s->height*s->width); i++)
426 {
427 asubsref(s,i) = asubsref(s,i) * M_STDDEV_POS;
428 }
429
430 /** s_ above stores: randn(n,3)*M_STDDEV_POS **/
431
432 is = fPlus(pos, t);
433 fFreeHandle(pos);
434 pos = fPlus(is, s);
435
436 fFreeHandle(s);
437 fFreeHandle(t);
438 fFreeHandle(is);
439
440 // vel=vel+accl*acclTimeInterval+randn(n,3)*M_STDDEV_VEL; %??
441
442 t = fDeepCopy(accl);
443 for(i=0; i<(accl->height*accl->width); i++)
444 asubsref(t,i) = asubsref(accl,i) * acclTimeInterval;
445
446 is = fPlus(vel, t);
447
448 fFreeHandle(accl);
449 fFreeHandle(t);
450 s = randnWrapper(n,3);
451 for(i=0; i<(s->height*s->width); i++)
452 {
453 asubsref(s,i) = asubsref(s,i) * M_STDDEV_VEL;
454 }
455
456 fFreeHandle(vel);
457 vel = fPlus(is, s);
458 fFreeHandle(is);
459 fFreeHandle(s);
460 }
461
462 }
463
464 // Self check
465 {
466 F2D* temp;
467 float quatOut=0, velOut=0, posOut=0;
468 int i;
469
470 for(i=0; i<(quat->height*quat->width); i++)
471 quatOut += asubsref(quat, i);
472
473 for(i=0; i<(vel->height*vel->width); i++)
474 velOut += asubsref(vel, i);
475
476 for(i=0; i<(pos->height*pos->width); i++)
477 posOut += asubsref(pos, i);
478
479 subsref(resultMat,0,icount) = quatOut;
480 subsref(resultMat,1,icount) = velOut;
481 subsref(resultMat,2,icount) = posOut;
482 }
483
484 fFreeHandle(sData);
485
486 if (asubsref(isEOF,0) == 1)
487 break;
488 }
489 fFreeHandle(STDDEV_GPSPos);
490 iFreeHandle(index);
491 iFreeHandle(sType);
492 iFreeHandle(isEOF);
493 fFreeHandle(quat);
494 fFreeHandle(randW);
495 }
496
497#ifdef CHECK
498
499 // Self checking - use expected.txt from data directory
500 {
501 int ret=0;
502 float tol = 2.0;
503#ifdef GENERATE_OUTPUT
504 fWriteMatrix(resultMat, argv[1]);
505#endif
506 ret = fSelfCheck(resultMat, "./expected_C.txt", tol);
507 if (ret == -1)
508 printf("Error in Localization\n");
509 }
510 // Self checking done
511#endif
512
513 fFreeHandle(fid);
514 fFreeHandle(resultMat);
515 fFreeHandle(pos);
516 fFreeHandle(vel);
517 fFreeHandle(ones);
518 fFreeHandle(randn);
519 WRITE_TO_FILE
520 return 0;
521
522}
523
524
525
diff --git a/SD-VBS/benchmarks/localization/src/c/weightedSample.c b/SD-VBS/benchmarks/localization/src/c/weightedSample.c
deleted file mode 100644
index 28099e8..0000000
--- a/SD-VBS/benchmarks/localization/src/c/weightedSample.c
+++ /dev/null
@@ -1,37 +0,0 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include <math.h>
8#include "localization.h"
9
10I2D* weightedSample(F2D* w)
11{
12 I2D *bin;
13 F2D *seed;
14 int n, i, j;
15
16 n = w->height;
17 seed = randWrapper(n, 1);
18 bin = iSetArray(n, 1, 0);
19
20 for(i=0; i<n; i++)
21 {
22 for(j=0; j<n; j++)
23 {
24 if(asubsref(seed,j) > 0)
25 asubsref(bin,j) = asubsref(bin,j) + 1;
26 }
27 for(j=0; j<n; j++)
28 asubsref(seed,j) = asubsref(seed,j) - asubsref(w,i);
29 }
30
31 free(seed);
32 return bin;
33}
34
35
36
37
generated by cgit v1.2.2 (git 2.25.0) at 2026-11-08 23:37:46 -0500