diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..0772b29
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,3 @@
+[submodule "tracker_CLASS"]
+	path = tracker_CLASS
+	url = https://github.com/mive93/tracker_CLASS.git
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 727a606..2730de0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,7 +39,8 @@ file(GLOB tkdnn_SRC "src/*.cpp")
 set(tkdnn_LIBS kernels ${CUDA_LIBRARIES} ${CUDA_CUBLAS_LIBRARIES} -lcudnn -lnvinfer ${OpenCV_LIBS} -lgdal)
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11")
-include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${CUDA_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} ${NVINFER_INCLUDES} "~/repos/cereal/include")
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${CUDA_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} ${NVINFER_INCLUDES} "~/repos/cereal/include" ${CMAKE_CURRENT_SOURCE_DIR}/tracker_CLASS/c++/src)
+include_directories( BEFORE ${MY_SOURCE_DIR}/src /usr/include/python2.7 )
 add_library(tkDNN SHARED ${tkdnn_SRC})
 target_link_libraries(tkDNN ${tkdnn_LIBS})
 
@@ -93,8 +94,15 @@ target_link_libraries(test_yolo3_tetrapack_resize tkDNN)
 add_executable(test_rtinference tests/test_rtinference/rtinference.cpp)
 target_link_libraries(test_rtinference tkDNN)
 
-add_executable(yolo3_demo demo/demo/demo.cpp)
+add_executable(yolo3_demo demo/demo/demo.cpp 
+                        tracker_CLASS/c++/src/ekf.cpp
+                        tracker_CLASS/c++/src/trackutils.cpp
+                        tracker_CLASS/c++/src/plot.cpp
+                        tracker_CLASS/c++/src/tracker.cpp )
+
+                        
 target_link_libraries(yolo3_demo tkDNN)
+target_link_libraries(yolo3_demo python2.7)
 
 add_executable(class_server demo/server/server_less_dummy.cpp)
 target_link_libraries(class_server pthread tkDNN )
diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp
index f834a86..164b9ef 100644
--- a/demo/demo/demo.cpp
+++ b/demo/demo/demo.cpp
@@ -11,6 +11,10 @@
 
 #include "Yolo3Detection.h"
 #include "send.h"
+#include "ekf.h"
+#include "trackutils.h"
+#include "plot.h"
+#include "tracker.h"
 
 
 #define MAX_DETECT_SIZE 100
