diff --git a/GPU_version/vio/include/vio/depth_filter.h b/GPU_version/vio/include/vio/depth_filter.h
index 04190a9..8c9eee2 100644
--- a/GPU_version/vio/include/vio/depth_filter.h
+++ b/GPU_version/vio/include/vio/depth_filter.h
@@ -123,7 +123,7 @@ public:
   std::list<Seed, Eigen::aligned_allocator<Seed>>& getSeeds() { return seeds_; }
 
   /// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty
-  static void updateSeed(
+  static bool updateSeed(
       const float x,
       const float tau2,
       Seed* seed);
@@ -150,7 +150,7 @@ protected:
   double new_keyframe_min_depth_;       //!< Minimum depth in the new keyframe. Used for range in new seeds.
   double new_keyframe_mean_depth_;      //!< Maximum depth in the new keyframe. Used for range in new seeds.
   Matcher matcher_;
-  uint seeds_cont_=0;
+  //uint seeds_cont_=0;
 
   /// Initialize new seeds from a frame.
   void initializeSeeds(FramePtr frame);
diff --git a/GPU_version/vio/include/vio/matcher.h b/GPU_version/vio/include/vio/matcher.h
index c7c1c6e..0a79204 100644
--- a/GPU_version/vio/include/vio/matcher.h
+++ b/GPU_version/vio/include/vio/matcher.h
@@ -87,7 +87,7 @@ public:
       max_epi_search_steps(1000),
       subpix_refinement(true),
       epi_search_edgelet_filtering(true),
-      epi_search_edgelet_max_angle(0.7)
+      epi_search_edgelet_max_angle(0.3)
     {}
   } options_;
 
diff --git a/GPU_version/vio/include/vio/ukf.h b/GPU_version/vio/include/vio/ukf.h
index 066e25c..8cd09ec 100644
--- a/GPU_version/vio/include/vio/ukf.h
+++ b/GPU_version/vio/include/vio/ukf.h
@@ -18,22 +18,16 @@ public:
     virtual ~Base(){
     };
     void predict(double dx,double dz,double dpitch,double time){
-        if(t_==0.0){
-            t_=time;
-            return;
-        }
-        double dt=time-t_;
-        t_=time;
-        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));
+        state_h_(2)=state_(2)+dt_angular*dpitch;
+        state_h_(1)=state_(1)+dt_liner*(dz*cos(state_(2))-dx*sin(state_(2)));
+        state_h_(0)=state_(0)+dt_liner*(dx*cos(state_(2))+dz*sin(state_(2)));
         Eigen::Matrix<double,3,3> R;
         R<<vio::Config::Cmd_Cov(),0,0,
            0,vio::Config::Cmd_Cov(),0,
            0,0,vio::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)),
+        G<<1.0,0,dt_liner*(-dx*sin(state_(2))+dz*cos(state_(2))),
+           0,1.0,dt_liner*(-dz*sin(state_(2))-dx*cos(state_(2))),
            0,0,1.0;
         cov_h_=G*cov_*G.transpose()+R;
         predict_up=true;
@@ -43,33 +37,30 @@ public:
         Eigen::Matrix<double,3,3> Q;
         Eigen::Matrix<double,3,1> E;
         Eigen::Matrix<double,3,3> k;
-        double dt=time-t_;
-        t_=time;
-        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))),
+        H<<1.0,0,pow(0.03*dt_liner,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
+           0,1.0,-pow(0.03*dt_liner,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
            0,0,1.0;
         Q<<vio::Config::ACC_Noise(),0,0,
            0,vio::Config::ACC_Noise(),0,
            0,0,vio::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);
+        E(0)=state_(0)+pow(0.03*dt_liner,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)))-state_h_(0);
+        E(1)=state_(1)+pow(0.03*dt_liner,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
+        E(2)=state_(2)+0.03*dt_angular*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(3,3)-k*H)*cov_h_;
-        state_h_(2)=state_(2)+dt*dpitch;
-        state_h_(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)));
-        state_h_(0)=state_(0)+0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)));
+        state_h_(2)=state_(2)+0.03*dt_angular*dpitch;
+        state_h_(1)=state_(1)+pow(0.03*dt_liner,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)));
+        state_h_(0)=state_(0)+pow(0.03*dt_liner,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)));
         Eigen::Matrix<double,3,3> R;
         R<<vio::Config::ACC_Noise(),0,0,
                 0,vio::Config::ACC_Noise(),0,
                 0,0,vio::Config::GYO_Noise();
         Eigen::Matrix<double,3,3> G;
