57 files changed, 1752 insertions, 0 deletions
diff --git a/SD-VBS/benchmarks/localization/Makefile b/SD-VBS/benchmarks/localization/Makefile
new file mode 100644
index 0000000..60cc0ec
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/Makefile
@@ -0,0 +1,3 @@
+include ../../common/makefiles/Makefile.include
+include $(MAKEFILE_COMMON_DIR)/Makefile.recurse
diff --git a/SD-VBS/benchmarks/localization/data/Makefile b/SD-VBS/benchmarks/localization/data/Makefile
new file mode 100644
index 0000000..ba33d35
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/Makefile
@@ -0,0 +1,3 @@
+include ../../../common/makefiles/Makefile.include
+include $(MAKEFILE_COMMON_DIR)/Makefile.recurse
diff --git a/SD-VBS/benchmarks/localization/data/cif/Makefile b/SD-VBS/benchmarks/localization/data/cif/Makefile
new file mode 100644
index 0000000..4bd562f
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/cif/Makefile
@@ -0,0 +1,7 @@
+include ../../../../common/makefiles/Makefile.include
+BMARK=localization
+INPUT=cif
+include $(MAKEFILE_COMMON_DIR)/Makefile.common
diff --git a/SD-VBS/benchmarks/localization/data/cif/VBS_Ver1 b/SD-VBS/benchmarks/localization/data/cif/VBS_Ver1
new file mode 100755
index 0000000..b04da34
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/cif/VBS_Ver1
Binary files differ
diff --git a/SD-VBS/benchmarks/localization/data/cif/cif b/SD-VBS/benchmarks/localization/data/cif/cif
new file mode 100755
index 0000000..8489f9a
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/cif/cif
Binary files differ
diff --git a/SD-VBS/benchmarks/localization/data/cif/expected.m b/SD-VBS/benchmarks/localization/data/cif/expected.m
new file mode 100644
index 0000000..22ef940
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/cif/expected.m
@@ -0,0 +1,3 @@
+1264.100919     1018.646860     -66.744065      1325.148554     -524.072802     104.959493      1293.736304     -176.791171     129.794173      28.123781       -825.907469     110.949588      -53.030419      -387.071088     175.278628      -306.960407     -332.128807     -859.326326     96.550227       -269.287595     -19.835881      207.383563      -251.431495     -71.876639      241.454426      -291.088405     115.707219      337.421875      329.434722      -31.556631      200.761584      273.097048      -209.534328     -460.765107     186.245449      116.289195      -351.230797     108.699099      -71.331712      386.736600      -292.419981     -0.334981       -242.589950     -353.060399     -99.262004      -109.249025     -360.813810     -40.844284      -206.722293     -430.241768     29.072174       -266.322402     -260.740639     -436.982547     -175.357022     283.583788      -279.093973     -387.176464     248.837096      447.196705      -268.019516     -298.939012     343.244024      206.071785      -225.746220     -187.288143     200.444712      -93.034358      -165.697562     15.808615       -304.358091     415.538855      63.043500       -136.137632     -383.437919     -7.746789       -329.599742     -221.224972     -348.912560     -202.666314     -227.899182     -232.552441     384.227756      166.317852      -184.182201     -68.725094      54.730375       0.000000        
+148.469605      249783.963758   249783.963758   2180.681112     383736.909385   383736.909385   253883.626524   253883.626524   476143.774773   476143.774773   389683.019120   389683.019120   545807.588908   545807.588908   232299.455988   599622.582238   599622.582238   417714.138595   417714.138595   646854.785707   646854.785707   517757.007454   517757.007454   687063.797171   687063.797171   337856.935013   720555.709843   720555.709843   508182.601804   508182.601804   752242.685858   752242.685858   596994.900082   596994.900082   781076.846278   781076.846278   408387.972486   804774.536536   804774.536536   572505.759937   572505.759937   829541.440024   829541.440024   656637.597514   656637.597514   852188.058601   852188.058601   464317.955836   871738.416433   871738.416433   625271.852328   625271.852328   892047.804954   892047.804954   706766.734908   706766.734908   911202.007529   911202.007529   761878.379462   761878.379462   929603.367197   929603.367197   805063.056324   805063.056324   947078.978085   947078.978085   841497.089926   841497.089926   714530.798901   871063.266696   871063.266696   847094.846486   847094.846486   900419.631759   900419.631759   907069.193562   907069.193562   927072.876646   927072.876646   695517.442884   948856.006854   948856.006854   840738.929439   840738.929439   972511.540879   972511.540879   909607.468542   0.000000        
+4907681162.639265       4996983773.346421       4996983773.346421       4907470495.183228       4968360833.406909       4968360833.406909       4908709186.117507       4908709186.117507       4953769759.629911       4953769759.629911       4909434990.624061       4909434990.624061       4944946321.273969       4944946321.273969       4908726987.816192       4939055160.653013       4939055160.653013       4909755868.543180       4909755868.543180       4934891702.718277       4934891702.718277       4910373711.010693       4910373711.010693       4931739973.964623       4931739973.964623       4909601991.354140       4929263185.565628       4929263185.565628       4910590381.898005       4910590381.898005       4927366117.137535       4927366117.137535       4911182724.385408       4911182724.385408       4925842684.055954       4925842684.055954       4910387212.896085       4924580415.310893       4924580415.310893       4911364419.082825       4911364419.082825       4923588892.702791       4923588892.702791       4911945399.855221       4911945399.855221       4922665132.406710       4922665132.406710       4911146778.096742       4921854906.683125       4921854906.683125       4912117527.337012       4912117527.337012       4921247956.389248       4921247956.389248       4912691752.735250       4912691752.735250       4920632038.287462       4920632038.287462       4913140704.670214       4913140704.670214       4920169402.955149       4920169402.955149       4913521889.855532       4913521889.855532       4919786752.370605       4919786752.370605       4913870380.414716       4913870380.414716       4918135706.514756       4914176948.628077       4914176948.628077       4918436009.170393       4918436009.170393       4914475956.108191       4914475956.108191       4918424403.658169       4918424403.658169       4914736253.512666       4914736253.512666       4917126279.233858       4914920865.993999       4914920865.993999       4917765910.500541       4917765910.500541       4915042054.464935       4915042054.464935       4918118358.250251       0.000000        
diff --git a/SD-VBS/benchmarks/localization/data/cif/localization b/SD-VBS/benchmarks/localization/data/cif/localization
new file mode 100755
index 0000000..be524d2
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/cif/localization
Binary files differ
diff --git a/SD-VBS/benchmarks/localization/data/qcif/Makefile b/SD-VBS/benchmarks/localization/data/qcif/Makefile
new file mode 100644
index 0000000..d18c02d
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/qcif/Makefile
@@ -0,0 +1,8 @@
+include ../../../../common/makefiles/Makefile.include
+BMARK=localization
+INPUT=qcif
+include $(MAKEFILE_COMMON_DIR)/Makefile.common
diff --git a/SD-VBS/benchmarks/localization/data/qcif/expected.m b/SD-VBS/benchmarks/localization/data/qcif/expected.m
new file mode 100644
index 0000000..4263b3d
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/qcif/expected.m
@@ -0,0 +1,3 @@
+631.976450      516.498284      -197.996728     663.497659      -22.288426      -28.903321      647.817138      -351.955521     -312.433355     44.896667       -150.353868     -67.000702      -370.502974     26.299828       -488.261017     -321.672753     -20.467725      -293.286835     -85.465502      -243.718793     -74.565859      -549.266195     30.999329       -160.813808     -121.355881     -483.242401     -79.323855      -151.906467     -595.771606     12.823360       -12.448671      -172.470587     -511.488467     -21.514722      42.089059       -185.300910     -324.317055     89.756283       -187.135260     -628.505815     29.732164       124.069805      -182.587245     -367.573870     -76.374400      148.479017      -172.587616     -145.562423     167.983942      -157.609772     -529.296685     21.322336       179.630194      -137.981888     -185.584066     -112.106506     185.662406      -114.184459     25.859838       -163.799486     189.409619      -87.299860      149.829422      -166.716356     189.327882      -58.163997      221.918824      -144.524825     183.798380      253.096143      -116.072162     284.431081      -110.520707     261.347591      -74.376768      318.351401      -83.847971      256.673574      -30.452642      285.051087      232.480691      6.336376        352.034688      -65.985798      202.705055      47.633804       382.774465      0.000000        
+74.359725       55442.431887    55442.431887    916.112559      85048.747743    85048.747743    57211.398438    57211.398438    105779.314664   105779.314664   87600.029083    87600.029083    121512.447801   121512.447801   52860.067135    133141.106400   133141.106400   94517.909665    94517.909665    143946.045181   143946.045181   117118.457039   117118.457039   153064.488091   153064.488091   77359.640694    160318.887671   160318.887671   115674.880954   115674.880954   167827.918882   167827.918882   135968.435504   135968.435504   174698.161121   174698.161121   94713.555770    180133.542229   180133.542229   131871.515823   131871.515823   186165.689411   186165.689411   151252.883116   151252.883116   191909.078626   191909.078626   109392.213684   196358.978485   196358.978485   146078.668403   146078.668403   201651.395781   201651.395781   165046.805127   165046.805127   206711.131954   206711.131954   178320.376082   178320.376082   211596.482547   211596.482547   188947.507546   188947.507546   216497.738288   216497.738288   197921.386091   197921.386091   165985.571620   205091.855127   205091.855127   196779.521358   196779.521358   212609.207639   212609.207639   211533.666997   211533.666997   219561.468379   219561.468379   166056.319552   225227.570848   225227.570848   200047.795792   200047.795792   231596.764178   231596.764178   216995.904046   0.000000        
+2453723755.101060       2473138738.478060       2473138738.478060       2453633812.224546       2466744375.113051       2466744375.113051       2453912441.757200       2453912441.757200       2463514723.222767       2463514723.222767       2454091490.868995       2454091490.868995       2461535850.721794       2461535850.721794       2453959313.433379       2460231345.559122       2460231345.559122       2454218817.973337       2454218817.973337       2459307064.549955       2459307064.549955       2454389996.064240       2454389996.064240       2458639118.655468       2458639118.655468       2454254726.669796       2458128486.626955       2458128486.626955       2454513520.017527       2454513520.017527       2457726299.581051       2457726299.581051       2454684774.597973       2454684774.597973       2457410967.437045       2457410967.437045       2454549597.959815       2457150540.743558       2457150540.743558       2454807555.250977       2454807555.250977       2456916627.143601       2456916627.143601       2454977251.488404       2454977251.488404       2456719675.377667       2456719675.377667       2454837557.911356       2456573728.470717       2456573728.470717       2455086906.118260       2455086906.118260       2456453336.049111       2456453336.049111       2455239927.219034       2455239927.219034       2456371158.832802       2456371158.832802       2455342631.649824       2455342631.649824       2456355762.744594       2456355762.744594       2455396094.462677       2455396094.462677       2456381099.252142       2456381099.252142       2455441691.541200       2455441691.541200       2456128146.863575       2455477781.414840       2455477781.414840       2456282488.340037       2456282488.340037       2455516633.096739       2455516633.096739       2456357066.451140       2456357066.451140       2455552537.419121       2455552537.419121       2456129670.296010       2455581213.884797       2455581213.884797       2456300055.909151       2456300055.909151       2455615007.049098       2455615007.049098       2456385531.538922       0.000000        
diff --git a/SD-VBS/benchmarks/localization/data/qcif/localization b/SD-VBS/benchmarks/localization/data/qcif/localization
new file mode 100755
index 0000000..e2fb417
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/qcif/localization
Binary files differ
diff --git a/SD-VBS/benchmarks/localization/data/sim/Makefile b/SD-VBS/benchmarks/localization/data/sim/Makefile
new file mode 100644
index 0000000..5dde8fe
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/sim/Makefile
@@ -0,0 +1,8 @@
+include ../../../../common/makefiles/Makefile.include
+BMARK=localization
+INPUT=sim
+include $(MAKEFILE_COMMON_DIR)/Makefile.common
diff --git a/SD-VBS/benchmarks/localization/data/sim/expected.m b/SD-VBS/benchmarks/localization/data/sim/expected.m
new file mode 100644
index 0000000..eb3b6a7
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/sim/expected.m
@@ -0,0 +1,3 @@
+12.543452       13.658157       13.497180       13.894946       13.959641       13.788874       13.848866       13.673453       13.788413       13.616173       13.675684       13.496716       13.628587       13.453379       13.516593       13.560943       13.384332       13.462265       13.282183       13.388606       13.211571       13.284906       13.102213       13.225472       13.047242       13.124615       13.157999       12.976742       13.069521       12.884701       12.981088       12.800202       12.888504       12.701228       12.814751       12.629592       12.721974       12.743435       12.557293       12.665573       12.476815       12.562721       12.375911       12.479525       12.287222       12.390423       12.201016       12.309248       12.317883       12.128404       12.252886       12.058258       12.132629       11.941091       12.061026       11.865997       11.957748       11.764361       11.888852       11.692058       11.785975       11.590614       11.715017       11.515945       11.611867       11.414693       11.539084       11.338245       11.436080       11.460551       11.257957       11.375602       11.175996       11.267413       11.064235       11.177627       10.976366       11.082948       10.877846       10.998140       11.002795       10.797280       10.937839       10.734714       10.807476       10.600371       10.736720       0.000000        
+1.632000        8.714650        8.714650        5.129768        12.757521       12.757521       15.132782       15.132782       19.299372       19.299372       21.467915       21.467915       24.972680       24.972680       19.672340       27.024481       27.024481       28.587185       28.587185       33.309821       33.309821       34.866787       34.866787       38.902191       38.902191       33.038006       40.913687       40.913687       41.893676       41.893676       47.112254       47.112254       48.100977       48.100977       52.615488       52.615488       46.241199       54.584753       54.584753       55.038349       55.038349       60.707646       60.707646       61.196608       61.196608       66.137221       66.137221       59.308127       68.071141       68.071141       68.059927       68.059927       74.125293       74.125293       74.174756       74.174756       79.495501       79.495501       79.469368       79.469368       84.667799       84.667799       84.745876       84.745876       89.812215       89.812215       90.008499       90.008499       87.828764       91.936749       91.936749       96.350144       96.350144       98.015654       98.015654       102.254126      102.254126      103.429097      103.429097      100.240384      105.344169      105.344169      108.716083      108.716083      111.397402      111.397402      114.567665      0.000000        
+49065484.652770 49065766.480798 49065766.480798 49064922.448163 49065254.824174 49065254.824174 49064855.994251 49064855.994251 49065287.991179 49065287.991179 49064888.152999 49064888.152999 49065317.057455 49065317.057455 49064879.431549 49065327.674120 49065327.674120 49064924.733918 49064924.733918 49065360.164908 49065360.164908 49064957.077832 49064957.077832 49065389.450818 49065389.450818 49064948.446741 49065400.174667 49065400.174667 49064993.921333 49064993.921333 49065432.868549 49065432.868549 49065026.426106 49065026.426106 49065462.347468 49065462.347468 49065017.873205 49065473.165037 49065473.165037 49065063.495276 49065063.495276 49065506.035072 49065506.035072 49065096.136128 49065096.136128 49065535.679932 49065535.679932 49065087.648925 49065546.577815 49065546.577815 49065133.393946 49065133.393946 49065579.597091 49065579.597091 49065166.145727 49065166.145727 49065609.380717 49065609.380717 49065194.860230 49065194.860230 49065638.305742 49065638.305742 49065223.608757 49065223.608757 49065667.272949 49065667.272949 49065252.386982 49065252.386982 49065658.990318 49065263.289712 49065263.289712 49065705.110400 49065705.110400 49065296.295918 49065296.295918 49065738.239086 49065738.239086 49065326.035265 49065326.035265 49065729.995233 49065336.962269 49065336.962269 49065776.182626 49065776.182626 49065370.006395 49065370.006395 49065809.367845 0.000000        
diff --git a/SD-VBS/benchmarks/localization/data/sim/localization b/SD-VBS/benchmarks/localization/data/sim/localization
new file mode 100755
index 0000000..4650997
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/sim/localization
Binary files differ
diff --git a/SD-VBS/benchmarks/localization/data/sim_fast/Makefile b/SD-VBS/benchmarks/localization/data/sim_fast/Makefile
new file mode 100644
index 0000000..236ff8b
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/sim_fast/Makefile
@@ -0,0 +1,8 @@
+include ../../../../common/makefiles/Makefile.include
+BMARK=localization
+INPUT=sim_fast
+include $(MAKEFILE_COMMON_DIR)/Makefile.common
diff --git a/SD-VBS/benchmarks/localization/data/sim_fast/expected.m b/SD-VBS/benchmarks/localization/data/sim_fast/expected.m
new file mode 100644
index 0000000..9841d62
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/sim_fast/expected.m
@@ -0,0 +1,3 @@
+3.685028        3.432348        3.427322        3.424184        3.427974        3.422786        3.419637        3.414578        3.415206        3.410068        3.410692        3.405226        3.405845        3.400579        3.401194        3.405009        3.399313        3.396114        3.390933        3.391537        3.386565        3.387164        3.382115        3.382710        0.000000        
+0.570000        1.130093        1.130093        1.003619        1.457224        1.457224        1.872936        1.872936        2.195236        2.195236        2.517823        2.517823        2.839430        2.839430        2.624478        3.070964        3.070964        3.479898        3.479898        3.796360        3.796360        4.113478        4.113478        4.428227        0.000000        
+14719241.537176 14719192.885416 14719192.885416 14719192.253043 14719194.529914 14719194.529914 14719196.620169 14719196.620169 14719198.260497 14719198.260497 14719199.898726 14719199.898726 14719201.534841 14719201.534841 14719200.454656 14719202.721015 14719202.721015 14719204.800656 14719204.800656 14719206.430317 14719206.430317 14719208.057811 14719208.057811 14719209.683107 0.000000        
diff --git a/SD-VBS/benchmarks/localization/data/sim_fast/localization b/SD-VBS/benchmarks/localization/data/sim_fast/localization
new file mode 100755
index 0000000..82e40a9
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/sim_fast/localization
Binary files differ
diff --git a/SD-VBS/benchmarks/localization/data/sqcif/Makefile b/SD-VBS/benchmarks/localization/data/sqcif/Makefile
new file mode 100644
index 0000000..496053b
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/sqcif/Makefile
@@ -0,0 +1,8 @@
+include ../../../../common/makefiles/Makefile.include
+BMARK=localization
+INPUT=sqcif
+include $(MAKEFILE_COMMON_DIR)/Makefile.common
diff --git a/SD-VBS/benchmarks/localization/data/sqcif/expected.m b/SD-VBS/benchmarks/localization/data/sqcif/expected.m
new file mode 100644
index 0000000..ec25c0d
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/sqcif/expected.m
@@ -0,0 +1,3 @@
+1011.303794     817.965338      52.925944       1061.980393     0.000000        
+118.818062      154080.851618   154080.851618   1652.527205     0.000000        
+3926083200.206786       3980824114.940394       3980824114.940394       3925922636.742242       0.000000        
diff --git a/SD-VBS/benchmarks/localization/data/sqcif/localization b/SD-VBS/benchmarks/localization/data/sqcif/localization
new file mode 100755
index 0000000..d26f656
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/sqcif/localization
Binary files differ
diff --git a/SD-VBS/benchmarks/localization/data/test/Makefile b/SD-VBS/benchmarks/localization/data/test/Makefile
new file mode 100644
index 0000000..c997c9b
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/test/Makefile
@@ -0,0 +1,8 @@
+include ../../../../common/makefiles/Makefile.include
+BMARK=localization
+INPUT=test
+include $(MAKEFILE_COMMON_DIR)/Makefile.common
diff --git a/SD-VBS/benchmarks/localization/data/test/expected.m b/SD-VBS/benchmarks/localization/data/test/expected.m
new file mode 100644
index 0000000..d64ef46
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/test/expected.m
@@ -0,0 +1,3 @@
+3.685028        3.432348        3.380028        3.349426        0.000000        
+0.570000        6.035935        6.035935        4.771188        0.000000        
+14719241.537176 14719192.900190 14719192.900190 14719192.267816 0.000000        
diff --git a/SD-VBS/benchmarks/localization/data/test/localization b/SD-VBS/benchmarks/localization/data/test/localization
new file mode 100755
index 0000000..2ac32af
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/test/localization
Binary files differ
diff --git a/SD-VBS/benchmarks/localization/data/vga/Makefile b/SD-VBS/benchmarks/localization/data/vga/Makefile
new file mode 100644
index 0000000..9d6ce33
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/vga/Makefile
@@ -0,0 +1,7 @@
+include ../../../../common/makefiles/Makefile.include
+BMARK=localization
+INPUT=vga
+include $(MAKEFILE_COMMON_DIR)/Makefile.common
diff --git a/SD-VBS/benchmarks/localization/data/vga/expected.m b/SD-VBS/benchmarks/localization/data/vga/expected.m
new file mode 100644
index 0000000..652c96f
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/vga/expected.m
@@ -0,0 +1,3 @@
+2528.691892     2020.606280     42.673530       2649.158291     -514.634787     -292.512047     2578.263949     -7.269003       194.344630      324.075534      -884.272217     -529.018602     -176.396317     -373.999381     141.933329      509.836051      -61.997773      -777.150225     -582.644009     -1449.348371    117.943370      -1299.856855    -24.357194      303.856978      101.324880      1351.287196     1422.667628     -81.220795      344.584054      305.716500      394.536408      -315.995792     1224.113905     -116.964887     -895.290651     -373.194309     -1302.711786    -1435.755958    -242.449596     -575.245533     -412.057612     -1163.249025    31.633454       -103.634994     415.883979      -436.888318     292.040993      -444.018359     393.038935      455.211286      226.985703      -74.940082      1088.234539     506.285008      -1138.224729    231.353564      1450.108183     424.629158      1302.669340     -76.648346      1416.427855     244.239852      681.206342      -224.646828     1050.911515     9.704005        -1471.352151    -152.384502     480.602928      -1300.853407    143.685698      523.473516      287.188506      131.080484      309.861529      -1053.044149    150.207963      1360.602516     248.798640      1127.783455     1393.329528     1.177020        559.095648      -122.280172     663.266614      -284.008725     -1411.582432    0.000000        
+296.698921      1111596.955926  1111596.955926  5126.049004     1711524.062418  1711524.062418  1121309.558827  1121309.558827  2122440.593817  2122440.593817  1725419.018071  1725419.018071  2432383.209869  2432383.209869  1024122.336452  2673880.369984  2673880.369984  1845737.015282  1845737.015282  2878366.041242  2878366.041242  2287693.000146  2287693.000146  3049953.380640  3049953.380640  1482353.660475  3188497.912222  3188497.912222  2229222.979737  2229222.979737  3305572.873952  3305572.873952  2612927.563784  2612927.563784  3403916.191632  3403916.191632  1753858.573799  3484955.565597  3484955.565597  2457415.775389  2457415.775389  3562678.977792  3562678.977792  2811889.463107  2811889.463107  3634601.849246  3634601.849246  1933573.519582  3697605.496416  3697605.496416  2624334.404817  2624334.404817  3762379.511669  3762379.511669  2969426.090059  2969426.090059  3822569.851774  3822569.851774  3195245.108477  3195245.108477  3881298.776581  3881298.776581  3368029.173453  3368029.173453  3937734.089771  3937734.089771  3515391.167840  3515391.167840  2881400.524568  3633838.763944  3633838.763944  3446715.106174  3446715.106174  3742334.428186  3742334.428186  3694017.653500  3694017.653500  3843749.772586  3843749.772586  2733802.286928  3926154.773246  3926154.773246  3358856.453555  3358856.453555  4004479.104494  4004479.104494  3646943.706728  0.000000        
+9815876281.390493       10219639146.152300      10219639146.152300      9815391366.338209       10092396791.994455      10092396791.994455      9820918023.651031       9820918023.651031       10026942674.566353      10026942674.566353      9824054607.271921       9824054607.271921       9986540179.952711       9986540179.952711       9820749328.610571       9958801841.680529       9958801841.680529       9825149742.124517       9825149742.124517       9937856152.645214       9937856152.645214       9827762146.439236       9827762146.439236       9919827156.765738       9919827156.765738       9824274661.168266       9902186131.605909       9902186131.605909       9828584290.748890       9828584290.748890       9890460431.944111       9890460431.944111       9831101864.497566       9831101864.497566       9882225999.229254       9882225999.229254       9827389673.717871       9876391451.389120       9876391451.389120       9831433794.626633       9831433794.626633       9872465819.701366       9872465819.701366       9833630954.938072       9833630954.938072       9870024758.678486       9870024758.678486       9829700710.912729       9867664635.469746       9867664635.469746       9833696340.678921       9833696340.678921       9865370958.567533       9865370958.567533       9835815770.213318       9835815770.213318       9864069463.541668       9864069463.541668       9837362817.867367       9837362817.867367       9862759213.674345       9862759213.674345       9838621454.607281       9838621454.607281       9861780884.733627       9861780884.733627       9839784146.273586       9839784146.273586       9855374465.163700       9840749921.502296       9840749921.502296       9856998863.783821       9856998863.783821       9841720196.125462       9841720196.125462       9857185100.692745       9857185100.692745       9842629577.938219       9842629577.938219       9851296681.760395       9843405989.632635       9843405989.632635       9853618282.752998       9853618282.752998       9844190452.540911       9844190452.540911       9854355745.217056       0.000000        
diff --git a/SD-VBS/benchmarks/localization/data/vga/localization b/SD-VBS/benchmarks/localization/data/vga/localization
new file mode 100755
index 0000000..cf7cdfd
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/vga/localization
Binary files differ
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/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;
+}
diff --git a/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m b/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m
new file mode 100644
index 0000000..3d2ac76
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m
@@ -0,0 +1,12 @@
+function retQuat=addEulNoise(quat, STD)
+n=size(quat,1);
+% KVS: replacing randnWrapper(n,3) with rand(n,3)
+gyro=randnWrapper(n,3)*STD;
+norm_gyro=sqrt(sum(gyro.^2,2));
+angleAlpha=norm_gyro;
+quatDelta=[cos(angleAlpha/2), gyro./(norm_gyro*ones(1,3)).*(sin(angleAlpha/2)*ones(1,3))];
+retQuat=quatMul(quat, quatDelta);
diff --git a/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m b/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m
new file mode 100644
index 0000000..060713a
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m
@@ -0,0 +1,11 @@
+function [meanColor A]=calculate3DGaussian(data)
+n_data=size(data,1);
+n_channel=size(data,2);
+meanColor=mean(data);
+diff=double(data)-ones(n_data,1)*meanColor;
+diifTr = transpose(diff);
+Ainv=(diffTr*diff/n_data);
+AinvTr = transpose(Ainv);
+A=inv(AinvTr);
diff --git a/SD-VBS/benchmarks/localization/src/matlab/drawLog.m b/SD-VBS/benchmarks/localization/src/matlab/drawLog.m
new file mode 100644
index 0000000..f88e078
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/drawLog.m
@@ -0,0 +1,44 @@
+function drawLog(data, ground_data)
+subplot(1,3,1);
+[nr nc]=size(data);
+for i=1:nr
+    x=data(i,2);
+    y=data(i,3);
+    z=data(i,4);
+    R=[1 0 0; 0 cos(x) sin(x); 0 -sin(x) cos(x)]...
+        *[cos(y) 0 -sin(y); 0 1 0; sin(y) 0 cos(y)]...
+        *[cos(z) sin(z) 0; -sin(z) cos(z) 0; 0 0 1];
+    coord=R*[1 0 0]';
+    axis([-1 1 -1 1 -1 1]);axis on
+    plot3([0 coord(1)], [0 coord(2)], [0 coord(3)], 'b'); hold on
+end
+[nr nc]=size(ground_data);
+for i=1:nr
+x=ground_data(i,2);
+y=ground_data(i,3);
+z=ground_data(i,4);
+R=[1 0 0; 0 cos(x) sin(x); 0 -sin(x) cos(x)]...
+        *[cos(y) 0 -sin(y); 0 1 0; sin(y) 0 cos(y)]...
+        *[cos(z) sin(z) 0; -sin(z) cos(z) 0; 0 0 1];
+coord=R*[1 0 0]';
+coord2=R*[0 -1 0]';
+axis([-1 1 -1 1 -1 1]);axis on
+plot3([0 coord(1)], [0 coord(2)], [0 coord(3)], 'r'); hold on
+plot3([0 coord2(1)], [0 coord2(2)], [0 coord2(3)], 'g');
+end
+xlabel('x');
+ylabel('y');
+zlabel('z');
+hold off
+drawnow
+subplot(1,3,2);
+Xoffset=4422610;
+Yoffset=483660;
+axis([4422610-Xoffset 4422660-Xoffset 483620-Yoffset 483720-Yoffset]);axis on
+scatter(data(:,5)-Xoffset, data(:,6)-Yoffset, 8, 'b'); hold on
+axis([4422610-Xoffset 4422660-Xoffset 483620-Yoffset 483720-Yoffset]);axis on
+scatter(ground_data(:,5)-Xoffset, ground_data(:,6)-Yoffset, 10, 'r'); hold off
+drawnow
diff --git a/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m b/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m
new file mode 100644
index 0000000..57001a8
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m
@@ -0,0 +1,9 @@
+function retQuat=eul2quat(eulAngle)
+x=eulAngle(:,1);
+y=eulAngle(:,2);
+z=eulAngle(:,3);
+retQuat=[cos(x/2).*cos(y/2).*cos(z/2)+sin(x/2).*sin(y/2).*sin(z/2) ...
+        sin(x/2).*cos(y/2).*cos(z/2)-cos(x/2).*sin(y/2).*sin(z/2) ...
+        cos(x/2).*sin(y/2).*cos(z/2)+sin(x/2).*cos(y/2).*sin(z/2) ...
+        cos(x/2).*cos(y/2).*sin(z/2)-sin(x/2).*sin(y/2).*cos(z/2)];
+    
diff --git a/SD-VBS/benchmarks/localization/src/matlab/generateSample.m b/SD-VBS/benchmarks/localization/src/matlab/generateSample.m
new file mode 100644
index 0000000..f3658f8
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/generateSample.m
@@ -0,0 +1,25 @@
+function [retQuat retVel retPos]=generateSample(w, quat, vel, pos, STDDEV_VEL, STDDEV_POS)
+sampledXId=weightedSample(w);
+rows = size(sampledXId, 1);
+cols = size(sampledXId, 2);
+if(cols > 1)
+    disp(123456);
+end
+% retQuat = zeros(rows, 1);
+% retVel = zeros(rows, 1);
+% retPos = zeros(rows, 1);
+% for i=1:rows
+%     retQuat(i,1) = quat(sampleXId(i,1),1);
+%     retVel(i,1) = vel(sampleXId(i,1),1) + randnWrapper(1,1) * STDDEV_VEL;
+%     retPos(i,1) = pos(sampleXId(i,1),1) + randnWrapper(1,1) * STDDEV_POS;    
+% end
+retQuat=quat(sampledXId,:);
+retVel=vel(sampledXId,:);%+randnWrappern(n,3)*STDDEV_VEL;
+retPos=pos(sampledXId,:);%+randnWrappern(n,3)*STDDEV_POS;
diff --git a/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m b/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m
new file mode 100644
index 0000000..6a54530
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m
@@ -0,0 +1,22 @@
+function p=get3DGaussianProb(data, mean, A)
+n_data=size(data,1);
+n_channel=size(data,2);
+p=zeros(n_data,1);
+diff=(data)-ones(n_data,1)*mean;
+detA = 1; %detA = det(A)
+dotA = randWrapper(size(diff,1),1); %dotA = dot(diff*A, diff, 2)
+p=sqrt(detA/((2*pi)^n_channel))*exp(-0.5*dotA);
+%% KVS If the above doesnt work, try uncommenting these lines below
+%%temp = (det(A)/((2*pi)^n_channel));
+%temp = (1.0/((2*pi)^n_channel));
+%temp1 = dot(diff*A,diff,2);
+%%temp1 = rand(1000,1); 
+%temp2 = exp(-0.5*temp1);
+%p = sqrt(temp) * exp(temp2);
+%
diff --git a/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m b/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m
new file mode 100644
index 0000000..7cf5f9c
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m
@@ -0,0 +1,5 @@
+function retData=getGroundData(data, tStamp)
+idx=find(data(:,1)==tStamp);
+retData=data(idx,:);
diff --git a/SD-VBS/benchmarks/localization/src/matlab/mcl.m b/SD-VBS/benchmarks/localization/src/matlab/mcl.m
new file mode 100644
index 0000000..8ab8aa6
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/mcl.m
@@ -0,0 +1,9 @@
+function [retX, retW]=mcl(x,sData, invCov)
+%instead of using importance resampling, I assumed 3D gaussian for each
+%particle
+%retX=x+randn(nr,nc)*(invCov^-1); %add noise
+retX=x;
+retW=get3DGaussianProb(retX, sData, invCov);
+retW=retW/sum(retW);
diff --git a/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m b/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m
new file mode 100644
index 0000000..4e78b8f
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m
@@ -0,0 +1,18 @@
+function retW=mclWhole(quat, Ovel, STDDEV_GPSVel, accl, STDDEV_ACCL)
+n=size(quat,1);
+OvelNorm=norm(Ovel);
+if (OvelNorm>0.5)
+    Ovel=Ovel/norm(Ovel);
+    %trick
+    %quat=addEulNoise(quat, STDDEV_GPSVel); 
+    orgWorld=quatRot([1 0 0],quatConj(quat));
+    p1=get3DGaussianProb(orgWorld, Ovel, eye(3)./STDDEV_GPSVel);
+else
+    p1=zeros(n,1);
+end
+gravity=quatRot(ones(n,1)*[0 0 -9.8], quat);
+p2=get3DGaussianProb(gravity, accl, eye(3)./(STDDEV_ACCL));
+retW=p1+p2;
+retW=retW/sum(retW);
+            
+\ No newline at end of file
diff --git a/SD-VBS/benchmarks/localization/src/matlab/quat2eul.m b/SD-VBS/benchmarks/localization/src/matlab/quat2eul.m
new file mode 100644
index 0000000..a6b48ee
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/quat2eul.m
@@ -0,0 +1,6 @@
+function retEul=quat2eul(quat)
+retEul=[atan2(2*(quat(:,3).*quat(:,4)+quat(:,1).*quat(:,2))...
+               ,(quat(:,1).^2 -quat(:,2).^2 -quat(:,3).^2 +quat(:,4).^2)) ...
+        asin(-2*(quat(:,2).*quat(:,4)-quat(:,1).*quat(:,3))) ...
+        atan2(2*(quat(:,2).*quat(:,3)+quat(:,1).*quat(:,4))...
+               ,(quat(:,1).^2 +quat(:,2).^2 -quat(:,3).^2 -quat(:,4).^2))];
diff --git a/SD-VBS/benchmarks/localization/src/matlab/quatConj.m b/SD-VBS/benchmarks/localization/src/matlab/quatConj.m
new file mode 100644
index 0000000..ff494b4
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/quatConj.m
@@ -0,0 +1,5 @@
+function retQuat=quatConj(a)
+rows = size(a,1);
+retQuat = zeros(rows, 4);
+retQuat=[a(:,1), -a(:,2), -a(:,3), -a(:,4)];
+         
diff --git a/SD-VBS/benchmarks/localization/src/matlab/quatMul.m b/SD-VBS/benchmarks/localization/src/matlab/quatMul.m
new file mode 100644
index 0000000..b397554
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/quatMul.m
@@ -0,0 +1,18 @@
+function retQuat=quatMul(a, b)
+rowsa = size(a,1);
+colsa = size(a,2);
+rowsb = size(b,1);
+colsb = size(b,2);
+retQuat=[a(:,1).*b(:,1) - a(:,2).*b(:,2) - a(:,3).*b(:,3) - a(:,4).*b(:,4) ...
+    a(:,1).*b(:,2) + a(:,2).*b(:,1) + a(:,3).*b(:,4) - a(:,4).*b(:,3) ...
+    a(:,1).*b(:,3) - a(:,2).*b(:,4) + a(:,3).*b(:,1) + a(:,4).*b(:,2) ...
+    a(:,1).*b(:,4) + a(:,2).*b(:,3) - a(:,3).*b(:,2) + a(:,4).*b(:,1)];
+% disp('retQuat');
+% disp(retQuat);
diff --git a/SD-VBS/benchmarks/localization/src/matlab/quatRot.m b/SD-VBS/benchmarks/localization/src/matlab/quatRot.m
new file mode 100644
index 0000000..3688172
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/quatRot.m
@@ -0,0 +1,24 @@
+function ret=quatRot(vec, rQuat)
+nr=size(vec,1);
+tv = zeros(nr,1);
+vQuat=[tv, vec];
+temp = quatMul(rQuat, vQuat);
+temp1 = quatConj(rQuat);
+retVec = quatMul(temp, temp1);
+%retVec=quatMul(quatMul(rQuat, vQuat),quatConj(rQuat));
+%ret=retVec(:,2:4);
+rows = size(retVec, 1);
+ret = zeros(rows,3);
+for i=1:rows
+    k =1;
+    for j=2:4
+        ret(i,k) = retVec(i,j);
+        k = k+1;
+    end
+end
+%size(ret)
diff --git a/SD-VBS/benchmarks/localization/src/matlab/readLoc.m b/SD-VBS/benchmarks/localization/src/matlab/readLoc.m
new file mode 100644
index 0000000..fc679d4
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/readLoc.m
@@ -0,0 +1,53 @@
+function out = readLoc
+out = [...
+10.512346 3.466955 9.006616 ;...
+13.242645 4.280358 14.675757 ;...
+12.418589 6.286052 22.721924 ;...
+13.598760 5.229711 19.622804 ;...
+13.568058 5.229711 19.622804 ;...
+13.537888 5.229711 19.622804 ;...
+13.508315 5.229711 19.622804 ;...
+13.479378 5.229711 19.622804 ;...
+13.450420 5.229711 19.622804 ;...
+13.345976 7.607621 29.644342 ;...
+13.500197 7.803792 32.248338 ;...
+13.354279 10.248983 42.939018 ;...
+13.500197 10.442337 45.593042 ;...
+13.354279 12.925516 56.404386 ;...
+13.700372 10.538259 47.007521 ;...
+13.669945 10.538259 47.007521 ;...
+13.640058 10.538259 47.007521 ;...
+13.608179 10.538259 47.007521 ;...
+13.576901 10.538259 47.007521 ;...
+13.545771 10.538259 47.007521 ;...
+13.731515 12.682360 55.887526 ;...
+1.016192 12.058634 55.442021 ;...
+6.263918 15.094210 64.436498 ;...
+6.263918 18.208480 70.198939 ;...
+6.263918 20.848498 78.618094 ;...
+6.263918 23.887747 84.333007 ;...
+6.263918 22.065907 78.853015 ;...
+6.237774 22.065907 78.853015 ;...
+6.211166 22.065907 78.853015 ;...
+6.183471 22.065907 78.853015 ;...
+6.155114 22.065907 78.853015 ;...
+6.126730 22.065907 78.853015 ;...
+6.154098 26.480108 89.543387 ;...
+6.168298 28.571946 93.679254 ;...
+6.157243 32.868572 105.963003 ;...
+6.155032 35.245287 110.401650 ;...
+6.155032 38.531472 119.557291 ;...
+6.155032 36.849296 111.296948 ;...
+6.128369 36.849296 111.296948 ;...
+6.101609 36.849296 111.296948 ;...
+6.073750 36.849296 111.296948 ;...
+6.046859 36.849296 111.296948 ;...
+6.019815 36.849296 111.296948 ;...
+6.046273 40.863016 123.894104 ;...
+6.060599 43.054591 127.680881 ;...
+6.049203 47.179478 140.858302 ;...
+6.046924 49.530293 145.484328 ;...
+6.046924 52.526113 153.189673 ;...
+6.046924 51.113294 146.241709 ;...
+6.021072 51.113294 146.241709 ...
+];
+\ No newline at end of file
diff --git a/SD-VBS/benchmarks/localization/src/matlab/readMatrix.m b/SD-VBS/benchmarks/localization/src/matlab/readMatrix.m
new file mode 100644
index 0000000..fe684c5
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/readMatrix.m
@@ -0,0 +1,28 @@
+function readMatrix(srcImage, outName)
+write = fopen([outName '.m'], 'w');
+count = fwrite(write, 'function out = ');
+count = fwrite(write, outName);
+fprintf(write, '\n');
+count = fwrite(write, 'out = [...');
+fprintf(write, '\n');    
+height = size(srcImage,1);
+width = size(srcImage,2);
+for nI=1:height
+    for nJ=1:width
+        fprintf(write, '%f ', srcImage(nI,nJ));
+    end
+    if(nI < height)
+        fprintf(write, ';...\n');
+    end
+end
+fprintf(write, '...\n');
+count = fwrite(write, '];');
+fclose(write);
+end
diff --git a/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m b/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m
new file mode 100644
index 0000000..e84a6db
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m
@@ -0,0 +1,46 @@
+function [retTStamp, retType, retData, retEOF, index]=readSensorData(index1, fid)
+index = index1+1;
+rows = size(fid, 1);
+retTStamp = 0;
+retType = 0;
+retData = zeros(1,8);
+if(index > rows)
+    retEOF = 1;
+else
+%     for i=index:rows
+%         index = i;
+%         if(fid(i,2) == 4)
+%             break;
+%         end
+%     end
+    if(index == rows)
+        retEOF = 1;
+    else
+        retEOF = 0;
+    end
+    k = index;
+    retTStamp=fid(k,1);
+    retType=fid(k,2);
+    if(fid(k, 2) == 1 || fid(k, 2) == 2 || fid(k, 2) == 3)
+        index = k;
+        for i=1:3
+            retData(1,i)=fid(k,i+2);
+%            fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i);
+        end
+    end
+    if(fid(k, 2) == 4)
+        index = k;
+        for i=1:3
+            retData(1,i)=fid(k,i+2);
+%            fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i);
+        end
+        for i=4:8
+            retData(1,i) = fid(k+1,i-3);
+%            fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i);
+        end
+        index = index + 1;        
+    end    
+end
diff --git a/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m b/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m
new file mode 100644
index 0000000..44ce3aa
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m
@@ -0,0 +1,209 @@
+function script_run_profile(dataDir, resultDir, type, common, toolDir)
+path(path, common);
+file = fopen([dataDir, '/1.txt'], 'r');
+full = fscanf(file,'%f');
+elapsed = zeros(1,2);
+rows = full(2);
+cols = full(1);
+fid = zeros(rows, cols);
+k = 3;
+for i=1:rows
+    for j =1:cols
+        fid(i,j) = full(k);
+        k = k+1;
+    end
+end
+fclose(file);
+n=1000;
+gyroTimeInterval=0.01;
+acclTimeInterval=0.01;
+STDDEV_GPSVel=0.5;
+STDDEV_ODOVel=0.1;
+STDDEV_ACCL=1;
+M_STDDEV_GYRO=0.1;
+M_STDDEV_POS=0.1;
+M_STDDEV_VEL=0.02;
+if(strcmp(type,'test'))
+        n = 3;
+        gyroTimeInterval = 0.1;
+        acclTimeInterval = 0.1;
+        M_STDDEV_VEL = 0.2;
+    
+elseif(strcmp(type, 'sim_fast'))
+        n = 3;
+elseif(strcmp(type, 'sim'))
+        n = 10;
+elseif(strcmp(type, 'sqcif'))
+        n = 800;
+elseif(strcmp(type, 'qcif'))
+        n = 500;
+elseif(strcmp(type, 'vga'))
+        n = 2000;
+elseif(strcmp(type, 'wuxga'))
+        n = 3000;
+end
+fprintf(1,'Input size\t\t- (%dx%dx%d)\n', rows, cols,n);
+pos=zeros(n,3);
+vel=zeros(n,3) + randWrapper(n,3)*STDDEV_ODOVel;
+pi = 3.1416;
+eul1 = eul2quat([zeros(n,2), randWrapper(n,1)*2*pi]);
+eul2 = eul2quat([pi, 0, 0]);
+quat=quatMul(eul1, eul2);
+i=0;
+index = 0;
+resultMat = zeros(3,rows);
+while 1
+    i=i+1;
+    [tStamp, sType, sData, isEOF, index] = readSensorData(index, fid);
+    start = photonStartTiming;
+    if(sType==2)
+        %Motion model
+        gyro = sData(1, 1:3);
+        randnWrapper_mat = randnWrapper(n,3);    % KVS: We do not have function implementation for randnWrapper()
+        gyro=ones(n,1)*gyro+randnWrapper_mat*M_STDDEV_GYRO;
+        abc = gyro.^2;
+        abcd = sumCol(abc);
+        norm_gyro = sqrt(abcd);
+%        norm_gyro=sqrt(sum(gyro.^2,2));
+        angleAlpha=norm_gyro*gyroTimeInterval;
+        quatDelta=[cos(angleAlpha/2), gyro./(norm_gyro*ones(1,3)+0.00000001).*(sin(angleAlpha/2)*ones(1,3))];
+        quat=quatMul(quat, quatDelta);
+    end
+    if(sType==4)
+        %Observation
+        STDDEV_GPSPos=[sData(1,7), 0, 0; 0, sData(1,8), 0; 0, 0, 15];
+        Opos=sData(1,1:3);
+        %Initialize
+        
+        randnWrapper_mat = randnWrapper(n,3);
+        if sum(sum(pos))==0
+            pos=ones(n,1) * Opos + randnWrapper_mat*STDDEV_GPSPos;
+        else 
+           rows = size(STDDEV_GPSPos, 1);
+           cols = size(STDDEV_GPSPos, 2);
+            temp_STDDEV_GPSPos = ones(rows,cols);
+            for mnrows=1:rows                                 % KVS" Photon rejects this loop becasue of too many nestings ??
+                for mncols=1:cols
+                    temp_STDDEV_GPSPos(mnrows, mncols) = power(STDDEV_GPSPos(mnrows,mncols),-1);
+                end
+            end
+           [temp, w]=mcl(pos, Opos , temp_STDDEV_GPSPos);
+           [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
+       end
+       %compare direction
+       Ovel=sData(1,4:6);
+%        KVS: We do not have function for norm() yet! So replacing this operating with OvelNorm
+  
+%        OvelNorm=norm(Ovel);                               
+       OvelNorm= 2.0;  %1.1169e+09;
+       
+       if (OvelNorm>0.5)
+       
+           % KVS: Similar here: No impln of norm(), so replacing
+           % norm(Ovel) with OvelNorm value
+           
+           Ovel=Ovel/OvelNorm;
+           %trick
+           %quat=addEulNoise(quat, STDDEV_GPSVel);
+           qConj = quatConj(quat);
+            orgWorld=quatRot([1, 0, 0],qConj);
+            eye3 = [1,0,0;0,1,0;0,0,1];
+            [temp, w]=mcl(orgWorld, Ovel, eye3./STDDEV_GPSVel);
+            [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
+       end
+ 
+   end
+   
+   if(sType==1)
+       %Observation
+       Ovel=sData(1,1);
+       [temp, w]=mcl(sqrt(vel(:,1).^2+vel(:,2).^2+vel(:,3).^2), Ovel, 1/(STDDEV_ODOVel));
+       [quat vel pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
+   end
+   
+   if(sType==3)
+       %Observation
+       accl=sData(1,1:3);
+       
+       gtemp = ones(n,1) * [0 0 -9.8];
+       
+       gravity=quatRot(gtemp, quat);
+        eye3 = [1,0,0;0,1,0;0,0,1];
+        [gravity, w]=mcl(gravity, accl, eye3./(STDDEV_ACCL));
+        [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS);
+        %Motion model
+        accl=ones(n,1)*accl;
+        accl=accl-gravity;
+        pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval ...
+            +1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2 ...
+            +randnWrapper(n,3)*M_STDDEV_POS;
+        vel=vel+accl*acclTimeInterval+randnWrapper(n,3)*M_STDDEV_VEL;
+   end
+   
+    stop = photonEndTiming;
+    
+    temp = photonReportTiming(start, stop);
+    elapsed(1) = elapsed(1) + temp(1);
+    elapsed(2) = elapsed(2) + temp(2);
+    % Self check
+    quatOut = 0;
+    posOut = 0;
+    velOut = 0;
+    for ij=1:(size(quat,1)*size(quat,2))
+        quatOut = quatOut + quat(ij);
+    end
+    for ij=1:(size(vel,1)*size(vel,2))
+        velOut = velOut + vel(ij);
+    end
+    for ij=1:(size(pos,1)*size(pos,2))
+        posOut = posOut + pos(ij);
+    end
+    resultMat(:, i) = [quatOut, velOut, posOut];
+    if (isEOF == 1)
+        break;
+    end
+end
+%% Timing
+photonPrintTiming(elapsed);
+%% Self checking %%
+fWriteMatrix(resultMat,dataDir);
diff --git a/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m b/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m
new file mode 100644
index 0000000..ac80ebd
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m
@@ -0,0 +1,33 @@
+function ret = selfCheck(in1, in2, tol)
+r1 = size(in1, 1);
+c1 = size(in1, 2);
+r2 = size(in2, 1);
+c2 = size(in2, 2);
+ret = 1;
+if r1~=r2 
+    disp(1298);
+    ret = 0;
+end
+if c1 ~= c2
+        disp(1297);
+        ret = 0;
+end
+for i=1:r1
+    if(ret == 0)
+        break;
+    end
+    for j=1:c1
+        if( abs(in1(i,j)-in2(i,j)) > tol)
+            ret = 0;
+            break;
+        end
+    end
+end
diff --git a/SD-VBS/benchmarks/localization/src/matlab/sumCol.m b/SD-VBS/benchmarks/localization/src/matlab/sumCol.m
new file mode 100644
index 0000000..ad0114e
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/sumCol.m
@@ -0,0 +1,14 @@
+function ret = sumCol(mat)
+row = size(mat, 1);
+col = size(mat, 2);
+ret = zeros(row, 1);
+for i=1:row
+        temp = 0;
+        for j=1:col
+                temp = temp + mat(i, j);
+        end
+        ret(i, 1) = temp;
+end
diff --git a/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m b/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m
new file mode 100644
index 0000000..dc9274b
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m
@@ -0,0 +1,20 @@
+%function x_gen_id=weightedSample(w)
+function bin=weightedSample(w)
+n=size(w,1);
+seed=randWrapper(n,1);
+bin = zeros(n,1);
+%x_gen_id=zeros(n,1);
+ for i=1:n
+     for j=1:n
+         if(seed(j,1) > 0)
+             bin(j,1) = bin(j,1) + 1;
+%             x_gen_id(j,1) = x_gen_id(j,1) + bin(j,1);
+         end
+     end
+%     bin = (seed>0);
+%     x_gen_id=x_gen_id+bin;
+     seed=seed-w(i,1);
+ end
+% x_gen_id = bin;
+%x_gen=x(:,x_gen_id);