change kalman log info, image, angle fix

catkin_ws/src/asclinic_pkg/src/Kalman.cpp

@@ -115,7 +115,7 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
 
 
 
-void ImageAnswer(const asclinic_pkg::position msg){
+void ImageAnswer(const asclinic_pkg::position& msg) {
     ros::Time current_time = ros::Time::now();
 
     // Check if elapsed time is less than 0.5 seconds
@@ -126,43 +126,45 @@ void ImageAnswer(const asclinic_pkg::position msg){
     // Update the last time to the current time
     last_time = current_time;
 
-    MatrixXd z_position(3,1);
+    MatrixXd z_position(3, 1);
     z_position << msg.x,
                   msg.y,
                   msg.phi;
 
-    MatrixXd K = pose_covariance*(pose_covariance + z_cov).inverse();
-    MatrixXd odo_position(3,1);
+    MatrixXd K = pose_covariance * (pose_covariance + z_cov).inverse();
+    MatrixXd odo_position(3, 1);
     odo_position << x,
                     y,
                     phi;
-    MatrixXd new_pose = odo_position + K*(z_position - odo_position);
+
+    ROS_INFO_STREAM("odo_position: " << odo_position);
+    ROS_INFO_STREAM("z_position: " << z_position);
+    ROS_INFO_STREAM("K: " << K);
+
+    MatrixXd new_pose = odo_position + K * (z_position - odo_position);
+
+    ROS_INFO_STREAM("new_pose: " << new_pose);
+
     if (std::isnan(new_pose(0)) || std::isnan(new_pose(1)) || std::isnan(new_pose(2))) {
         x = odo_position(0);
         y = odo_position(1);
         phi = odo_position(2);
-        ROS_INFO_STREAM("fail!!!!!!!!!!!!!!!!!!!!!!!!!!");
-    }
-    else{
+        ROS_WARN("Fail: new_pose contains NaN values");
+    } else {
         x = new_pose(0);
         y = new_pose(1);
         phi = new_pose(2);
-        pose_covariance = pose_covariance - K*(pose_covariance + z_cov)*K.inverse();
-
+        pose_covariance = pose_covariance - K * (pose_covariance + z_cov) * K.transpose();
+        ROS_INFO_STREAM("Updated pose: X:" << x << ", Y:" << y << ", Phi:" << phi);
     }
 
     phi = normalize_angle(phi);
+    ROS_INFO_STREAM("Normalized Phi: " << phi);
 
-    // Publish position
-    //asclinic_pkg::position position_msg;
-    //position_msg.x = x;
-    //position_msg.y = y;
-    //position_msg.phi = phi;
-    //m_publisher.publish(position_msg); 
-    ROS_INFO_STREAM("image updated");
+    ROS_INFO("Image updated");
+}
 
 
-}
 void pwmAnswer(const asclinic_pkg::LeftRightFloat32& msg){
     if(msg.left<0){
         left_p = -1;
@@ -187,9 +189,9 @@ int main(int argc, char **argv) {
     time_now = ros::Time::now();
     last_time = ros::Time::now();
     
-    pose_covariance <<  0, 0, 0,
-                        0, 0, 0,
-                        0, 0, 0;
+    pose_covariance <<  1, 1, 1,
+                        1, 1, 1,
+                        1, 1, 1;
 
     z_cov <<  0.07246, 0.05582, 0.01839,
               0.05582, 0.32237, 0.46697,

catkin_ws/src/asclinic_pkg/src/image.py

@@ -63,7 +63,7 @@ def answer_callback(msg, markers, publisher):
                 ry = fiducial_marker.rvec[1]
                 rz = fiducial_marker.rvec[2]
                 now_d = math.sqrt(tx**2 + tz**2)
-                if now_d > 4:
+                if now_d > 2.5:
                     continue
                 valid_number+=1
                 # Compute rotation matrix
@@ -96,6 +96,7 @@ def answer_callback(msg, markers, publisher):
 
                 if now_d< mini_d:
                     phi_avg == phi_now
+                    mini_d = now_d
                 
                 x_avg += x
                 y_avg += y