diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch
index ced1e84..99e9768 100644
--- a/launch/mapping_avia.launch
+++ b/launch/mapping_avia.launch
@@ -20,7 +20,7 @@
 	<param name="edgeb" type="double" value="0.1"/>
 	<param name="smallp_intersect" type="double" value="172.5"/>
 	<param name="smallp_ratio" type="double" value="1.2"/>
-	<param name="point_filter_num" type="int" value="1"/>
+	<param name="point_filter_num" type="int" value="3"/>
   <node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/>
 
   <node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen" required="true">
diff --git a/launch/mapping_avia_outdoor.launch b/launch/mapping_avia_outdoor.launch
index 03bde28..e6f13a3 100644
--- a/launch/mapping_avia_outdoor.launch
+++ b/launch/mapping_avia_outdoor.launch
@@ -20,7 +20,7 @@
 	<param name="edgeb" type="double" value="0.1"/>
 	<param name="smallp_intersect" type="double" value="172.5"/>
 	<param name="smallp_ratio" type="double" value="1.2"/>
-	<param name="point_filter_num" type="int" value="4"/>
+	<param name="point_filter_num" type="int" value="3"/>
   <node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/>
 
   <node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen">
diff --git a/src/feature_extract.cpp b/src/feature_extract.cpp
index a589a3e..62f1295 100644
--- a/src/feature_extract.cpp
+++ b/src/feature_extract.cpp
@@ -792,23 +792,23 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
       
       if(j == uint(last_surface+point_filter_num-1))
       {
-        for(uint k=last_surface; k<=j; k++)
-        {
-          PointType ap;
-          ap.x = pl[k].x;
-          ap.y = pl[k].y;
-          ap.z = pl[k].z;
-          ap.curvature = pl[k].curvature;
-          ap.intensity = pl[k].intensity;
-          pl_surf.push_back(ap);
-        }
+        // for(uint k=last_surface; k<=j; k++)
+        // {
+        //   PointType ap;
+        //   ap.x = pl[k].x;
+        //   ap.y = pl[k].y;
+        //   ap.z = pl[k].z;
+        //   ap.curvature = pl[k].curvature;
+        //   ap.intensity = pl[k].intensity;
+        //   pl_surf.push_back(ap);
+        // }
 
-        // PointType ap;
-        // ap.x = pl[last_surface].x;
-        // ap.y = pl[last_surface].y;
-        // ap.z = pl[last_surface].z;
-        // ap.curvature += pl[last_surface].curvature;
-        // pl_surf.push_back(ap);
+        PointType ap;
+        ap.x = pl[j].x;
+        ap.y = pl[j].y;
+        ap.z = pl[j].z;
+        ap.curvature += pl[j].curvature;
+        pl_surf.push_back(ap);
 
         last_surface = -1;
       }
diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp
index 7d8571e..2ef885a 100644
--- a/src/laserMapping.cpp
+++ b/src/laserMapping.cpp
@@ -1040,16 +1040,19 @@ int main(int argc, char** argv)
                         // state += solution;
 
                         auto vec = state_propagat - state;
-                        solution = K * (meas_vec - Hsub * vec.block<6,1>(0,0));
-                        state = state_propagat + solution;
+                        // solution = K * (meas_vec - Hsub * vec.block<6,1>(0,0));
+                        // state = state_propagat + solution;
+
+                        solution = K * meas_vec + vec - K * Hsub * vec.block<6,1>(0,0);
+                        state += solution;
 
                         rot_add = solution.block<3,1>(0,0);
                         t_add   = solution.block<3,1>(3,0);
 
                         flg_EKF_converged = false;
 
-                        if (((rot_add.norm() * 57.3 - deltaR) < 0.01) \
-                           && ((t_add.norm() * 100 - deltaT) < 0.015))
+                        if ((rot_add.norm() * 57.3 < 0.01) \
+                           && (t_add.norm() * 100 < 0.015))
                         {
                             flg_EKF_converged = true;
                         }