-        G<<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))),
+        G<<1.0,0,pow(0.03*dt_liner,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
+                0,1.0,-pow(0.03*dt_liner,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
                 0,0,1.0;
         cov_h_=G*cov_*G.transpose()+R;
-        yaw_t_1_=state_(2);
 
     }
     void correct(double x, double z,double pitch,size_t match,double time){
@@ -77,7 +68,6 @@ public:
         Eigen::Matrix<double,3,3> Q;
         Eigen::Matrix<double,3,1> E;
         Eigen::Matrix<double,3,3> k;
-        t_=time;
         ++match;
         H<<1.0,0,0,
            0,1.0,0,
@@ -89,17 +79,16 @@ public:
         k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
         state_=state_h_+(k*E);
         cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
-        yaw_t_1_=state_(2);
         predict_up=false;
     }
     Eigen::Matrix<double,3,3> cov_;
     Eigen::Matrix<double, 3, 1> state_;
     bool predict_up=false;
 private:
-    double t_=0.0;
     Eigen::Matrix<double,3,3> cov_h_;
     Eigen::Matrix<double, 3, 1> state_h_;//state{x,z,pitch}
-    double yaw_t_1_=0.0;
+    double dt_liner=0.2;
+    double dt_angular=0.2;
 
 };
 
@@ -128,16 +117,16 @@ public:
         while(lock)usleep(5);
         lock=true;
         pitch=pitch+M_PI;
-        if(filter_->predict_up)filter_->correct(-x,-z,pitch,match,1e-9*time.toNSec());
+        if(filter_->predict_up)filter_->correct(-z,-x,pitch,match,1e-9*time.toNSec());
         lock=false;
         double pitch_f;
         pitch_f=filter_->state_(2)+M_PI;
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),pitch_f));
     };
     std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
         double pitch_f;
         pitch_f=filter_->state_(2)+M_PI;
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),pitch_f));
     }
 private:
      Base* filter_= nullptr;
diff --git a/GPU_version/vio/out.txt b/GPU_version/vio/out.txt
index ab3b540..72bd21a 100644
Binary files a/GPU_version/vio/out.txt and b/GPU_version/vio/out.txt differ
diff --git a/GPU_version/vio/param/px30.yaml b/GPU_version/vio/param/px30.yaml
index 4b69c4f..a9bfd59 100644
--- a/GPU_version/vio/param/px30.yaml
+++ b/GPU_version/vio/param/px30.yaml
@@ -2,8 +2,8 @@ vio:
     cam_model: ATAN
     cam_width: 640
     cam_height: 480
-    cam_fx: 0.463632
-    cam_fy: 0.823661
-    cam_cx: 0.497465
-    cam_cy: 0.498443
-    cam_d0: 1.01687
+    cam_fx: 0.490202
+    cam_fy: 0.866527
+    cam_cx: 0.485102
+    cam_cy: 0.508577
+    cam_d0: 1.03345
diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml
index 69954f4..26b815b 100644
--- a/GPU_version/vio/param/vo_fast.yaml
+++ b/GPU_version/vio/param/vo_fast.yaml
@@ -5,12 +5,12 @@ svo:
   quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
   quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
   init_min_tracked: 100
-  init_min_disparity: 60
+  init_min_disparity: 80
   structureoptim_max_pts: 50
   kfselect_mindist: 0.01
   poseoptim_thresh: 4.0
-  init_min_inliers: 30
+  init_min_inliers: 40
   ACC_noise: 0.5          #acc white noise in continuous in EKF
-  GYO_noise: 0.001          #gyro white noise in continuous in EKF
+  GYO_noise: 0.005          #gyro white noise in continuous in EKF
   svo_ekf: 1.5                #Svo noise that will be devided by number of Reprojected points after opmization in EKF
