Squashed commit of the sb-vbs branch.

Includes the SD-VBS benchmarks modified to:
- Use libextra to loop as realtime jobs
- Preallocate memory before starting their main computation
- Accept input via stdin instead of via argc

Does not include the SD-VBS matlab code.

Fixes libextra execution in LITMUS^RT.

13 files changed, 1068 insertions, 0 deletions

diff --git a/SD-VBS/benchmarks/localization/src/c/eul2quat.c b/SD-VBS/benchmarks/localization/src/c/eul2quat.c
new file mode 100644
index 0000000..bea75cf
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/eul2quat.c
@@ -0,0 +1,50 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+F2D* eul2quat(F2D* angle)
+{
+    F2D *ret;
+    F2D *x, *y, *z;
+    int k, i, j;
+    int rows, cols;
+
+    rows = angle->height;
+    cols = angle->width;
+
+    x = fDeepCopyRange(angle, 0, angle->height, 0, 1);
+    y = fDeepCopyRange(angle, 0, angle->height, 1, 1);
+    z = fDeepCopyRange(angle, 0, angle->height, 2, 1);
+
+    ret = fSetArray(x->height, 4, 0);
+    
+    for(i=0; i<rows; i++)
+    {
+        float xi, yi, zi;
+        k = 0;
+        xi = asubsref(x,i);
+        yi = asubsref(y,i);
+        zi = asubsref(z,i);
+
+        subsref(ret,i,k) = cos(xi/2)*cos(yi/2)*cos(zi/2)+sin(xi/2)*sin(yi/2)*sin(zi/2);
+        k++;
+        subsref(ret,i,k) = sin(xi/2)*cos(yi/2)*cos(zi/2)-cos(xi/2)*sin(yi/2)*sin(zi/2);
+        k++;
+        subsref(ret,i,k) = cos(xi/2)*sin(yi/2)*cos(zi/2)+sin(xi/2)*cos(yi/2)*sin(zi/2);
+        k++;
+        subsref(ret,i,k) = cos(xi/2)*cos(yi/2)*sin(zi/2)-sin(xi/2)*sin(yi/2)*cos(zi/2);
+    }
+
+    fFreeHandle(x);
+    fFreeHandle(y);
+    fFreeHandle(z);
+
+    return ret;
+}
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/generateSample.c b/SD-VBS/benchmarks/localization/src/c/generateSample.c
new file mode 100644
index 0000000..3acb4d7
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/generateSample.c
@@ -0,0 +1,88 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+void generateSample(F2D *w, F2D *quat, F2D *vel, F2D *pos)
+{
+    int rows, cols, i, j, index;
+    I2D *sampleXId;
+    F2D *retQuat, *retVel, *retPos;
+
+    sampleXId = weightedSample(w);
+
+    rows = sampleXId->height;
+    cols = sampleXId->width;
+
+    if(cols > 1)
+        printf("ERROR: Cols more than 1.. Handle this case \n");
+
+    retQuat = fSetArray(quat->height, quat->width, 0);
+    retVel = fSetArray(vel->height, vel->width, 0);
+    retPos = fSetArray(pos->height, pos->width, 0);
+
+    for(i=0; i<rows; i++)
+    {
+        index = asubsref(sampleXId, i) - 1;
+        for(j=0; j<quat->width; j++)
+        {
+            subsref(retQuat,i,j) = subsref(quat,index,j);
+        }
+    }
+
+    for(i=0; i<rows; i++)
+    {
+        index = asubsref(sampleXId, i) - 1;
+        for(j=0; j<vel->width; j++)
+        {
+            subsref(retVel,i,j) = subsref(vel,index,j);
+        }
+    }
+    
+    for(i=0; i<rows; i++)
+    {
+        index = asubsref(sampleXId, i) - 1;
+        for(j=0; j<pos->width; j++)
+        {
+            subsref(retPos,i,j) = subsref(pos,index,j);
+        }
+    }
+
+    for(i=0; i<quat->height; i++)
+    {
+        for(j=0; j<quat->width; j++)
+        {
+            subsref(quat,i,j) = subsref(retQuat,i,j);
+        }
+    }
+
+    for(i=0; i<vel->height; i++)
+    {
+        for(j=0; j<vel->width; j++)
+        {
+            subsref(vel,i,j) = subsref(retVel,i,j);
+        }
+    }
+    
+    for(i=0; i<pos->height; i++)
+    {
+        for(j=0; j<pos->width; j++)
+        {
+            subsref(pos,i,j) = subsref(retPos,i,j);
+        }
+    }
+    
+    fFreeHandle(retQuat);
+    fFreeHandle(retVel);
+    fFreeHandle(retPos);
+    iFreeHandle(sampleXId);
+    
+    return;
+}
+
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c b/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c
new file mode 100644
index 0000000..58c4475
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c
@@ -0,0 +1,49 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+F2D* get3DGaussianProb( F2D* data, F2D* mean, F2D* A)
+{
+    F2D *p, *diff, *temp1, *temp2, *mt;
+    float temp;
+    int n_data, n_channel;
+    int i, j, k;
+    F2D* t;
+    float pi = 3.1412;
+
+    n_data = data->height;
+    n_channel = data->width;
+
+    t = fSetArray(n_data, 1, 1);
+
+    mt = fMtimes(t, mean); 
+    diff = fMinus( data, mt);
+    p = fSetArray(diff->height, 1, 0);
+
+    temp = sqrt(1.0/(pow(2*pi, n_channel)));
+    temp2 = randWrapper(diff->height,1);
+
+    j = (temp2->height*temp2->width);
+    for(i=0; i<j; i++)
+    {
+        float temp2i;
+        temp2i = asubsref(temp2,i);
+
+        temp2i = exp(-0.5*temp2i);
+        asubsref(p,i) = temp2i*temp;
+    }
+
+    fFreeHandle(t);
+    fFreeHandle(temp2);
+    fFreeHandle(mt);
+    fFreeHandle(diff);
+    
+    return p;
+}
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/input.txt b/SD-VBS/benchmarks/localization/src/c/input.txt
new file mode 100644
index 0000000..dd6d1cf
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/input.txt
@@ -0,0 +1,8 @@
+5   6
+11168.4 3   -0.098  0.247    9.59
+11168.175   1   -0.000000   0.000000    0.000000
+11168.04    4   4422.01 4836.36 33.99
+-0.0006 -0.0007 0.021   9.463   21.130
+11168.4 3   -0.098  0.247   9.59
+11168.4 3   -0.098  0.247   9.59
+
diff --git a/SD-VBS/benchmarks/localization/src/c/localization.h b/SD-VBS/benchmarks/localization/src/c/localization.h
new file mode 100644
index 0000000..b2c03bb
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/localization.h
@@ -0,0 +1,24 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#ifndef _LOCALIZATION_
+#define _LOCALIZATION_
+
+#include "sdvbs_common.h"
+
+int script_localization();
+F2D* eul2quat(F2D* angle);
+void generateSample(F2D *w, F2D *quat, F2D *vel, F2D *pos);
+F2D* get3DGaussianProb( F2D* data, F2D* mean, F2D* A);
+F2D* mcl(F2D* x, F2D* sData, F2D* invConv);
+F2D* quat2eul(F2D* quat);
+F2D* quatConj(F2D* a);
+F2D* quatMul(F2D* a, F2D* b);
+F2D* quatRot(F2D* vec, F2D* rQuat);
+F2D* readSensorData(I2D* index, F2D* fid, I2D* type, I2D* eof);
+I2D* weightedSample(F2D* w);
+
+#endif
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/mcl.c b/SD-VBS/benchmarks/localization/src/c/mcl.c
new file mode 100644
index 0000000..a3c56b6
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/mcl.c
@@ -0,0 +1,34 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+F2D* mcl(F2D* x, F2D* sData, F2D* invConv)
+{
+    int i, j;
+    F2D *retW, *retX, *sum;
+    float sumVal;
+
+    retX = fDeepCopy(x);
+    retW = get3DGaussianProb(retX, sData, invConv);
+    sum = fSum(retW);
+    if(sum->height == 1 && sum->width ==1)
+    {
+        sumVal = asubsref(sum,0);
+        for(i=0; i<retW->height; i++)
+            for(j=0; j<retW->width; j++)
+                subsref(retW,i,j) = subsref(retW,i,j)/sumVal;
+    }
+    else
+        retW = fMdivide(retW, sum);
+
+    fFreeHandle(retX);
+    fFreeHandle(sum);
+
+    return retW;
+}
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/quat2eul.c b/SD-VBS/benchmarks/localization/src/c/quat2eul.c
new file mode 100644
index 0000000..bc581c9
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/quat2eul.c
@@ -0,0 +1,47 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+#include <math.h>
+
+F2D* quat2eul(F2D* quat)
+{
+    F2D *retEul;
+    int i, j, k;
+    int rows, cols;
+
+    rows = quat->height;
+    cols = quat->width;
+
+    retEul = fSetArray(rows, 3, 0);
+
+    for(i=0; i<rows; i++)
+    {
+        float temp, temp1, temp2, temp3, temp4;
+        float quati2, quati3, quati1, quati0;
+
+        quati0 = subsref(quat,i,0);
+        quati1 = subsref(quat,i,1);
+        quati2 = subsref(quat,i,2);
+        quati3 = subsref(quat,i,3);
+
+        temp = 2 *quati2 * quati3 + quati0 * quati1;
+        temp1 = pow(quati0,2) - pow(quati1,2) - pow(quati2,2) + pow(quati3,2);
+        temp2 = -2*quati1 * quati2 + quati0 * quati3;
+        temp3 = 2*quati1 * quati2 + quati0 * quati3;
+        temp4 = pow(quati0,2) + pow(quati1,2) - pow(quati2,2) - pow(quati3,2);
+        
+        asubsref(retEul,k++) = atan2(temp, temp1);
+        asubsref(retEul,k++) = asin(temp2);
+        asubsref(retEul,k++) = atan2(temp3, temp4);
+    }
+
+    return retEul;
+}
+
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/quatConj.c b/SD-VBS/benchmarks/localization/src/c/quatConj.c
new file mode 100644
index 0000000..6607186
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/quatConj.c
@@ -0,0 +1,32 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+F2D* quatConj(F2D* a)
+{
+    F2D* retQuat;
+    int rows, cols;
+    int i, j, k;
+
+    rows = a->height;
+    cols = a->width;
+    retQuat = fSetArray(rows, 4, 0);
+
+    for(i=0; i<rows; i++)
+    {
+        k=0;
+        subsref(retQuat,i,k++) = subsref(a,i,0);
+        subsref(retQuat,i,k++) = -subsref(a,i,1);
+        subsref(retQuat,i,k++) = -subsref(a,i,2);
+        subsref(retQuat,i,k) = -subsref(a,i,3);
+    }
+
+    return retQuat;
+}
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/quatMul.c b/SD-VBS/benchmarks/localization/src/c/quatMul.c
new file mode 100644
index 0000000..8b8dafe
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/quatMul.c
@@ -0,0 +1,55 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+F2D* quatMul(F2D* a, F2D* b)
+{
+    int ra, ca, rb, cb;
+    F2D *ret;
+    int i, j, k=0;
+
+    ra = a->height;
+    ca = a->width;
+
+    rb = b->height;
+    cb = b->width;
+
+    ret = fSetArray(ra, 4, 0);
+
+    j = 0;
+    for(i=0; i<ra; i++)
+    {
+        k = 0;
+        float ai0, ai1, ai2, ai3;
+        float bj0, bj1, bj2, bj3;
+
+        ai0 = subsref(a,i,0);
+        ai1 = subsref(a,i,1);
+        ai2 = subsref(a,i,2);
+        ai3 = subsref(a,i,3);
+        
+        bj0 = subsref(b,j,0);
+        bj1 = subsref(b,j,1);
+        bj2 = subsref(b,j,2);
+        bj3 = subsref(b,j,3);
+        
+        subsref(ret,i,k++) = ai0*bj0 - ai1*bj1 - ai2*bj2 - ai3*bj3;
+        subsref(ret,i,k++) = ai0*bj1 + ai1*bj0 + ai2*bj3 - ai3*bj2;
+        subsref(ret,i,k++) = ai0*bj2 - ai1*bj3 + ai2*bj0 + ai3*bj1;
+        subsref(ret,i,k++) = ai0*bj3 + ai1*bj2 - ai2*bj1 + ai3*bj0;
+    
+        if(rb == ra)
+            j++;
+    }
+
+    return ret;
+}
+
+
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/quatRot.c b/SD-VBS/benchmarks/localization/src/c/quatRot.c
new file mode 100644
index 0000000..4962b55
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/quatRot.c
@@ -0,0 +1,47 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+F2D* quatRot(F2D* vec, F2D* rQuat)
+{
+    F2D *ret;
+    int nr, i, j, k, rows, cols;
+    F2D *tv, *vQuat, *temp, *temp1;
+    F2D *retVec;
+
+    nr = vec->height;
+    tv = fSetArray(nr, 1, 0);
+    vQuat = fHorzcat(tv, vec);
+    temp = quatMul(rQuat, vQuat);
+    temp1 = quatConj(rQuat);
+    retVec = quatMul(temp, temp1);
+
+    rows = retVec->height;
+    cols = retVec->width;
+
+    ret = fSetArray(rows, 3, 0);
+
+    for(i=0; i<rows; i++)
+    {
+        k = 0;
+        for(j=1; j<4; j++)
+        {
+            subsref(ret,i,k) = subsref(retVec,i,j);
+            k++;
+        }
+    }
+
+    fFreeHandle(tv);
+    fFreeHandle(vQuat);
+    fFreeHandle(temp);
+    fFreeHandle(temp1);
+    fFreeHandle(retVec);
+        
+    return ret;
+}
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/readSensorData.c b/SD-VBS/benchmarks/localization/src/c/readSensorData.c
new file mode 100644
index 0000000..12c0dc0
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/readSensorData.c
@@ -0,0 +1,63 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+F2D* readSensorData(I2D* index, F2D* fid, I2D* type, I2D* eof)
+{
+    F2D *retData;
+    int rows, i, j, k;
+    int atype=-1, aindex;
+
+    aindex = asubsref(index, 0);
+
+    asubsref(index,0) = asubsref(index,0) + 1;
+    rows = fid->height;
+    asubsref(type,0) = 0;
+    retData = fSetArray(1, 8, 0);
+    
+    if( asubsref(index,0) > (rows-1) )
+        asubsref(eof,0) = 1;
+    else
+    {
+        if( asubsref(index,0) == rows)
+            asubsref(eof,0) = 1;
+        else
+            asubsref(eof,0) = 0;
+
+        k = asubsref(index,0);
+        atype = subsref(fid, k, 1);
+        if( (atype == 1) || (atype == 2) || (atype == 3) )
+        {
+            for(i=0; i<3; i++)
+            {
+                asubsref(retData,i) = subsref(fid,k,(i+2));
+            }
+        }
+        if( atype == 4 )
+        {
+            for(i=0; i<3; i++)
+            {
+                asubsref(retData,i) = subsref(fid,k,(i+2));
+            }
+            for(i=3; i<8; i++)
+            {
+                asubsref(retData,i) = subsref(fid,k+1,(i-3));
+            }
+            aindex = aindex + 1;
+        }
+        aindex = aindex + 1;
+    }
+
+    asubsref(index,0) = aindex;
+    asubsref(type, 0) = atype;
+
+    return retData;
+}
+
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/script_localization.c b/SD-VBS/benchmarks/localization/src/c/script_localization.c
new file mode 100644
index 0000000..3392320
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/script_localization.c
@@ -0,0 +1,534 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <malloc.h>
+#include "localization.h"
+#include "extra.h"
+#define LOCOLIZATION_MEM 1<<24
+int main(int argc, char* argv[])
+{
+    SET_UP
+    mallopt(M_TOP_PAD, LOCOLIZATION_MEM);
+    mallopt(M_MMAP_MAX, 0);
+    int n, i, j, k, icount=-1;
+    F2D* fid;
+    float gyroTimeInterval=0.01;
+    float acclTimeInterval=0.01;
+
+    float STDDEV_GPSVel=0.5;
+    float STDDEV_ODOVel=0.1;
+    float STDDEV_ACCL=1;
+    float M_STDDEV_GYRO=0.1;
+    float M_STDDEV_POS=0.1;
+    float M_STDDEV_VEL=0.02;
+
+    F2D *pos, *vel;
+    float pi = 3.1416;
+    F2D *eul1, *eul2, *quat;
+    F2D *sData, *gyro, *norm_gyro, *angleAlpha;
+    F2D *quatDelta, *Opos, *temp_STDDEV_GPSPos, *w;
+    F2D *qConj, *orgWorld, *accl, *gtemp;
+    F2D *gravity, *t1;
+    I2D *tStamp, *sType, *isEOF;
+    I2D *index;
+    int rows, cols;
+    F2D *resultMat;
+    F2D *STDDEV_GPSPos;
+    F2D *ones, *randW;
+
+    char im1[100];
+
+    printf("Input txt File: ");
+    scanf("%s", im1);
+
+    fid = readFile(im1);
+
+    n = 1000;
+
+    #ifdef test
+        n = 3;
+        gyroTimeInterval = 0.1;
+        acclTimeInterval = 0.1;
+        M_STDDEV_VEL = 0.2;
+    #endif
+    #ifdef sim_fast
+        n = 3;
+    #endif
+    #ifdef sim
+        n = 10;
+    #endif
+    #ifdef sqcif
+        n = 800;
+    #endif
+    #ifdef qcif
+        n = 500;
+    #endif
+    #ifdef vga
+        n = 2000;
+    #endif
+    #ifdef wuxga
+        n = 3000;
+    #endif
+
+    resultMat = fSetArray(3,fid->height, 0);
+
+    pos = fSetArray(n, 3, 0);
+    vel = fSetArray(n, 3, 0);
+    ones = fSetArray(n,1,1);
+
+    
+    F2D *randn;
+
+    randn = randWrapper(n,3);
+    printf("start\n");
+    for_each_job{
+        fResetArray(pos,n, 3, 0);
+        fResetArray(vel,n, 3, 0);
+        fResetArray(ones,n,1,1);
+
+        {
+            int j;
+            for(i=0; i<n; i++)
+                for(j=0; j<3; j++)
+                    subsref(vel, i, j) += subsref(randn,i,j) * STDDEV_ODOVel;
+
+        }  
+
+
+ 
+        {
+            F2D *eulAngle, *randn;
+            eulAngle = fSetArray(n, 3, 0);
+            randn = randWrapper(n,1);
+    
+            for(i=0; i<n; i++)
+            {
+                subsref(eulAngle, i, 2) = subsref(randn, i, 0) * 2 * pi;
+            }
+            
+            eul1 = eul2quat(eulAngle); 
+            fFreeHandle(eulAngle);
+    
+            eulAngle = fSetArray(1, 3, 0);
+            subsref(eulAngle, 0, 0) = pi;
+            eul2 = eul2quat(eulAngle);
+            
+            fFreeHandle(randn);
+            fFreeHandle(eulAngle);
+        }
+        
+        quat = quatMul(eul1, eul2);
+        fFreeHandle(eul1);
+        fFreeHandle(eul2);
+    
+        i=0;
+        index = iSetArray(1,1,-1);
+        sType = iSetArray(1,1,-1);
+        isEOF = iSetArray(1,1,-1);
+        
+    
+        rows =0;
+        cols = 5;
+        STDDEV_GPSPos = fSetArray(3,3,0);
+        randW = randnWrapper(n,3);
+        icount=-1;
+        while(1)
+        {
+            icount=icount+1;
+            
+            /*     
+            S = 4
+            A = 3
+            G = 2
+            V = 1
+            */
+    
+            sData = readSensorData(index, fid, sType, isEOF);    
+            rows++;
+    
+    
+    
+            if( asubsref(sType,0) ==2)
+            {
+    
+            //Motion model
+                
+                {
+                    int i;
+                    F2D *t, *t1;
+                    F2D *abc, *abcd;
+                    int qD_r=0, qD_c=0;
+                    F2D *cosA, *sinA;
+                    
+                    t = fDeepCopyRange(sData, 0, 1, 0, 3);
+                    gyro = fMtimes(ones, t);
+                    abc = fMallocHandle(gyro->height, gyro->width);
+                    t1 = fDeepCopy(randW);
+                                    
+                    for(i=0; i<(n*3); i++)
+                    {
+                        asubsref(t1,i) = asubsref(randW,i) * M_STDDEV_GYRO;
+                        asubsref(gyro, i) += asubsref(t1,i);
+                        asubsref(abc, i) = pow(asubsref(gyro, i), 2);
+                    }
+                    fFreeHandle(t1);
+                    abcd = fSum2(abc, 2);
+    
+                    norm_gyro = fMallocHandle(abcd->height,abcd->width);
+                    angleAlpha = fMallocHandle(abcd->height, abcd->width);
+    
+                    for(i=0; i<(abcd->height*abcd->width); i++)
+                    {
+                        asubsref(norm_gyro, i) = sqrt(asubsref(abcd,i));
+                        asubsref(angleAlpha,i) = asubsref(norm_gyro,i) * gyroTimeInterval;
+                    }
+    
+                    qD_r += angleAlpha->height + gyro->height;
+                    qD_c += angleAlpha->width + 3;
+                    
+                    fFreeHandle(t);
+                    fFreeHandle(abcd); 
+                    
+                    cosA = fSetArray(angleAlpha->height, angleAlpha->width, 0);
+                    sinA = fSetArray(angleAlpha->height, angleAlpha->width, 0);
+    
+                    for(i=0; i<(cosA->height*cosA->width); i++)
+                        asubsref(cosA,i) = cos( asubsref(angleAlpha,i) /2 );
+     
+                    for(i=0; i<(sinA->height*sinA->width); i++)
+                        asubsref(sinA,i) = sin( asubsref(angleAlpha,i) /2 );
+        
+                    
+                    fFreeHandle(abc);
+                    abc = fSetArray(1,3,1);
+                    t1 = fMtimes(norm_gyro, abc);
+                    t = ffDivide(gyro, t1);
+                    fFreeHandle(t1);
+    
+                    abcd = fMtimes(sinA, abc);
+                    t1 = fTimes(t, abcd);
+                    quatDelta = fHorzcat(cosA, t1);
+                    
+                    fFreeHandle(abcd);
+                    fFreeHandle(t);
+                    fFreeHandle(t1);
+                    fFreeHandle(abc);
+    
+                    t = quatMul(quat, quatDelta);
+                    fFreeHandle(quat);
+                    fFreeHandle(quatDelta);
+                    quat = fDeepCopy(t);
+                    
+                    fFreeHandle(t);
+                    fFreeHandle(norm_gyro);
+                    fFreeHandle(gyro);
+                    fFreeHandle(angleAlpha);
+                    fFreeHandle(cosA);
+                    fFreeHandle(sinA);
+    
+                }            
+           }
+    
+         if( asubsref(sType,0) ==4)
+         {
+             //Observation
+            
+            float tempSum=0;
+            F2D *Ovel;
+            float OvelNorm;
+            int i;
+    
+    
+            asubsref(STDDEV_GPSPos, 0) = asubsref(sData, 6);
+            asubsref(STDDEV_GPSPos, 4) = asubsref(sData,7);
+            asubsref(STDDEV_GPSPos, 8) = 15;
+    
+            Opos = fDeepCopyRange(sData, 0, 1, 0, 3);
+     
+            //Initialize
+             
+            for(i=0; i<(pos->height*pos->width); i++)
+                tempSum += asubsref(pos,i);
+    
+            if(tempSum == 0)
+            {
+                F2D *t, *t1;
+                t = fMtimes( randW, STDDEV_GPSPos);
+                t1 = fMtimes(ones, Opos);
+    
+                for(i=0; i<(pos->height*pos->width); i++)
+                    asubsref(pos,i) = asubsref(t,i) + asubsref(t1,i);
+    
+                fFreeHandle(t);
+                fFreeHandle(t1);
+            }
+            else 
+            {
+                int rows, cols;
+                int mnrows, mncols;
+    
+                rows = STDDEV_GPSPos->height;
+                cols = STDDEV_GPSPos->width;
+    
+                temp_STDDEV_GPSPos = fSetArray(rows,cols,1);
+                for( mnrows=0; mnrows<rows; mnrows++)
+                {
+                    for( mncols=0; mncols<cols; mncols++)
+                    {
+                        subsref(temp_STDDEV_GPSPos,mnrows,mncols) = pow(subsref(STDDEV_GPSPos,mnrows,mncols),-1);
+                    }
+                }
+    
+                w = mcl(pos, Opos , temp_STDDEV_GPSPos);
+                generateSample(w, quat, vel, pos);
+            }
+            fFreeHandle(Opos);
+    
+            //compare direction
+            Ovel = fDeepCopyRange(sData, 0, 1, 3, 3);
+            OvelNorm=2;//1.1169e+09;
+        
+            if (OvelNorm>0.5)
+            {
+                F2D *t;
+                t = fDeepCopy(Ovel);
+                fFreeHandle(Ovel);
+                /* This is a double precision division */
+                Ovel = fDivide(t, OvelNorm);
+                qConj = quatConj(quat);
+                fFreeHandle(t);
+                
+                {
+                    t = fSetArray(1,3,0);
+                    subsref(t,0,0) = 1;
+                    orgWorld = quatRot(t, qConj);
+                    fFreeHandle(t);
+                    fFreeHandle(qConj);
+                    t = fSetArray(3,3,0);
+                    asubsref(t,0) = 1;
+                    asubsref(t,4) = 1;
+                    asubsref(t,8) = 1;
+                   
+                    {
+                        int i;
+                        for(i=0; i<(t->height*t->width); i++)
+                            asubsref(t, i) = asubsref(t,i)/STDDEV_GPSVel;
+                        w = mcl( orgWorld, Ovel, t);
+                        generateSample(w, quat, vel, pos);
+                    }
+    
+                    fFreeHandle(t);
+                    fFreeHandle(w);
+                    fFreeHandle(orgWorld);
+                }
+            }
+            fFreeHandle(Ovel);
+        }
+        
+        if( asubsref(sType,0) ==1)
+        {
+            //Observation
+            F2D *Ovel;
+            F2D *t, *t1, *t2;
+            float valVel;
+    
+            t = fSetArray(vel->height, 1, 0);
+    
+            for(i=0; i<vel->height; i++)
+            {
+                subsref(t,i,0) = sqrt(pow(subsref(vel,i,0),2) + pow(subsref(vel,i,1),2) + pow(subsref(vel,i,2),2));
+            }
+    
+            Ovel = fSetArray(1, 1, asubsref(sData,0));
+            valVel = 1.0/STDDEV_ODOVel;
+    
+            t1 = fSetArray(1,1,(1.0/STDDEV_ODOVel));
+            w = mcl (t, Ovel, t1);
+            generateSample(w, quat, vel, pos);
+    
+            
+            fFreeHandle(w);
+            fFreeHandle(t);
+            fFreeHandle(t1);
+            fFreeHandle(Ovel);
+        }
+        
+        if( asubsref(sType,0) ==3)
+        {
+            //Observation
+            F2D *t;
+            t = fSetArray(1, 3, 0);
+            asubsref(t,2) = -9.8;
+    
+            accl = fDeepCopyRange(sData, 0, 1, 0, 3);
+            gtemp = fMtimes( ones, t); 
+            gravity = quatRot(gtemp, quat);
+    
+            fFreeHandle(gtemp);
+            fFreeHandle(t);
+            t = fSetArray(3,3,0);
+            asubsref(t,0) = 1;
+            asubsref(t,4) = 1;
+            asubsref(t,8) = 1;
+            
+            {                
+                int i;
+                for(i=0; i<(t->height*t->width); i++)
+                    asubsref(t,i) = asubsref(t,i)/STDDEV_ACCL;
+                w = mcl( gravity, accl, t);
+            }
+            
+            generateSample(w, quat, vel, pos);
+            fFreeHandle(t);
+            //Motion model
+            t = fMtimes(ones, accl);
+            fFreeHandle(accl);
+    
+            accl = fMinus(t, gravity);
+            
+            fFreeHandle(w);
+            fFreeHandle(gravity);
+            fFreeHandle(t);
+    
+            {
+                //pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval+1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2+randn(n,3)*M_STDDEV_POS;            
+                
+                F2D *s, *is;
+                int i;
+                is = quatConj(quat);
+                s = quatRot(vel, is);
+                fFreeHandle(is);
+    
+                for(i=0; i<(s->height*s->width); i++)
+                {
+                    asubsref(s,i) = asubsref(s,i)*acclTimeInterval;   //+(1/2);
+                }
+                is = fPlus(pos, s);
+                fFreeHandle(pos);
+                pos = fDeepCopy(is);
+                fFreeHandle(is);
+                fFreeHandle(s);
+    
+                /** pos_ above stores: pos+quatRot(vel,quatConj(quat))*acclTimeInterval **/
+                
+                is = quatConj(quat);
+                s = quatRot(accl, is);
+                t = fDeepCopy(s);
+                for(i=0; i<(s->height*s->width); i++)
+                {
+                    asubsref(t,i) = 1/2*asubsref(s,i)*acclTimeInterval*acclTimeInterval;
+                }
+    
+                /** t_ above stores: 1/2*quatRot(accl,quatCong(quat))*acclTimeInterval^2 **/
+    
+                fFreeHandle(s);
+                fFreeHandle(is);
+                s = randnWrapper(n,3);
+    
+                for(i=0; i<(s->height*s->width); i++)
+                {
+                    asubsref(s,i) = asubsref(s,i) * M_STDDEV_POS;
+                }
+    
+                /** s_ above stores: randn(n,3)*M_STDDEV_POS **/
+    
+                is = fPlus(pos, t);
+                fFreeHandle(pos);
+                pos = fPlus(is, s);
+    
+                fFreeHandle(s);
+                fFreeHandle(t);
+                fFreeHandle(is);
+            
+    //            vel=vel+accl*acclTimeInterval+randn(n,3)*M_STDDEV_VEL; %??
+                
+                t = fDeepCopy(accl);
+                for(i=0; i<(accl->height*accl->width); i++)
+                    asubsref(t,i) = asubsref(accl,i) * acclTimeInterval;
+                
+                is = fPlus(vel, t);
+    
+                fFreeHandle(accl);
+                fFreeHandle(t);
+                s = randnWrapper(n,3);
+                for(i=0; i<(s->height*s->width); i++)
+                {
+                    asubsref(s,i) = asubsref(s,i) * M_STDDEV_VEL;
+                }
+    
+                fFreeHandle(vel);
+                vel = fPlus(is, s);
+                fFreeHandle(is);
+                fFreeHandle(s);
+           }
+    
+        }
+    
+            // Self check
+            {
+                F2D* temp;
+                float quatOut=0, velOut=0, posOut=0;
+                int i;
+    
+                for(i=0; i<(quat->height*quat->width); i++)
+                    quatOut += asubsref(quat, i);
+    
+                for(i=0; i<(vel->height*vel->width); i++)
+                    velOut += asubsref(vel, i);
+                
+                for(i=0; i<(pos->height*pos->width); i++)
+                    posOut += asubsref(pos, i);
+                
+                subsref(resultMat,0,icount) = quatOut;
+                subsref(resultMat,1,icount) = velOut;
+                subsref(resultMat,2,icount) = posOut;
+            }
+    
+            fFreeHandle(sData);
+    
+        
+            if (asubsref(isEOF,0) == 1)
+               break;
+        }
+    }
+    printf("end..\n");
+
+#ifdef CHECK   
+    
+    // Self checking - use expected.txt from data directory
+    {
+        int ret=0;
+        float tol = 2.0;
+#ifdef GENERATE_OUTPUT
+        fWriteMatrix(resultMat, argv[1]);
+#endif
+        ret = fSelfCheck(resultMat, "./expected_C.txt", tol);
+        if (ret == -1)
+            printf("Error in Localization\n");
+    }
+    // Self checking done
+#endif
+
+    
+    fFreeHandle(STDDEV_GPSPos);
+    iFreeHandle(index);
+    iFreeHandle(sType);
+    iFreeHandle(isEOF);
+    fFreeHandle(fid);
+    fFreeHandle(resultMat);
+    fFreeHandle(pos);
+    fFreeHandle(vel);
+    fFreeHandle(quat);
+    fFreeHandle(ones);
+    fFreeHandle(randW);
+    fFreeHandle(randn);
+    WRITE_TO_FILE
+    return 0;
+
+}
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/weightedSample.c b/SD-VBS/benchmarks/localization/src/c/weightedSample.c
new file mode 100644
index 0000000..28099e8
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/weightedSample.c
@@ -0,0 +1,37 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include "localization.h"
+
+I2D* weightedSample(F2D* w)
+{
+    I2D *bin;
+    F2D *seed;
+    int n, i, j;
+
+    n = w->height;
+    seed = randWrapper(n, 1);
+    bin = iSetArray(n, 1, 0);
+
+    for(i=0; i<n; i++)
+    {
+        for(j=0; j<n; j++)
+        {
+            if(asubsref(seed,j) > 0)
+                asubsref(bin,j) = asubsref(bin,j) + 1;
+        }
+        for(j=0; j<n; j++)
+            asubsref(seed,j) = asubsref(seed,j) - asubsref(w,i);
+    }
+
+    free(seed);
+    return bin;
+}
+
+
+
+