MOCAP system with PX4. VISION_POSITION_ESTIMATE work but LOCAL_POSITION_NED doesn't

PX4 Autopilot
1

Hi,

Thanks for reading this.

I am setting up the mocap system with PX4 for indoor flight without GPS. Now I wish to pass my mocap coordinates directly to the PX4 local position. However, I only see the VISION_POSITION_ESTIMATE work with my desired data, while LOCAL_POSITION_NED still only use the imu data.

Here are the things I did:

  1. change follow parameters for EKF2:
    image
    image819×404 76 KB
  2. write a ros2 code, pass the mocap data to /mavros/vision_pose/pose topic. My mocap data is in PoseStamped message.
  3. I did the transformation for frame vision_world and map. Basically just to make the vision_world locate at the same place and same orientation as the map frame.

Then I run the code. But in QGC, I could find following results:

Correct VISION_POSITION_ESTIMATION:

image
image843×377 35.4 KB

Wrong LOCAL_POSITION_NED:

image
image843×377 28.1 KB

ESTIMATOR_STATUS:

image
image843×377 39.2 KB

For checking uORB topic:

listener vehicle_visual_odometry

TOPIC: vehicle_visual_odometry
 vehicle_visual_odometry
    timestamp: 1897628745 (0.002373 seconds ago)
    timestamp_sample: 1897616192 (12553 us before timestamp)
    position: [-0.02321, -0.01561, -0.05697]
    q: [0.99980, -0.01720, -0.00253, -0.00962] (Roll: -2.0 deg, Pitch: -0.3 deg, Yaw: -1.1 deg)
    velocity: [-0.00018, -0.00022, -0.00017]
    angular_velocity: [0.00037, -0.00200, 0.00117]
    position_variance: [0.00000, 0.00000, 0.00000]
    orientation_variance: [0.00000, 0.00000, 0.00000]
    velocity_variance: [0.00002, 0.00003, 0.00001]
    pose_frame: 2
    velocity_frame: 3
    reset_counter: 1
    quality: 34

TOPIC: estimator_status
 estimator_status
    timestamp: 1936384413 (0.002673 seconds ago)
    timestamp_sample: 1936293115 (91298 us before timestamp)
    control_mode_flags: 67138049 (0b100'0000'0000'0111'0010'0000'0001)
    output_tracking_error: [0.00001, 0.00019, 0.00019]
    filter_fault_flags: 0
    pos_horiz_accuracy: 0.03818
    pos_vert_accuracy: 0.02782
    mag_test_ratio: 0.00032
    vel_test_ratio: nan
    pos_test_ratio: 0.00016
    hgt_test_ratio: 0.02116
    tas_test_ratio: 0.00000
    hagl_test_ratio: 0.00000
    beta_test_ratio: 0.00000
    time_slip: -0.05459
    accel_device_id: 2490378 (Type: 0x26, SPI:1 (0x00))
    gyro_device_id: 2490378 (Type: 0x26, SPI:1 (0x00))
    baro_device_id: 12018473 (Type: 0xB7, I2C:5 (0x63))
    mag_device_id: 0 (Type: 0x00, UNKNOWN:0 (0x00))
    gps_check_fail_flags: 0
    innovation_check_flags: 0
    solution_status_flags: 830 (0b11'0011'1110)
    reset_count_vel_ne: 4
    reset_count_vel_d: 1
    reset_count_pos_ne: 98
    reset_count_pod_d: 2
    reset_count_quat: 186
    pre_flt_fail_innov_heading: False
    pre_flt_fail_innov_vel_horiz: False
    pre_flt_fail_innov_vel_vert: False
    pre_flt_fail_innov_height: False
    pre_flt_fail_mag_field_disturbed: False
    health_flags: 0
    timeout_flags: 0

I feel it is really strange that the vehicle_visual_odometry topic echoes different data as the VISION_POSITION_ESTIMATION, but similar to the LOCAL_POSITION_NED. And I am really confused about why I could not pass the direct data to local_position_ned.

I will be really grateful if someone could help!

Best wishes

2

That’s odd. Let me ping @bresch, maybe he knows what’s going on.

3

That’s indeed quit strange. The position in vehicle_visual_odometry should just be a copy of VISION_POSITION_ESTIMATION.
But it looks like vehicle_visual_odometry comes from the ODOMETRY mavlink message since pose_frame: 2 (pos_frame can only be set to 1 if the data comes from VISION_POSITION_ESTIMATE, see here).
Are you maybe sending both messages?
Do you have a log to share?