-  Cmd_Cov: 1.0  #control vector noise in EKF
+  Cmd_Cov: 0.1  #control vector noise in EKF
diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp
index 0a1f1e2..bef435e 100644
--- a/GPU_version/vio/src/depth_filter.cpp
+++ b/GPU_version/vio/src/depth_filter.cpp
@@ -109,13 +109,8 @@ void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_mi
     new_keyframe_set_ = true;
     seeds_updating_halt_ = true;
     frame_queue_cond_.notify_one();
-  }else if(seeds_cont_>3){
-        initializeSeeds(frame);
-        seeds_cont_=0;
-  }else if(seeds_cont_==0){
-        initializeSeeds(frame);
   }
-  ++seeds_cont_;
+  initializeSeeds(frame);
 }
 
 void DepthFilter::initializeSeeds(FramePtr frame)
@@ -185,9 +180,6 @@ void DepthFilter::updateSeedsLoop()
       if(new_keyframe_set_)
       {
         new_keyframe_set_ = false;
-        if(seeds_cont_>3){
-            seeds_updating_halt_ = false;
-        }
         clearFrameQueue();
         frame = new_keyframe_;
       }
@@ -198,12 +190,6 @@ void DepthFilter::updateSeedsLoop()
       }
     }
     updateSeeds(frame);
-
-    if((frame->isKeyframe() && seeds_cont_>3) ||(frame->isKeyframe() && seeds_cont_==0)){
-        initializeSeeds(frame);
-        seeds_cont_=0;
-    }
-
   }
 }
 
@@ -211,7 +197,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
 {
   // update only a limited number of seeds, because we don't have time to do it
   // for all the seeds in every frame!
-  size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size();
+  size_t n_updates=0, n_failed_matches=0;
   lock_t lock(seeds_mut_);
   std::list<Seed>::iterator it=seeds_.begin();
 
@@ -224,11 +210,6 @@ void DepthFilter::updateSeeds(FramePtr frame)
     if(seeds_updating_halt_)
       return;
 
-    // check if seed is not already too old
-    if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) {
-      it = seeds_.erase(it);
-      continue;
-    }
     if(it->ftr->frame == NULL){
         it = seeds_.erase(it);
         continue;
@@ -241,7 +222,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
     T.rotation_matrix()(1,0),0.0,T.rotation_matrix()(1,1);
     SE3 T_ref_cur(R,Vector3d(T.translation()(0),0.0,T.translation()(1)));
     const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
-    if(xyz_f.z() < 0.0 && xyz_f.y() < 0.08)  {
+    if(xyz_f.z() < 0.0)  {
       ++it; // behind the camera
       continue;
     }
@@ -271,7 +252,11 @@ void DepthFilter::updateSeeds(FramePtr frame)
     double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
 
     // update the estimate
-    updateSeed(1.0/z, tau_inverse*tau_inverse, &*it);
+    if(!updateSeed(1.0/z, tau_inverse*tau_inverse, &*it)){
+        ++it;
+        ++n_failed_matches;
+        continue;
+    }
     ++n_updates;
 
     if(frame->isKeyframe())
@@ -315,12 +300,11 @@ void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds)
       seeds.push_back(*it);
   }
 }
-
-void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
+ bool DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
 {
   float norm_scale = sqrt(seed->sigma2 + tau2);
   if(std::isnan(norm_scale))
-    return;
+    return false;
   boost::math::normal_distribution<float> nd(seed->mu, norm_scale);
   float s2 = 1./(1./seed->sigma2 + 1./tau2);
   float m = s2*(seed->mu/seed->sigma2 + x/tau2);
@@ -339,6 +323,7 @@ void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
   seed->mu = mu_new;
   seed->a = (e-f)/(f-e/f);
   seed->b = seed->a*(1.0f-f)/f;
+  return true;
 }
 
 double DepthFilter::computeTau(
diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp
index 576b57d..1b4de63 100644
--- a/GPU_version/vio/src/frame_handler_mono.cpp
+++ b/GPU_version/vio/src/frame_handler_mono.cpp
@@ -171,11 +171,11 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
   if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-/*
+
   std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
              <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
              <<"\t distance between two frames: "<<
-             (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';*/
+             (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
   if( reprojector_.n_trials_ < 10)return RESULT_FAILURE;
   size_t sfba_n_edges_final=0;
   double sfba_thresh, sfba_error_init, sfba_error_final;
@@ -183,16 +183,14 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
             Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
             new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
   if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
-       fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){
+       fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI ||
+       sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
         return RESULT_FAILURE;
     }
   auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),
                                 new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_);
   new_frame_->T_f_w_ =result.second;
   new_frame_->Cov_ = result.first;