@@ -79,8 +83,23 @@ int main(int argc, char *argv[]) {
     /*socket*/
     int sock;
     int socket_opened = 0;
-    
+
+    /*Conversion for tracker, from gps to meters and viceversa*/
+    geodetic_converter::GeodeticConverter gc;
+    gc.initialiseReference(44.655540,10.934315, 0);
+    double east, north, up;
+    double lat, lon, alt;
+
+    /*tracker infos*/
+    std::vector<Tracker> trackers;
+    std::vector<Data> cur_frame;
+    int initial_age = -5;
+    int age_threshold = -10;
+    int n_states = 5;
+    float dt = 0.03;
+
     struct obj_coords *coords = (struct obj_coords*)malloc(MAX_DETECT_SIZE*sizeof(struct obj_coords));
+    
 
     int frame_nbr = 0;
     while(gRun) {
@@ -115,6 +134,9 @@ int main(int argc, char *argv[]) {
 
         // draw dets
         for(int i=0; i<num_detected; i++) {
+
+            
+
             tk::dnn::box b = yolo.detected[i];
             int x0   = b.x;
             int x1   = b.x + b.w;
@@ -132,10 +154,44 @@ int main(int argc, char *argv[]) {
 
             float prob = b.prob;
 
-            std::cout<<obj_class<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
+            //std::cout<<obj_class<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
             cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[obj_class], 2);                
         }
 
+
+        cur_frame.clear();
+        for(int i=0; i<coord_i; i++)
+        {
+            std::cout<<"lat orig: "<<coords[i].LAT<<" lon orig: "<<coords[i].LONG<<std::endl;
+            gc.geodetic2Enu(coords[i].LAT, coords[i].LONG, 0, &east, &north, &up);
+            std::cout<<"east: "<<east<<" north: "<<north<<std::endl;
+            cur_frame.push_back(Data(east,north, frame_nbr));
+        }
+
+        if(frame_nbr == 0)
+        {
+            for(auto f:cur_frame)
+                trackers.push_back(Tracker(f, initial_age, dt, n_states));
+        }
+        else
+        {
+            Track(cur_frame, dt, n_states, initial_age, age_threshold, trackers);
+        }
+
+        std::cout<<"There are "<<trackers.size()<<" trackers"<<std::endl;
+
+        for(auto t:trackers)
+        {
+            State s = t.ekf_.getEstimatedState();
+            gc.enu2Geodetic(s.x_, s.y_, 0, &lat, &lon, &alt);
+            std::cout<<"lat: "<<lat<<" lon: "<<lon<<std::endl;
+            int pix_x, pix_y;
+            coord2pixel(lat, lon, pix_x, pix_y,  adfGeoTransform);
+            std::cout<<"pix_x: "<<pix_x<<" pix_y: "<<pix_y<<std::endl;
+            //if(t.age_ > 0)
+                cv::circle( frame, cv::Point( pix_x, pix_y ), 10.0, cv::Scalar( 255, 0, 0 ), CV_FILLED, 8, 0);
+        }
+
         frame_nbr++;
 
         send_client_dummy(coords, coord_i, sock, socket_opened, CAM_IDX);
@@ -147,6 +203,11 @@ int main(int argc, char *argv[]) {
        } 
     }
 
+
+/*     for (size_t i = 0; i < trackers.size(); i++)
+        if (trackers[i].z_list_.size() > 10)
+            plotTruthvsPred(trackers[i].z_list_, trackers[i].pred_list_); */
+
     free(coords);
     free(adfGeoTransform);
 
diff --git a/include/send.h b/include/send.h
index 8c1d6e9..b1a2f51 100644
--- a/include/send.h
+++ b/include/send.h
@@ -18,12 +18,13 @@
 #include "gdal/cpl_conv.h"
 
 
+
 #include "serialize.hpp"
 
 struct obj_coords
 {
-    float LAT;
-    float LONG;
+    double LAT;
+    double LONG;
     float cl;
 };
 
@@ -43,7 +44,7 @@ void readTiff(char*filename, double *adfGeoTransform)
 
 void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform)
 {
-	
+	//Returns global coordinates from pixel x, y coordinates
 	double xoff, a, b, yoff, d, e;
 	xoff = adfGeoTransform[0];
 	a = adfGeoTransform[1];
@@ -58,6 +59,12 @@ void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform
     lat = d * x + e * y + yoff;
 
 }
+void coord2pixel(double lat, double lon, int &x, int &y,  double *adfGeoTransform)
+{
+    x = int(round((lon-adfGeoTransform[0])/adfGeoTransform[1]));
+    y = int(round((lat-adfGeoTransform[3])/adfGeoTransform[5]));
+}
+
 
 void fillMatrix(cv::Mat &H, float *matrix, bool show=false)
 {
@@ -156,7 +163,7 @@ void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected
     cv::perspectiveTransform( x_y, ll, H);
     //tranform to map pixel to map gps
     pixel2coord(ll[0].x, ll[0].y, latitude,longitude, adfGeoTransform);
-    printf("lat: %f, long:%f \n", latitude, longitude);
+    //printf("lat: %f, long:%f \n", latitude, longitude);
     coords[i].LAT = latitude;
     coords[i].LONG = longitude;
     coords[i].cl = map_class_coco_to_voc(detected_class);
@@ -237,7 +244,7 @@ int send_client_dummy(struct obj_coords *coords, int n_coords, int &sock, int &s
     serialize_coords(coords, n_coords, CAM_IDX, message);
 
 
-    std::cout<<message->str().length()<<std::endl;
+    //std::cout<<message->str().length()<<std::endl;
 
     /*open socket if not already opened*/
     if (socket_opened == 0)
diff --git a/tracker_CLASS b/tracker_CLASS
new file mode 160000
index 0000000..135e47b
--- /dev/null
+++ b/tracker_CLASS
@@ -0,0 +1 @@
+Subproject commit 135e47ba2e2202ad02fd95f2d16bb9ea9abfc29b