diff --git a/GPU_version/vio/include/gpu_svo/frame_handler_mono.h b/GPU_version/vio/include/gpu_svo/frame_handler_mono.h
index 3faa24f..ea323be 100644
--- a/GPU_version/vio/include/gpu_svo/frame_handler_mono.h
+++ b/GPU_version/vio/include/gpu_svo/frame_handler_mono.h
@@ -33,7 +33,7 @@ class FrameHandlerMono : public FrameHandlerBase
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   
-  FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,6,1>& init);
+  FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init);
   virtual ~FrameHandlerMono();
 
   /// Provide an image.
diff --git a/GPU_version/vio/include/gpu_svo/ukf.h b/GPU_version/vio/include/gpu_svo/ukf.h
index 7919c32..0f1d787 100644
--- a/GPU_version/vio/include/gpu_svo/ukf.h
+++ b/GPU_version/vio/include/gpu_svo/ukf.h
@@ -10,93 +10,87 @@
 #include <Eigen/Dense>
 class Base{
 public:
-    Base(const Eigen::Matrix<double, 6, 1> &init) {
+    Base(const Eigen::Matrix<double, 3, 1> &init) {
         state_=init;
         cov_.setIdentity();
         cov_*=0.0001;
     };
     virtual ~Base(){
     };
-    void predict(double ddx,double ddz,double dpitch,double time){
+    void predict(double dx,double dz,double dpitch,double time){
         if(t_==0.0){
             t_=time;
             return;
         }
-        double t=time-t_;
+        double dt=time-t_;
         t_=time;
-        state_h_<<state_(0)+t*state_(2)+0.5*pow(t,2)*(ddx*cos(state_(4))-ddz*sin(state_(4))),
-                state_(1)+t*state_(3)+0.5*pow(t,2)*(ddz*cos(state_(4))+ddx*sin(state_(4))),
-                state_(2)+t*(ddx*cos(state_(4))-ddz*sin(state_(4))),
-                state_(3)+t*(ddz*cos(state_(4))+ddx*sin(state_(4))),
-                state_(4)+0.5*t*(state_(5)+dpitch),
-                dpitch;
-        Eigen::Matrix<double,6,6> G;
-        G<<1.0,0,t,0,-0.5*pow(t,2)*(ddz*cos(state_(4))+ddx*sin(state_(4))),0,
-           0,1.0,0,t,0.5*pow(t,2)*(ddx*cos(state_(4))-ddz*sin(state_(4))),0,
-           0,0,1.0,0,-t*(ddz*cos(state_(4))+ddx*sin(state_(4))),0,
-           0,0,0,1.0,t*(ddx*cos(state_(4))-ddz*sin(state_(4))),0,
-           0,0,0,0,1.0,t*dpitch,
-           0,0,0,0,0,1.0;
-        Eigen::Matrix<double,6,6> R;
-        R<<10.0*svo::Config::ACC_Noise(),0,0,0,0,0,
-           0,10.0*svo::Config::ACC_Noise(),0,0,0,0,
-           0,0,svo::Config::ACC_Noise(),0,0,0,
-           0,0,0,svo::Config::ACC_Noise(),0,0,
-           0,0,0,0,10.0*svo::Config::GYO_Noise(),0,
-           0,0,0,0,0,svo::Config::GYO_Noise();
+        state_h_(2)=state_(2)+dt*dpitch;
+        state_h_(1)=state_(1)+dz*cos(state_(2))-dx*sin(state_(2));
+        state_h_(0)=state_(0)+dx*cos(state_(2))+dz*sin(state_(2));
+        Eigen::Matrix<double,3,3> R;
+        R<<svo::Config::Cmd_Cov(),0,0,
+           0,svo::Config::Cmd_Cov(),0,
+           0,0,svo::Config::Cmd_Cov();
+        Eigen::Matrix<double,3,3> G;
+        G<<1.0,0,-dx*sin(state_(2))+dz*cos(state_(2)),
+           0,1.0,-dz*sin(state_(2))-dx*cos(state_(2)),
+           0,0,1.0;
         cov_h_=G*cov_*G.transpose()+R;
         predict_up=true;
     }
-    void correct(double x, double z, double pitch, double time){
-        Eigen::Matrix<double,3,6> H;
+    void correct(double ddx, double ddz, double dpitch, double time){
+        Eigen::Matrix<double,3,3> H;
         Eigen::Matrix<double,3,3> Q;
         Eigen::Matrix<double,3,1> E;
-        Eigen::Matrix<double,6,3> k;
+        Eigen::Matrix<double,3,3> k;
+        double dt=time-t_;
         t_=time;
-        H<<0,0,1.0,0,0,0,
-           0,0,0,1.0,0,0,
-           0,0,0,0,0,1.0;
-        Q<<svo::Config::Cmd_Cov(),0,0,
-           0,svo::Config::Cmd_Cov(),0,
-           0,0,svo::Config::Cmd_Cov();
-        E<<x-state_h_(2),z-state_h_(3),pitch-state_h_(5);
+        H<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
+           0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
+           0,0,1.0;
+        Q<<svo::Config::ACC_Noise(),0,0,
+           0,svo::Config::ACC_Noise(),0,
+           0,0,svo::Config::GYO_Noise();
+        E(0)=state_(0)+0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)))-state_h_(0);
+        E(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
+        E(2)=state_(2)+dt*dpitch-state_h_(2);
         k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
         state_=state_h_+k*E;
-        cov_=(Eigen::MatrixXd::Identity(6,6)-k*H)*cov_h_;
+        cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
         predict_up=false;
     }
     void correct(double x, double z,double pitch,size_t match,double time){
-        Eigen::Matrix<double,3,6> H;
+        Eigen::Matrix<double,3,3> H;
         Eigen::Matrix<double,3,3> Q;
         Eigen::Matrix<double,3,1> E;
-        Eigen::Matrix<double,6,3> k;
+        Eigen::Matrix<double,3,3> k;
         t_=time;
         ++match;
-        H<<1.0,0,0,0,0,0,
-           0,1.0,0,0,0,0,
-           0,0,0,0,1.0,0;
+        H<<1.0,0,0,
+           0,1.0,0,
+           0,0,1.0;
         Q<<svo::Config::Svo_Ekf()/match,0,0,
            0,svo::Config::Svo_Ekf()/match,0,
-           0,0,10*svo::Config::Svo_Ekf()/match;
-        E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(4);
+           0,0,50*svo::Config::Svo_Ekf()/match;
+        E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(2);
         k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
         state_=state_h_+k*E;
-        cov_=(Eigen::MatrixXd::Identity(6,6)-k*H)*cov_h_;
+        cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
         predict_up=false;
     }
-    Eigen::Matrix<double,6,6> cov_;
-    Eigen::Matrix<double, 6, 1> state_;
+    Eigen::Matrix<double,3,3> cov_;
+    Eigen::Matrix<double, 3, 1> state_;
     bool predict_up=false;
 private:
     double t_=0.0;
-    Eigen::Matrix<double,6,6> cov_h_;
-    Eigen::Matrix<double, 6, 1> state_h_;//state{x,z,dx,dv,pitch}
+    Eigen::Matrix<double,3,3> cov_h_;
+    Eigen::Matrix<double, 3, 1> state_h_;//state{x,z,pitch}
 
 };
 
 class UKF {
 public:
-    UKF(const Eigen::Matrix<double, 6, 1>& init) : filter_(new Base(init)) {
+    UKF(const Eigen::Matrix<double, 3, 1>& init) : filter_(new Base(init)) {
     };
     virtual ~UKF() {
         delete filter_;
@@ -104,30 +98,24 @@ public:
     void UpdateIMU(double x,double z,double pitch,const ros::Time& time) {
         while(lock)usleep(5);
         lock=true;
-        //if(abs(x)<1.0)x=pow(x,3);//picked up from your code
-        //if(abs(z)<1.0)z=pow(z,3);//picked up from your code
-        if(!filter_->predict_up)filter_->predict(x,z,pitch,1e-9*time.toNSec());
+        if(abs(x)<1.0)x=pow(x,3);//picked up from your code
+        if(abs(z)<1.0)z=pow(z,3);//picked up from your code
+        if(filter_->predict_up)filter_->correct(x,z,pitch,1e-9*time.toNSec());
         lock=false;
     };
     void UpdateCmd(double x,double z,double pitch,const ros::Time& time) {
         while(lock)usleep(5);
         lock=true;
-        filter_->correct(x,z,pitch,1e-9*time.toNSec());
+        filter_->predict(x,z,pitch,1e-9*time.toNSec());
         lock=false;
     };
     std::pair<Eigen::Matrix<double,3,3>,svo::SE2> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) {
         while(lock)usleep(5);
         lock=true;
-        filter_->correct(-1.0*x,-1.0*z,pitch,match,1e-9*time.toNSec());
+        if(filter_->predict_up)filter_->correct(x,z,pitch,match,1e-9*time.toNSec());
         lock=false;
-        //std::cout<<"filter cov\n"<<filter_->cov_<<'\n';
-        //std::cout<<"filter state\n"<<filter_->state_<<'\n';
-        Eigen::Matrix<double,3,3> cov;
-        cov<<filter_->cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,4),
-             filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,4),
-             filter_->cov_(4,0),filter_->cov_(4,1),filter_->cov_(4,4);
-        svo::SE2 out(filter_->state_(4),Eigen::Vector2d(-1.0*filter_->state_(0),-1.0*filter_->state_(1)));
-        return std::pair<Eigen::Matrix<double,3,3>,svo::SE2>(cov,out);
+        svo::SE2 out(filter_->state_(2),Eigen::Vector2d(filter_->state_(0),filter_->state_(1)));
+        return std::pair<Eigen::Matrix<double,3,3>,svo::SE2>(filter_->cov_,out);
     };
 private:
      Base* filter_= nullptr;
diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml
index 99da116..11b2b18 100644
--- a/GPU_version/vio/param/vo_fast.yaml
+++ b/GPU_version/vio/param/vo_fast.yaml
@@ -12,4 +12,4 @@ svo:
   ACC_noise: 0.1          #acc white noise in continuous in EKF
   GYO_noise: 0.1          #gyro white noise in continuous in EKF
   svo_ekf: 5                #Svo noise that will be devided by number of Reprojected points after opmization in EKF
-  Cmd_Cov: 0.05  #control vector noise in EKF
+  Cmd_Cov: 0.5  #control vector noise in EKF
diff --git a/GPU_version/vio/src/feature_alignment.cpp b/GPU_version/vio/src/feature_alignment.cpp
index 0a3de63..9e42e47 100644
--- a/GPU_version/vio/src/feature_alignment.cpp
+++ b/GPU_version/vio/src/feature_alignment.cpp
@@ -203,7 +203,7 @@ bool align2D(
   bool sign=true;// +
 
   // termination condition
-  const float min_update_squared = 0.1;//0.001
+  const float min_update_squared = 0.5;//0.001
   const int cur_step = cur_img.step.p[0];
 //  float chi2 = 0;
   Vector3f update; update.setZero();
@@ -337,7 +337,7 @@ bool align2D_SSE2(
   float v = cur_px_estimate.y();
 
   // termination condition
-  const float min_update_squared = 0.03*0.03;
+  const float min_update_squared = 0.1*0.1;
   const int cur_step = cur_img.step.p[0];
   const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16
   float chi2 = 0;
@@ -502,7 +502,7 @@ bool align2D_NEON (
   float v = cur_px_estimate.y();
 
   // termination condition
-  const float min_update_squared = 0.03*0.03;
+  const float min_update_squared = 0.1*0.1;
   const int cur_step = cur_img.step.p[0];
   Vector3f update;
   Vector3f Jres;
diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp
index 74527d1..f9782a5 100644
--- a/GPU_version/vio/src/frame_handler_mono.cpp
+++ b/GPU_version/vio/src/frame_handler_mono.cpp
@@ -27,7 +27,7 @@
 
 namespace svo {
 
-FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,6,1>& init) :
+FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init) :
   FrameHandlerBase(),
   cam_(cam),
   reprojector_(cam_, map_),
@@ -133,7 +133,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
 
 FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 {
-
   new_frame_->T_f_w_ = last_frame_->T_f_w_;
   // sparse image align
   SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp
index 20e874d..8fe3f84 100644
--- a/GPU_version/vio/src/vo_node.cpp
+++ b/GPU_version/vio/src/vo_node.cpp
@@ -74,8 +74,8 @@ VoNode::VoNode() :
           Eigen::Vector3d(vk::getParam<double>("vio/init_tx", 0.0),
                           vk::getParam<double>("vio/init_ty", 0.0),
                           vk::getParam<double>("vio/init_tz", 0.0)));
-    Eigen::Matrix<double,6,1> init;
-    init<<1e-5,1e-5,1e-5,1e-5,1e-5,1e-5;
+    Eigen::Matrix<double,3,1> init;
+    init<<1e-19,1e-19,1e-19;
     vo_ = new svo::FrameHandlerMono(cam_,init);
     usleep(500);
 
@@ -157,6 +157,7 @@ void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
         }
 #endif
     vo_->UpdateIMU(imu_in,imu->header.stamp);
+    imu_time_=imu->header.stamp;
 }
 void VoNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
         double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z};