diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h
index eb5375c..ff92225 100644
--- a/GPU_version/vio/include/vio/global.h
+++ b/GPU_version/vio/include/vio/global.h
@@ -80,19 +80,20 @@ namespace vio
                   -sin(pitch),cos(pitch);
             T2_=SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
         }
-        SE2_5(double x,double z,double pitch){
+        SE2_5(double y,double z,double roll){
             Eigen::Matrix<double, 2,2> R;
-            R<<cos(pitch),sin(pitch),
-               -sin(pitch),cos(pitch);
-            T2_=SE2(R,Vector2d(x,z));
+            R<<cos(roll),sin(roll),
+               -sin(roll),cos(roll);
+            T2_=SE2(R,Vector2d(y,z));
         };
         SE2 se2() const{
             return T2_;
         }
         SE2 inverse() const{
-            double yaw=atan2(T2_.so2().unit_complex().imag(),T2_.so2().unit_complex().real());
-            return SE2(yaw+M_PI,-1.0*T2_.translation());
+            double roll=atan2(T2_.so2().unit_complex().imag(),T2_.so2().unit_complex().real());
+            return SE2(roll+M_PI,-1.0*T2_.translation());
         }
+        // Rotation around x
         double pitch()const{
             return atan2(T2_.rotation_matrix()(0,1),T2_.rotation_matrix()(0,0));
         }
@@ -102,7 +103,7 @@ namespace vio
             R<<T2_.rotation_matrix()(0,0),0.0,T2_.rotation_matrix()(0,1),
                      0.0,1.0,0.0,
                     T2_.rotation_matrix()(1,0),0.0,T2_.rotation_matrix()(1,1);
-              return SE3(R,Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1)));
+            return SE3(R,Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1)));
         }
         ~SE2_5(){}
 
diff --git a/GPU_version/vio/include/vio/ukf.h b/GPU_version/vio/include/vio/ukf.h
index 983c643..8f5f49c 100644
--- a/GPU_version/vio/include/vio/ukf.h
+++ b/GPU_version/vio/include/vio/ukf.h
@@ -17,7 +17,7 @@ public:
     };
     virtual ~Base(){
     };
-    void predict(double dx,double dz,double dpitch,double time){
+    void predict(double dx,double dy,double dpitch,double time){
         double dt=time-t_cmd;
         if(t_cmd==0.0){
             t_cmd=time;
@@ -25,20 +25,20 @@ public:
         }
         t_cmd=time;
         state_h_(2)=state_(2)+dt*dpitch;
-        state_h_(1)=state_(1)+dt*(dz*cos(state_(2))-dx*sin(state_(2)));
-        state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dz*sin(state_(2)));
+        state_h_(1)=state_(1)+dt*(dy*cos(state_(2))-dx*sin(state_(2)));
+        state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dy*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,dt*(-dx*sin(state_(2))+dz*cos(state_(2))),
-           0,1.0,dt*(-dz*sin(state_(2))-dx*cos(state_(2))),
+        G<<1.0,0,dt*(-dx*sin(state_(2))+dy*cos(state_(2))),
+           0,1.0,dt*(-dy*sin(state_(2))-dx*cos(state_(2))),
            0,0,1.0;
         cov_h_=G*cov_*G.transpose()+R;
         predict_up=true;
     }
-    void correct(double ddx, double ddz, double dpitch, double time){
+    void correct(double ddx, double ddy, double dpitch, double time){
         double dt=time-t_imu;
         if(t_imu==0.0){
             t_imu=time;
@@ -49,28 +49,28 @@ public:
         Eigen::Matrix<double,3,3> Q;
         Eigen::Matrix<double,3,1> E;
         Eigen::Matrix<double,3,3> k;
-        H<<1.0,0,pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
-           0,1.0,-pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
+        H<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),
+           0,1.0,-pow(dt,2)*(ddx*cos(state_(2))+ddy*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)+pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)))-state_h_(0);
-        E(1)=state_(1)+pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
+        E(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)))-state_h_(0);
+        E(1)=state_(1)+pow(dt,2)*(ddy*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(3,3)-k*H)*cov_h_;
         state_h_(2)=state_(2)+dt*dpitch;
