diff --git a/svo/CMakeLists.txt b/svo/CMakeLists.txt
index 19beebd..ff39094 100644
--- a/svo/CMakeLists.txt
+++ b/svo/CMakeLists.txt
@@ -134,23 +134,23 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON)
 ################################################################################
 # TESTS
 
-ADD_EXECUTABLE(test_feature_align test/test_feature_alignment.cpp)
-TARGET_LINK_LIBRARIES(test_feature_align svo)
+#ADD_EXECUTABLE(test_feature_align test/test_feature_alignment.cpp)
+#TARGET_LINK_LIBRARIES(test_feature_align svo)
 
-ADD_EXECUTABLE(test_pipeline test/test_pipeline.cpp)
-TARGET_LINK_LIBRARIES(test_pipeline svo)
+#ADD_EXECUTABLE(test_pipeline test/test_pipeline.cpp)
+#TARGET_LINK_LIBRARIES(test_pipeline svo)
 
-ADD_EXECUTABLE(test_matcher test/test_matcher.cpp)
-TARGET_LINK_LIBRARIES(test_matcher svo)
+#ADD_EXECUTABLE(test_matcher test/test_matcher.cpp)
+#TARGET_LINK_LIBRARIES(test_matcher svo)
 
-ADD_EXECUTABLE(test_feature_detection test/test_feature_detection.cpp)
-TARGET_LINK_LIBRARIES(test_feature_detection svo)
+#ADD_EXECUTABLE(test_feature_detection test/test_feature_detection.cpp)
+#TARGET_LINK_LIBRARIES(test_feature_detection svo)
 
-ADD_EXECUTABLE(test_depth_filter test/test_depth_filter.cpp)
-TARGET_LINK_LIBRARIES(test_depth_filter svo)
+#ADD_EXECUTABLE(test_depth_filter test/test_depth_filter.cpp)
+#TARGET_LINK_LIBRARIES(test_depth_filter svo)
 
-ADD_EXECUTABLE(test_sparse_img_align test/test_sparse_img_align.cpp)
-TARGET_LINK_LIBRARIES(test_sparse_img_align svo)
+#ADD_EXECUTABLE(test_sparse_img_align test/test_sparse_img_align.cpp)
+#TARGET_LINK_LIBRARIES(test_sparse_img_align svo)
 
-ADD_EXECUTABLE(test_pose_optimizer test/test_pose_optimizer.cpp)
-TARGET_LINK_LIBRARIES(test_pose_optimizer svo) 
+#ADD_EXECUTABLE(test_pose_optimizer test/test_pose_optimizer.cpp)
+#TARGET_LINK_LIBRARIES(test_pose_optimizer svo)
diff --git a/svo/include/svo/imu_integration.h b/svo/include/svo/imu_integration.h
index 3cfb77e..442565d 100644
--- a/svo/include/svo/imu_integration.h
+++ b/svo/include/svo/imu_integration.h
@@ -99,19 +99,20 @@ class Imu_Integration{
 public:
     Imu_Integration(Sophus::SE3& SE_init);
     ~Imu_Integration();
-    bool reset();
+    bool reset(gtsam::Values& result);
     bool update(float* imu= nullptr);
-    bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&);
+    bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&,const double reproj_thresh);
 private:
     std::shared_ptr<gtsam::NavState> statePtr;
     std::shared_ptr<gtsam::imuBias::ConstantBias> imu_biasPtr;
-    std::unique_ptr<gtsam::Values> valuesPtr;
-    std::unique_ptr<gtsam::NonlinearFactorGraph> graphPtr;
+    std::shared_ptr<gtsam::Values> valuesPtr;
+    std::shared_ptr<gtsam::NonlinearFactorGraph> graphPtr;
     boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> parameterPtr;
-    std::shared_ptr<gtsam::PreintegrationType> preintegratedPtr;
+    std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> preintegratedPtr;
     boost::shared_ptr<gtsam::noiseModel::Isotropic> ProjectNoisePtr;
-    boost::shared_ptr<gtsam::Cal3_S2> cameraPtr;
-    std::uint32_t t_1=1e-18;
+    std::chrono::steady_clock::time_point t_1;
     std::uint32_t imu_factor_id=0;
+    double imu_mean=0.0;
+    bool syn=false;
 };
 #endif //SVO_IMU_INTEGRATION_H