-  if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
-      return RESULT_FAILURE;
-  }
 
   optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
   // select keyframe
diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp
index 0b9b9f7..3816fd8 100644
--- a/GPU_version/vio/src/matcher.cpp
+++ b/GPU_version/vio/src/matcher.cpp
@@ -249,7 +249,7 @@ bool Matcher::findEpipolarMatchDirect(
     return false;
   }
 
-  size_t n_steps = epi_length_/0.7; // one step per pixel
+  size_t n_steps = epi_length_/0.3; // one step per pixel
   Vector2d step = epi_dir_/n_steps;
 
   if(n_steps > options_.max_epi_search_steps)
diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp
index a64adf5..1ce97e2 100644
--- a/GPU_version/vio/src/sparse_img_align_gpu.cpp
+++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp
@@ -129,6 +129,8 @@ int SparseImgAlignGpu::solve()
     x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2]));
     if((bool) std::isnan((double) x_[0]) || (bool) std::isnan((double) x_[1]) ||(bool) std::isnan((double) x_[2]))
         return 0;
+    double norm=x_.norm();
+    if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1);
     return 1;
 }
 void SparseImgAlignGpu::update()
diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp
index 0145baa..e04a74c 100644
--- a/GPU_version/vio/src/visualizer.cpp
+++ b/GPU_version/vio/src/visualizer.cpp
@@ -55,8 +55,8 @@ void Visualizer::publishMinimal(
     Quaterniond q(AngleAxisd(odom.second.pitch(), Vector3d::UnitZ()));
     geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped);
       msg_pose_with_cov->header = header_msg;
-      msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(1);
-      msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(0);
+      msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(0);
+      msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(1);
       msg_pose_with_cov->pose.pose.position.z = 0.0;
       msg_pose_with_cov->pose.pose.orientation.x = q.x();
       msg_pose_with_cov->pose.pose.orientation.y = q.y();
@@ -71,7 +71,7 @@ void Visualizer::publishMinimal(
     pub_pose_with_cov_.publish(msg_pose_with_cov);
     if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
           publishPointMarker(
-                  pub_points_, Eigen::Vector3d(odom.second.se2().translation()(1),odom.second.se2().translation()(0),0.0), "trajectory",
+                  pub_points_, Eigen::Vector3d(odom.second.se2().translation()(0),odom.second.se2().translation()(1),0.0), "trajectory",
                   ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
     }
   }
diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp
index ef818eb..7f0bbd4 100644
--- a/GPU_version/vio/src/vo_node.cpp
+++ b/GPU_version/vio/src/vo_node.cpp
@@ -154,7 +154,7 @@ void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
     };
     imu_in[0] = 0.5*imu_[0]+0.5*imu->linear_acceleration.x;
     imu_in[1] = 0.5*imu_[1]+0.5*imu->linear_acceleration.y;
-    imu_in[2] = 0.5*imu_[2]+0.5*imu->angular_velocity.z;
+    imu_in[2] = imu->angular_velocity.z;
     memcpy(imu_, imu_in, static_cast<std::size_t>(3*sizeof(double)));
     vo_->UpdateIMU(imu_in,imu->header.stamp);
     imu_time_=imu->header.stamp;
@@ -184,8 +184,8 @@ bool VioNode::getOdom(vio::getOdom::Request &req, vio::getOdom::Response &res) {
         res.header.frame_id = "/world";
         res.header.seq = trace_id_;
         auto odom=vo_->ukfPtr_.get_location();
-        res.x=odom.second.se2().translation()(1);
-        res.y=odom.second.se2().translation()(0);
+        res.x=odom.second.se2().translation()(0);
+        res.y=odom.second.se2().translation()(1);
         res.yaw=odom.second.pitch();
         res.cov={odom.first(0,0),odom.first(0,1),odom.first(0,2),
                  odom.first(1,0),odom.first(1,1),odom.first(1,2),