-        state_h_(1)=state_(1)+pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)));
-        state_h_(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)));
+        state_h_(1)=state_(1)+pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)));
+        state_h_(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*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,pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
-                0,1.0,-pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
+        G<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),
+                0,1.0,-pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),
                 0,0,1.0;
         cov_h_=G*cov_*G.transpose()+R;
 
@@ -99,7 +99,7 @@ public:
 private:
     double t_cmd=0.0;
     Eigen::Matrix<double,3,3> cov_h_;
-    Eigen::Matrix<double, 3, 1> state_h_;//state{x,z,pitch}
+    Eigen::Matrix<double, 3, 1> state_h_;//state{x,y,pitch( rotation aound z)} in world frame
     double t_imu=0.0;
 
 };
@@ -111,29 +111,29 @@ public:
     virtual ~UKF() {
         delete filter_;
     };
-    void UpdateIMU(double x,double z,double pitch,const ros::Time& time) {
+    void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double pitch/*in imu frame*/,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_->correct(x,z,pitch,1e-9*time.toNSec());
+        if(abs(y)<1.0)y=pow(y,3);//picked up from your code
+        if(filter_->predict_up)filter_->correct(x,y,pitch,1e-9*time.toNSec());
         lock=false;
     };
-    void UpdateCmd(double x,double z,double pitch,const ros::Time& time) {
+    void UpdateCmd(double x/*in imu frame*/,double y/*in imu frame*/,double pitch/*in imu frame*/,const ros::Time& time) {
         while(lock)usleep(5);
         lock=true;
-        filter_->predict(x,z,pitch,1e-9*time.toNSec());
+        filter_->predict(x,y,pitch,1e-9*time.toNSec());
         lock=false;
     };
-    std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) {
+    std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double y/*in camera frame*/,double z/*in camera frame*/,double roll/*in camera frame*/,size_t match,ros::Time& time) {
         while(lock)usleep(5);
         lock=true;
-        pitch=pitch+M_PI;
-        if(filter_->predict_up)filter_->correct(-z,-x,pitch,match,1e-9*time.toNSec());
+        roll=roll+M_PI;
+        if(filter_->predict_up)filter_->correct(-z,-y,roll,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_(1),-filter_->state_(0),pitch_f));
+        double roll_f;
+        roll_f=filter_->state_(2)+M_PI;
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),roll_f));
     };
     std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
         double pitch_f;
diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl
index 158937a..7dc5ad0 100644
--- a/GPU_version/vio/kernel/compute-residual.cl
+++ b/GPU_version/vio/kernel/compute-residual.cl
@@ -15,25 +15,15 @@ float2 world2cam(float3 feature)
 
 float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
 {
-    ref.x*=-1.0;
-    ref.y*=-1.0;
-    ref.z = 3.141592653589793238462643383279502884197169399375 +ref.z;
-    float3 error=ref+cur;
-    float yaw=0.0;
-    float pitch=error.z;
-    float roll=15.0;
-    float R00=cos(yaw)*cos(pitch);//cos(pitch)
-    float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0
-    float R02=cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll);//sin(pitch)
-    float R10=sin(yaw)*cos(pitch);//0
-    float R11=sin(yaw)*sin(pitch)*sin(roll)+cos(yaw)*cos(roll);//1
-    float R12=sin(yaw)*sin(pitch)*cos(roll)-cos(yaw)*sin(roll);//0
-    float R20=-sin(pitch);//-sin(pitch)
-    float R21=cos(pitch)*sin(roll);//0
-    float R22=cos(pitch)*cos(roll);//cos(pitch)
-    return  (float3)(R00*ref_feature.x+R01*ref_feature.y+R02*ref_feature.z+error.x,
-                     R10*ref_feature.x+R11*ref_feature.y+R12*ref_feature.z,
-                     R20*ref_feature.x+R21*ref_feature.y+R22*ref_feature.z+error.y);
+    if(ref.z < 0.0)
+        ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
+    else
+        ref.z = ref.z - 3.1415926535897932384626433832795028841971693993;
+    float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
+    float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z);
+    return  (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
+                     ref_feature.y,
+                     -1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
 }
 
 void jacobian_xyz2uv(float3 xyz_in_f, float* J)