Pose estimate drifts from vision input


I was successfully using mocap for pose estimation until recently. I just updated the firmware on my pixhawk through QGC 3.0.0 (tried it twice). Updated Mavros to 0.17.4 on ROS Indigo.

I’m publishing to vision topic /mavros/vision_pose/pose but echoing the /mavros/local_position/pose is giving me different values. I can see that moving the vehicle introduces changes to those values so, my guess is that there are some unwanted filters being applied over my values.

Example of values being sent by a code that was working fine so far:

-0.17  1.53  0.16  0.01  0.00  0.65 # x, y, z, roll, pitch, yaw

Mavros node shows the following which interestingly has swapped the x and y value.:

[ INFO] [1449848270.607429699]: FCU: [lpe] vision position init:  1.53 -0.17 -0.16 m s

mavros/vision_pose/pose sample:

  seq: 1311
    secs: 1449848016
    nsecs: 782612416
  frame_id: fcu
    x: 0.00307422527112
    y: 0.00486058322713
    z: -1.87768530846
    x: -0.00456124526988
    y: -0.0241043553168
    z: -0.956350739566
    w: -0.291189694234

I’ve tried to set the values for some parameters found in https://pixhawk.org/firmware/parameters but received the following errors:

[ERROR] [1449848580.090083069]: PR: Unknown parameter to get: INAV_W_Z_VIS_P
[ERROR] [1449848604.929257906]: PR: Unknown parameter to get: INAV_W_XY_VIS_P
[ERROR] [1449848629.545696700]: PR: Unknown parameter to get: INAV_W_MOC_P
[ERROR] [1449848668.490732996]: PR: Unknown parameter to get: INAV_W_Z_BARO
[ERROR] [1449849711.621053162]: PR: Unknown parameter to get: CBRK_NO_VISION

Trying some other random parameters:

$ rosservice call /mavros/param/get ATT_EXT_HDG_M
success: True
  integer: 1
  real: 0.0
$ rosservice call /mavros/param/get LPE_VIS_Z 
success: True
  integer: 0
  real: 0.5
$ rosservice call /mavros/param/get LPE_VIS_XY
success: True
  integer: 0
  real: 0.5

Looking at parameters through QGC shows only EKF2_REC_PRL in EKF2 section and only INAV_LIDAR_ERR in Position Estimator INAV section.

Any help is appreciated.

I also tried this with another Pixhawk. Updated its firmware to current stable and many INAV and EKF2 parameters are missing. Here is the firmware update output from QGC:

QGroundControl can upgrade the firmware on Pixhawk devices, SiK Radios and PX4 Flow Smart Cameras.
All QGroundControl connections to vehicles must be disconnected prior to firmware upgrade.
Please unplug your Pixhawk and/or Radio from USB.
Plug in your device via USB to start firmware upgrade.
Plug in your device via USB to start firmware upgrade.
Found device: Pixhawk
Connected to bootloader:
Version: 4
Board ID: 9
Flash size: 2080768
Downloading firmware...
From: http://px4-travis.s3.amazonaws.com/Firmware/stable/px4fmu-v2_default.px4
Download complete
Succesfully decompressed parameter_xml
Succesfully decompressed airframe_xml
Unable to open airframe meta data file /home/naslab/.config/QGroundControl.org/PX4AirframeFactMetaData.xml for writing, error: Permission denied
Succesfully decompressed image
Erasing previous program...
Erase complete
Programming new version...
Program complete
Verifying program...
Verify complete
Upgrade complete