diff --git a/svo/include/svo/svoVisionFactor.h b/svo/include/svo/svoVisionFactor.h
new file mode 100644
index 0000000..b3f8ab3
--- /dev/null
+++ b/svo/include/svo/svoVisionFactor.h
@@ -0,0 +1,71 @@
+//
+// Created by root on 4/13/21.
+//
+
+#ifndef SVO_SVOVISIONFACTOR_H
+#define SVO_SVOVISIONFACTOR_H
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Pose3.h>
+#include <vikit/robust_cost.h>
+#include <vikit/math_utils.h>
+#include <svo/frame.h>
+
+namespace svo {
+
+    class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
+
+    private:
+        // measurement information
+        gtsam::Point3 feature;   ///<Unit-bearing vector of the feature.
+        gtsam::Point3 landmark;   ///<Unit-bearing vector of the feature.
+        boost::optional<gtsam::Point3> body_P_sensor_; ///< TODO The pose of the sensor in the body frame
+        int level;            ///< Image pyramid level where feature was extracted.
+
+    public:
+        /// shorthand for a smart pointer to a factor
+        typedef boost::shared_ptr<VisionFactor> shared_ptr;
+
+        /**
+        * Constructor
+        * @param feature Unit-bearing vector of the feature.
+        * @param model is the standard deviation
+        * @param pointKey is the index of the landmark
+        * @param poseKey is the index of the camera pose
+        * @param landmark 3D position of the landmark
+        * @param body_P_sensor is the transform from body to sensor frame (default identity)
+        * @param level Image pyramid level where feature was extracted.
+        */
+        VisionFactor(const gtsam::Point3& feature, const gtsam::Point3& landmark,const int level, const gtsam::SharedNoiseModel& Hp,
+                      gtsam::Key poseKey,boost::optional<gtsam::Point3> body_P_sensor = boost::none) :
+                gtsam::NoiseModelFactor1<gtsam::Pose3>(Hp, poseKey), feature(feature), landmark(landmark),body_P_sensor_(body_P_sensor),level(level){}
+        virtual ~VisionFactor(){}
+
+        /// error function
+        /// @param p    the Estimated pose in Pose3
+        /// @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
+        gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional<gtsam::Matrix&> Hp = boost::none) const {
+            // note that use boost optional like a pointer
+            // only calculate jacobian matrix when non-null pointer exists
+            gtsam::Matrix26 J;
+            gtsam::Point3 point(p*landmark);
+            svo::Frame::jacobian_xyz2uv(point, J);
+            if(Hp)(*Hp) = (gtsam::Matrix(2,6)<< J / (1<<level)).finished();
+            Vector2d e = vk::project2d(feature) - vk::project2d(point);
+            e *= 1.0 / (1<<level);
+            return e;
+        }
+        // The second is a 'clone' function that allows the factor to be copied. Under most
+        // circumstances, the following code that employs the default copy constructor should
+        // work fine.
+        virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+            return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+                    gtsam::NonlinearFactor::shared_ptr(new VisionFactor(*this))); }
+
+    };
+
+} // namespace gtsamexamples
+#endif //SVO_SVOVISIONFACTOR_H
diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp
index e4a3140..e51b3ba 100644
--- a/svo/src/frame_handler_mono.cpp
+++ b/svo/src/frame_handler_mono.cpp
@@ -160,7 +160,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   // pose optimization subject to change for adding IMU
   SVO_START_TIMER("pose_optimizer");
   size_t sfba_n_edges_final;
