still, on kalman no aruco test

catkin_ws/src/asclinic_pkg/src/Kalman.cpp

@@ -99,7 +99,7 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
     float angular_velocity = dPhi / time_float;
 
     // Log current position and angular velocity
-    ROS_INFO_STREAM("odometry Position - X:" << x << ", Y:" << y << ", Phi:" << phi << ", Angular Velocity:" << angular_velocity);
+    //ROS_INFO_STREAM("odometry Position - X:" << x << ", Y:" << y << ", Phi:" << phi << ", Angular Velocity:" << angular_velocity);
 
     // Publish position
     asclinic_pkg::position position_msg;
@@ -207,7 +207,7 @@ void update_authority(const std_msgs::UInt32& msg) {
             kalman_output.publish(Umsg);
             ROS_INFO_STREAM("go with no update");
         }
-        last_time = current_time;
+        
         ros::spinOnce();
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
@@ -227,9 +227,9 @@ int main(int argc, char **argv) {
                         0.5, 0.5, 0.5,
                         0.5, 0.5, 0.5;
 
-    z_cov <<  5, 5, 5,
-              5, 5, 5,
-              5, 5, 5;
+    z_cov <<  1, 1, 1,
+              1, 1, 1,
+              1, 1, 1;
 
     m_subscriber = nh.subscribe("/asc/encoder_counts", 1, answer);
     image_subscriber = nh.subscribe("/image_estimation", 1, ImageAnswer);