diff --git a/GPU_version/vio/include/gpu_svo/ukf.h b/GPU_version/vio/include/gpu_svo/ukf.h
index 0f1d787..b3a5cd2 100644
--- a/GPU_version/vio/include/gpu_svo/ukf.h
+++ b/GPU_version/vio/include/gpu_svo/ukf.h
@@ -56,8 +56,23 @@ public:
         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;
+        if(fabs(state_(2)-yaw_t_1_)>M_PI_4)state_(2)-=M_PI;
+        if(fabs(state_(2))>2*M_PI)state_(2)-=2.0*int(0.5*state_(2)/M_PI)*M_PI;
         cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
-        predict_up=false;
+        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)));
+        Eigen::Matrix<double,3,3> R;
+        R<<svo::Config::ACC_Noise(),0,0,
+                0,svo::Config::ACC_Noise(),0,
+                0,0,svo::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))),
+                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){
         Eigen::Matrix<double,3,3> H;
@@ -69,13 +84,16 @@ public:
         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,50*svo::Config::Svo_Ekf()/match;
+        Q<<svo::Config::Svo_Ekf(),0,0,
+           0,svo::Config::Svo_Ekf(),0,
+           0,0,svo::Config::Svo_Ekf();
         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;
+        state_=state_h_+(k*E);
+        if(fabs(state_(2)-yaw_t_1_)>M_PI_4)state_(2)-=M_PI;
+        if(fabs(state_(2))>2*M_PI)state_(2)-=2.0*int(0.5*state_(2)/M_PI)*M_PI;
         cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
+        yaw_t_1_=state_(2);
         predict_up=false;
     }
     Eigen::Matrix<double,3,3> cov_;
@@ -85,6 +103,7 @@ 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;
 
 };
 
@@ -109,13 +128,12 @@ public:
         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) {
+    std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) {
         while(lock)usleep(5);
         lock=true;
         if(filter_->predict_up)filter_->correct(x,z,pitch,match,1e-9*time.toNSec());
         lock=false;
-        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);
+        return std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5>(filter_->cov_,svo::SE2_5(filter_->state_(0),filter_->state_(1),filter_->state_(2)));
     };
 private:
      Base* filter_= nullptr;
diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml
index 11b2b18..f0c5dd7 100644
--- a/GPU_version/vio/param/vo_fast.yaml
+++ b/GPU_version/vio/param/vo_fast.yaml
@@ -7,9 +7,9 @@ svo:
   init_min_tracked: 100
   init_min_disparity: 30
   structureoptim_max_pts: 50
-  kfselect_mindist: 0.05
+  kfselect_mindist: 0.001
   poseoptim_thresh: 4.0
-  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.5  #control vector noise in EKF
+  ACC_noise: 0.001          #acc white noise in continuous in EKF
+  GYO_noise: 0.001          #gyro white noise in continuous in EKF
+  svo_ekf: 100                #Svo noise that will be devided by number of Reprojected points after opmization in EKF
+  Cmd_Cov: 0.001  #control vector noise in EKF
diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp
index f9782a5..5f172ab 100644
--- a/GPU_version/vio/src/frame_handler_mono.cpp
+++ b/GPU_version/vio/src/frame_handler_mono.cpp
@@ -113,7 +113,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
   new_frame_->T_f_w_ = last_frame_->T_f_w_;
   initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_);
   auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.getSE2().translation()(0),new_frame_->T_f_w_.getSE2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_);
-  new_frame_->T_f_w_ = SE2_5(result.second);
+  new_frame_->T_f_w_ = result.second;
   new_frame_->Cov_ = result.first;
   if(res == initialization::NO_KEYFRAME)
       return RESULT_NO_KEYFRAME;
@@ -138,8 +138,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
                            30, SparseImgAlign::LevenbergMarquardt, false, false);
   img_align.run(last_frame_, new_frame_);
- // std::cout<<"img_align:\n"<<new_frame_->T_f_w_.getSE2()<<"\n";
-  // map reprojection & feature alignment
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
   size_t sfba_n_edges_final=0;
   double sfba_thresh, sfba_error_init, sfba_error_final;
@@ -151,7 +149,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   }
   auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.getSE2().translation()(0),
                                 new_frame_->T_f_w_.getSE2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_);
-  new_frame_->T_f_w_ = SE2_5(result.second);
+  new_frame_->T_f_w_ =result.second;
   new_frame_->Cov_ = result.first;
   std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
   <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
@@ -172,10 +170,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   double depth_mean, depth_min;
 
   frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
-  if(depth_mean > 50 || depth_min < 0.0)
-  {
-      return RESULT_NO_KEYFRAME;
-  }
   if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited
   {
     depth_filter_->addFrame(new_frame_);
@@ -191,6 +185,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 
   // init new depth-filters
   depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
+
   // if limited number of keyframes, remove the one furthest apart
   if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs())
   {
@@ -275,13 +270,11 @@ bool FrameHandlerMono::needNewKf(double scene_depth_mean)
   if(scene_depth_mean >10.0)return true;
   for(auto&& it:overlap_kfs_)
   {
-    Vector3d relpos = new_frame_->w2f(Vector3d(it.first->pos()(0),0.0,it.first->pos()(1)));
-    if(fabs(relpos.norm())/scene_depth_mean < Config::kfSelectMinDist()){
-        return false;
-    }
-
+    if(fabs(it.first->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>0.0523599)return true;
+    if(fabs((it.first->T_f_w_.translation()-new_frame_->T_f_w_.translation()).norm()) > Config::kfSelectMinDist())
+        return true;
   }
-  return true;
+  return false;
 }
 
 void FrameHandlerMono::setCoreKfs(size_t n_closest)
diff --git a/GPU_version/vio/src/map.cpp b/GPU_version/vio/src/map.cpp
index 877d617..c6eb05b 100644
--- a/GPU_version/vio/src/map.cpp
+++ b/GPU_version/vio/src/map.cpp
@@ -109,7 +109,6 @@ void Map::getCloseKeyframes(
 {
   for(auto kf : keyframes_)
   {
-      std::cout<<"key points:"<<std::to_string(kf->key_pts_.size())<<'\t';
     // check if kf has overlaping field of view with frame, use therefore KeyPoints
     for(auto keypoint : kf->key_pts_)
     {