-  if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final))
+  if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh()))
         return RESULT_FAILURE;
   /*
   double sfba_thresh, sfba_error_init, sfba_error_final;
diff --git a/svo/src/imu_integration.cpp b/svo/src/imu_integration.cpp
index 575d31b..44e70ab 100644
--- a/svo/src/imu_integration.cpp
+++ b/svo/src/imu_integration.cpp
@@ -7,19 +7,20 @@
 #include <utility>
 #include "svo/feature.h"
 #include "svo/point.h"
+#include "svo/svoVisionFactor.h"
 
 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(gtsam::Rot3(SE_init.rotation_matrix()),SE_init.translation()),
                                                gtsam::Vector3(0.0,0.0,0.0));
     imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>();
-    valuesPtr=std::make_unique<gtsam::Values>();
+    valuesPtr=std::make_shared<gtsam::Values>();
     valuesPtr->insert(P(0),statePtr->pose());
     valuesPtr->insert(V(0),statePtr->v());
-    valuesPtr->insert(B(0),*imu_biasPtr.get());
-    graphPtr=std::make_unique<gtsam::NonlinearFactorGraph>();
+    valuesPtr->insert(B(0),*imu_biasPtr);
+    graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
     graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()));
     graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 0.1));
-    graphPtr->addPrior(B(0),*imu_biasPtr.get(),gtsam::noiseModel::Isotropic::Sigma(6, 1e-3));
+    graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-3));
     parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame
     parameterPtr->accelerometerCovariance=gtsam::I_3x3 * pow(0.0003924, 2); // acc white noise in continuous
     parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * pow(0.00020568902491,2); // gyro white noise in continuous
@@ -27,58 +28,87 @@ Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     parameterPtr->biasAccCovariance = gtsam::I_3x3 * pow(0.004905, 2);;   // acc bias in continuous
     parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * pow(0.000001454441043, 2);  // gyro bias in continuous
     parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * 1e-5; // error in the bias used for preintegration
-    preintegratedPtr = std::make_shared<gtsam::PreintegrationType>(parameterPtr,*imu_biasPtr.get());
-    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // error in the bias used for projection
-    cameraPtr=boost::make_shared<gtsam::Cal3_S2>(15,12,20,3,12);
+    preintegratedPtr = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(parameterPtr,*imu_biasPtr);
+    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // TODO change the noise used for projection
     assert(preintegratedPtr);
 }
 Imu_Integration::~Imu_Integration(){
 
 }
 bool Imu_Integration::update(float* imu){
-    if(t_1<1e-4){
-        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),0.005);
-        t_1=imu[6];
-    }else{
-        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),imu[6]-t_1);
-        t_1=imu[6];
+    if(syn)return false;
+    double m=0.0;
+    for(int i=0;i<6;++i){
+        m+=abs(imu[i]);
     }
-    return true;
-}
-bool Imu_Integration::reset(){
-    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr.get());
-    return true;
-}
-bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size_t& num_obs){
-    // Adding IMU factor and and pre-integration.
-    auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
-    gtsam::NavState Estimate_state = preintegratedPtr->predict(*statePtr.get(), *imu_biasPtr.get());
-    ++imu_factor_id;
-    valuesPtr->insert(P(imu_factor_id), Estimate_state.pose());
-    valuesPtr->insert(V(imu_factor_id), Estimate_state.v());
-    valuesPtr->insert(B(imu_factor_id), *imu_biasPtr.get());
-    gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id),
-                                 preint_imu_combined);
-    graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
-    for(auto&& f:enumerate(new_frame->fts_)){
-       if(f.item->point==NULL)continue;
-        valuesPtr->insert(F(f.index), new_frame->T_f_w_ * f.item->point->pos_);
-        graphPtr->emplace_shared<gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> >(
-                f.item->px, ProjectNoisePtr, P( imu_factor_id), F(f.index), cameraPtr);
+    if((0.1666666667*m - imu_mean)>1e-6){
+        imu_mean=0.1666666667*m;
+        auto in=std::chrono::steady_clock::now();
+        if(std::chrono::duration<double>(in-t_1).count()>20.0){
+            t_1=in;
+        }else{
+            preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),std::chrono::duration<double>(in-t_1).count());
+            t_1=in;
+        }
+        return true;
     }
+    return false;
 
-    // Now optimize.
-    gtsam::LevenbergMarquardtParams params;
-    params.setVerbosityLM("SUMMARY");
-    gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr.get(), *valuesPtr.get(), params);
-    gtsam::Values result = optimizer.optimize();
+}
+bool Imu_Integration::reset(gtsam::Values& result){
     // Overwrite the beginning of the preintegration for the next step.
     statePtr=std::make_shared<gtsam::NavState>(result.at<gtsam::Pose3>(P(imu_factor_id)), result.at<gtsam::Vector3>(V(imu_factor_id)));
     imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(result.at<gtsam::imuBias::ConstantBias>(B(imu_factor_id)));
-    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr.get());
-    t_1=1e-18;
+    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
+    graphPtr->resize(0);
+    valuesPtr->erase(P(imu_factor_id-1));
+    valuesPtr->erase(V(imu_factor_id-1));
+    valuesPtr->erase(B(imu_factor_id-1));
+    t_1=std::chrono::steady_clock::now();
+    return true;
+}
+bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size_t& num_obs,const double reproj_thresh){
+    // Adding IMU factor and and pre-integration.
+    syn=true;
+    auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
+    gtsam::NavState Estimate_state = preintegratedPtr->predict(*statePtr, *imu_biasPtr);
+    ++imu_factor_id;
+    valuesPtr->insert(P(imu_factor_id), Estimate_state.pose());
+    valuesPtr->insert(V(imu_factor_id), Estimate_state.v());
+    valuesPtr->insert(B(imu_factor_id), *imu_biasPtr);
+    gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id),
+                                 preint_imu_combined);
+    graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
+    num_obs=0;
+    for(auto&& f:new_frame->fts_){
+       if(f->point==NULL)continue;
+        graphPtr->add(svo::VisionFactor(f->f, f->point->pos_,f->level, ProjectNoisePtr,P(imu_factor_id)));
+        ++num_obs;
+    }
+    // Now optimize.
+    //graphPtr->print();
+    //valuesPtr->print();
+    gtsam::LevenbergMarquardtParams params;
+    params.setVerbosityLM("SUMMARY");
+    gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr, params);
+    gtsam::Values result = optimizer.optimize();
+    reset(result);
+    syn=false;
     new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
-   // new_frame->Cov_=gtsam::
-    //if(!reset())return false;
+    double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2();
+    for(auto&& f:new_frame->fts_)
+    {
+        if(f->point == NULL)
+            continue;
+        Eigen::Vector2d e = vk::project2d(f->f) - vk::project2d(result.at<gtsam::Pose3>(P(imu_factor_id)) * f->point->pos_);
+        double sqrt_inv_cov = 1.0 / (1<<f->level);
+        e *= sqrt_inv_cov;
+        if(e.norm() > reproj_thresh_scaled)
+        {
+            // we don't need to delete a reference in the point since it was not created yet
+            f->point = NULL;
+            --num_obs;
+        }
+    }
     return true;
 }
\ No newline at end of file
diff --git a/vio_svo/CMakeLists.txt b/vio_svo/CMakeLists.txt
index 059a8bf..bb8fdc9 100644
--- a/vio_svo/CMakeLists.txt
+++ b/vio_svo/CMakeLists.txt
@@ -8,7 +8,7 @@ SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/"
 # Set build flags. Set IS_ARM on odroid board as environment variable
 SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas")
 IF(DEFINED ENV{ARM_ARCHITECTURE})
-  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a")
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
 ELSE()
   SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3")
 ENDIF()
@@ -20,8 +20,9 @@ ENDIF()
 SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
 
 # Add catkin and required ROS packages
-FIND_PACKAGE(catkin REQUIRED COMPONENTS 
-  roscpp
+FIND_PACKAGE(catkin REQUIRED COMPONENTS
+        geometry_msgs
+        roscpp
   cmake_modules
   nav_msgs
   std_msgs
@@ -41,11 +42,12 @@ FIND_PACKAGE(OpenCV REQUIRED)
 FIND_PACKAGE(Eigen REQUIRED)
 FIND_PACKAGE(Sophus REQUIRED)
 FIND_PACKAGE(Boost REQUIRED COMPONENTS thread)
+FIND_PACKAGE(GTSAM REQUIRED)
 
 # Describe catkin Project
 catkin_package(
   DEPENDS Eigen OpenCV Sophus Boost
-  CATKIN_DEPENDS roscpp nav_msgs std_msgs visualization_msgs sensor_msgs image_transport cv_bridge tf svo svo_msgs vikit_common vikit_ros
+  CATKIN_DEPENDS roscpp nav_msgs std_msgs visualization_msgs sensor_msgs image_transport cv_bridge tf svo svo_msgs vikit_common vikit_ros geometry_msgs
   INCLUDE_DIRS include
   LIBRARIES svo_visualizer
 )
@@ -70,10 +72,10 @@ LIST(APPEND LINK_LIBS
 
 # Create visualizer library
 ADD_LIBRARY(svo_visualizer src/visualizer.cpp)
-TARGET_LINK_LIBRARIES(svo_visualizer ${LINK_LIBS})
+TARGET_LINK_LIBRARIES(svo_visualizer ${LINK_LIBS} gtsam)
                                  
 # Create Executables                      
 ADD_EXECUTABLE(vo src/vo_node.cpp)
-TARGET_LINK_LIBRARIES(vo svo_visualizer)
+TARGET_LINK_LIBRARIES(vo svo_visualizer ${LINK_LIBS})
 #ADD_EXECUTABLE(benchmark src/benchmark_node.cpp)
 #TARGET_LINK_LIBRARIES(benchmark svo_visualizer)
diff --git a/vio_svo/launch/unity.launch b/vio_svo/launch/unity.launch
new file mode 100644
index 0000000..ea34997
--- /dev/null
+++ b/vio_svo/launch/unity.launch
@@ -0,0 +1,22 @@
+<launch>
+
+  <node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
+
+    <!-- Camera topic to subscribe to -->
+    <param name="cam_topic" value="/image_converter/output_image " type="str" />
+    <param name="imu_topic" value="/imu_unity/imu" type="str" />
+
+    <!-- Camera calibration file -->
+    <rosparam file="$(find vio_svo)/param/unity.yaml" />
+
+    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+    <rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
+
+    <!-- Initial camera orientation, make it point downwards -->
+    <param name="init_rx" value="0.00" />
+    <param name="init_ry" value="0.00" />
+    <param name="init_rz" value="0.00" />
+
+  </node>
+
+</launch>
\ No newline at end of file
diff --git a/vio_svo/package.xml b/vio_svo/package.xml
index ff9eab3..bb5b0f2 100644
--- a/vio_svo/package.xml
+++ b/vio_svo/package.xml
@@ -26,6 +26,7 @@
   <build_depend>svo_msgs</build_depend>
   <build_depend>vikit_common</build_depend>
   <build_depend>vikit_ros</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   
   <!-- Dependencies needed after this package is compiled. -->
   <run_depend>roscpp</run_depend>
@@ -40,4 +41,5 @@
   <run_depend>svo_msgs</run_depend>
   <run_depend>vikit_common</run_depend>
   <run_depend>vikit_ros</run_depend>
+  <run_depend>geometry_msgs</run_depend>
 </package>
\ No newline at end of file
diff --git a/vio_svo/param/unity.yaml b/vio_svo/param/unity.yaml
new file mode 100644
index 0000000..023f378
--- /dev/null
+++ b/vio_svo/param/unity.yaml
@@ -0,0 +1,8 @@
+cam_model: ATAN
+cam_width: 1024
+cam_height: 720
+cam_fx: 1.98248
+cam_fy: 2.68212
+cam_cx: 0.502524
+cam_cy: 0.497744
+cam_d0: -0.000499999
\ No newline at end of file
diff --git a/vio_svo/src/vo_node.cpp b/vio_svo/src/vo_node.cpp
index 908abb8..0333176 100644
--- a/vio_svo/src/vo_node.cpp
+++ b/vio_svo/src/vo_node.cpp
@@ -22,6 +22,7 @@
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 #include <std_msgs/String.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <message_filters/synchronizer.h>
 #include <image_transport/image_transport.h>
 #include <boost/thread.hpp>
@@ -49,7 +50,8 @@ public:
       VoNode();
       ~VoNode();
       void imgCb(const sensor_msgs::ImageConstPtr& msg);
-      void imuCb(const sensor_msgs::ImuPtr& imu);
+      //void imuCb(const sensor_msgs::ImuPtr& imu);
+      void imuCb(const geometry_msgs::PoseStamped& imu);
       void imu();
 };
 
@@ -106,15 +108,22 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
         ROS_ERROR("cv_bridge exception: %s", e.what());
       }
 }
-void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
-    float imu_in[7];
-    imu_in[0] = imu->angular_velocity.x;
+void VoNode::imuCb(const geometry_msgs::PoseStamped &imu) {
+    float imu_in[6];
+    /*imu_in[0] = imu->angular_velocity.x;
     imu_in[1] = imu->angular_velocity.y;
     imu_in[2] = imu->angular_velocity.z;
     imu_in[3] = imu->linear_acceleration.x;
     imu_in[4] = imu->linear_acceleration.y;
     imu_in[5] = imu->linear_acceleration.z;
     imu_in[6] = imu->header.stamp.sec;
+     */
+    imu_in[0] = imu.pose.position.z;
+    imu_in[1] = imu.pose.position.x;
+    imu_in[2] = imu.pose.position.y+9.8;
+    imu_in[3] = imu.pose.orientation.z;
+    imu_in[4] = imu.pose.orientation.x;
+    imu_in[5] = imu.pose.orientation.y;
     vo_->imu_integPtr_->update(imu_in);
 }
 void VoNode::imu(){
@@ -137,11 +146,12 @@ int main(int argc, char **argv)
   ros::init(argc, argv, "vio");
   svo::VoNode vo_node;
   ros::NodeHandle nh;
-  ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),10,&svo::VoNode::imuCb, &vo_node);
+  ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),1,&svo::VoNode::imuCb, &vo_node);
   // start processing callbacks
   while(ros::ok() && !vo_node.quit_)
   {
       ros::spinOnce();
+      usleep(1000);
       // TODO check when last image was processed. when too long ago. publish warning that no msgs are received!
   }
   printf("SVO terminated.\n");