dump the long distance marker result, correct covariance matrix in kalman

catkin_ws/src/asclinic_pkg/src/Kalman.cpp

@@ -137,10 +137,10 @@ void ImageAnswer(const asclinic_pkg::position msg){
                     y,
                     phi;
     MatrixXd new_pose = odo_position + K*(z_position - odo_position);
-    if (std::isnan(x) || std::isnan(y) || std::isnan(phi)) {
-        x = odo_position(1);
-        y = odo_position(2);
-        phi = odo_position(3);
+    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{
@@ -187,13 +187,13 @@ int main(int argc, char **argv) {
     time_now = ros::Time::now();
     last_time = ros::Time::now();
     
-    pose_covariance <<  1, 1, 1,
-                        1, 1, 1,
-                        1, 1, 1;
+    pose_covariance <<  0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0;
 
-    z_cov <<  0.07246, -0.05582, -0.01839,
-              -0.05582, 0.32237, 0.46697,
-              -0.01839, 0.46697, 0.20327;
+    z_cov <<  0.07246, 0.05582, 0.01839,
+              0.05582, 0.32237, 0.46697,
+              0.01839, 0.46697, 0.20327;
 
     m_subscriber = nh.subscribe("/asc/encoder_counts", 1, answer);
     image_subscriber = nh.subscribe("/image_estimation", 1, ImageAnswer);

catkin_ws/src/asclinic_pkg/src/image.py

@@ -50,10 +50,8 @@ def answer_callback(msg, markers, publisher):
     x_avg = 0
     y_avg = 0
     phi_avg = 0
-    matrix_ctow = np.array([[0, 0, 1],
-                            [-1, 0, 0],
-                            [0, -1, 0]])
-    
+    mini_d = 1000
+    valid_number = 0
     for fiducial_marker in msg.markers:
         id = fiducial_marker.id
         for marker in markers:
@@ -64,7 +62,10 @@ def answer_callback(msg, markers, publisher):
                 rx = fiducial_marker.rvec[0]
                 ry = fiducial_marker.rvec[1]
                 rz = fiducial_marker.rvec[2]
-                
+                now_d = math.sqrt(tx**2 + tz**2)
+                if now_d > 4:
+                    continue
+                valid_number+=1
                 # Compute rotation matrix
                 npary = np.array([rx, ry, rz])
                 rospy.loginfo("id is:" + str(marker.id))
@@ -86,16 +87,22 @@ def answer_callback(msg, markers, publisher):
                 #rospy.loginfo("x_mtoc_world" + str(position_inertia_frame[0]) + " z_mtoc_world " + str(position_inertia_frame[1]))
                 x = position_inertia_frame[0] + marker.position[0]
                 y = position_inertia_frame[1] + marker.position[1]
-                rospy.loginfo("x" + str(x) + " Y " + str(y) + " phi " + str(marker.get_angle() + ry))
 
+                phi_now = marker.get_angle() + ry
+                phi_now = normalize_angle(phi_now)
+                #if phi_now>1.8*math.pi:
+                    #phi_now = -(2*math.pi - phi_now)
+                rospy.loginfo("x" + str(x) + " Y " + str(y) + " phi " + phi_now)
 
-                phi_avg += marker.get_angle() + ry
+                if now_d< mini_d:
+                    phi_avg == phi_now
+                
                 x_avg += x
                 y_avg += y
         
-    x_avg /= num_markers
-    y_avg /= num_markers
-    phi_avg /= num_markers
+    x_avg /= valid_number
+    y_avg /= valid_number
+    phi_avg = phi_avg
     phi_avg = normalize_angle(phi_avg)
 
     if x_avg != 0 or y_avg != 0 or phi_avg != 0: