diff --git a/modules/catkin_ws/src/car/include/environment/Environment.h b/modules/catkin_ws/src/car/include/environment/Environment.h
index 555eecbd66cf8822ed83f54d120bac7f7cbbd859..97ac2c16e809b764eb892b7279d7de84b47fce6f 100644
--- a/modules/catkin_ws/src/car/include/environment/Environment.h
+++ b/modules/catkin_ws/src/car/include/environment/Environment.h
@@ -3,6 +3,8 @@
 
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
+
+#include "KalmanFilter.h"
 #include <boost/numeric/ublas/vector.hpp>
 #include <boost/numeric/ublas/matrix.hpp>
 
@@ -27,8 +29,8 @@ private:
     ros::NodeHandle nh_;
     std::string name_;
 
-    float distance;
-    float relativeSpeed;
+    double dt;
+    KalmanFilter kalmanFilter;
 
     ros::Publisher environmentData;
     ros::Subscriber ussData;
@@ -37,6 +39,9 @@ private:
     void ussDataCallback(const ussDataMsg::ConstPtr & inMsg);
     void camDataCallback(const camDataMsg::ConstPtr & inMsg);
 
+    matrix<double> buildDynamicModelMatrix(double *deltaT);
+    std::string buildPath();
+
 
 };
 }
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index 3fe8de926f5eecbed5832e2115314f4f3ae875a0..698edf4a508b26860db135089e1d9fcd37e12816 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/Environment.cpp
@@ -12,16 +12,16 @@ PLUGINLIB_EXPORT_CLASS(car::Environment, nodelet::Nodelet);
 
 namespace car
 {
-Environment::Environment(ros::NodeHandle &nh, std::string &name) :
-    nh_(nh)
+Environment::Environment(ros::NodeHandle &nh, std::string &name)
+    : nh_(nh)
     , name_(name)
-    , distance(0)
-    , relativeSpeed(0)
+    , dt()
+    , kalmanFilter(2, buildDynamicModelMatrix(&dt), &dt, buildPath())
     {}
   
-Environment::Environment() :
-    distance(0)
-    , relativeSpeed(0)
+Environment::Environment() 
+    : dt()
+    , kalmanFilter(2, buildDynamicModelMatrix(&dt), &dt, buildPath())
     {}
   
 Environment::~Environment() {}
@@ -38,18 +38,36 @@ void Environment::onInit()
 void Environment::ussDataCallback(const ussDataMsg::ConstPtr& inMsg)
 {
     // std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
-    distance = inMsg->distance;
-    relativeSpeed = 1.0;
+    // distance = inMsg->distance;
+    // relativeSpeed = 1.0;
     
-    environmentDataMsg outMsg;
-    outMsg.distance = distance;
-    outMsg.relativeSpeed = relativeSpeed;
+    // environmentDataMsg outMsg;
+    // outMsg.distance = distance;
+    // outMsg.relativeSpeed = relativeSpeed;
     
-    environmentData.publish(outMsg);
+    // environmentData.publish(outMsg);
 }
 
 void Environment::camDataCallback(const camDataMsg::ConstPtr& inMsg)
 {
     // std::cout << "Environment recived new cam data ( ).\n";
 }
+
+std::string Environment::buildPath()
+{
+    std::string userHome = getenv("HOME");
+    return userHome + "/CarConfig/kalman.config";
+}
+
+matrix<double> Environment::buildDynamicModelMatrix(double *deltaT)
+{
+    /* TAKE CARE ON THE DIMENSION */
+    *deltaT = 1.0;
+    matrix<double> dynamicModel(2,2);
+    dynamicModel(0,0) = 1.0,
+    dynamicModel(0,1) = 1.0/(*deltaT);
+    dynamicModel(1,0) = 0;
+    dynamicModel(1,1) = 1.0;
+    return dynamicModel;
+}
 }
diff --git a/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp b/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp
index 86c51897263411513c3a0830b313f00b07ea93a8..e5f6d55fdfc257bcf3a6a773ad6832f0be6e523f 100644
--- a/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp
+++ b/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp
@@ -2,6 +2,8 @@
 #include "exceptions/Exceptions.h"
 #include <iterator>
 
+#include <stdio.h>
+
 namespace car
 {
 KalmanFilter::KalmanFilter(vector<double> x, matrix<double> P, matrix<double> F, matrix<double> Q, vector<double> H, vector<double> R, double *deltaT)
@@ -22,7 +24,7 @@ KalmanFilter::KalmanFilter(int dimension, matrix<double> F, double *deltaT, std:
     , H(dimension)
     , R(dimension)
     , dt(deltaT)
-    {
+    {   
         readConfigFile(configFilePath);
     }
 
@@ -43,7 +45,6 @@ std::vector<std::string> KalmanFilter::getNextFileLine(std::ifstream *file)
 void KalmanFilter::readConfigFile(std::string configFilePath)
 {
     // open config file
-    std::string userHome = getenv("HOME");
     std::ifstream configFile;
     configFile.open(configFilePath, std::ifstream::in);
     if (!configFile.is_open()) { throw FileNotFound(); }
@@ -57,43 +58,52 @@ void KalmanFilter::readConfigFile(std::string configFilePath)
             if ( words.at(0) == "//") { continue; }
             int rows = std::stoi(words.at(1));
             int cols = std::stoi(words.at(2));
-            int i = 0;
-            int j = 0;
             if (words.at(0) == "x") {
+                int i = 0;
                 while ( i < rows )
                 {
                     words = getNextFileLine(&configFile);
-                    x(i) = std::stod(words.at(0));
+                    x(i++) = std::stod(words.at(0));
                 }
             } else if (words.at(0) == "P"){
+                int i = 0;
                 while ( i < rows )
                 {
                     words = getNextFileLine(&configFile);
+                    int j = 0;
                     while ( j < cols )
                     {
                         P(i, j) = std::stod(words.at(j));
+                        j++;
                     }
+                    i++;
                 }
             } else if (words.at(0) == "Q"){
+                int i = 0;
                 while ( i < rows )
                 {
                     words = getNextFileLine(&configFile);
+                    int j = 0;
                     while ( j < cols )
                     {
                         Q(i, j) = std::stod(words.at(j));
+                        j++;
                     }
+                    i++;
                 }
             } else if (words.at(0) == "H"){
+                int i = 0;
                 while ( i < rows )
                 {
                     words = getNextFileLine(&configFile);
-                    H(i) = std::stod(words.at(0));
+                    H(i++) = std::stod(words.at(0));
                 }
             } else if (words.at(0) == "R"){
+                int i = 0;
                 while ( i < rows )
                 {
                     words = getNextFileLine(&configFile);
-                    x(i) = std::stod(words.at(j));
+                    x(i++) = std::stod(words.at(0));
                 }
             }       
         }