LPE vision position is not working - needs tuning?

I am trying to get Position Hold from Vision based odometry (ROVIO running on odroid).
I run test flight today and uploaded the logs. I am passing the position information via mavros plugin.
As far as I can see the visual odometry works fine, I do not have access to mocap system but just flying back and forward, turning circles etc “position looks ok” on the screen.
Example logs are here ROVIO Position Hold Test - logs.uaventure.com
I am running PX4 based on master from 8Nov2016 (643ccd66b623fd2b1812054bee85ee3d0b4b2f06)
I do not have GPS installed at all, the global position in log is arbitrary, this is indoor flight.
The UAV (DJI F450 frame) behaves quite well in stabilized and acro flight with default PID settings.
Should the default LPE settings works out of the box or it require tuning?
I saw find_lpe_gains function in px4tools python scripts but as far as I understand it deals with noise energy not gains per say. Anyway I can not use it as it depends on GPS.

@mhkabir can I call on you again for help, it seems everywhere I look your name is there. To me it looks like the LPSP follows the LPOS which does not make sense, it should fight against it? I assume LPSP is set point for local position?

Does anyone have example of good position hold logs based on vision, I am interesting how the LPSP should behave?

You might need some controller tuning. It’s hard to make out what’s going on in the logs, because you keep switching between poshold and althold. The setpoint will not remain fixed if you touch the roll/pitch sticks.

Maybe @tumbili can help debug this?

I do have some logs somewhere from back when I used to fuse Rovio into LPE. Currently I just forward our VIO estimate directly to the controllers. Drop me an email at kabir@uasys.io and I’ll see if I can find you some.

I can keep it in position hold mode only for one second or so after that it is trough the wall…
When I toggle it to position hold it starts drifting immediately.

@mhkabir I was wondering if LPE is required, is there standard way to provide position straight to controller or you had to modify the firmware?

Hmm. If the response is completely wrong, are you sure that the Rovio estimate is being sent in the correct coordinate frame? I would investigate that first. How are you sending the estimates? Also - have you enabled vision yaw fusion?

If you point the quad north and move forward, X should increase. Y should increase on moving right. Verify that just in case.

Also, with respect to direct forwarding to the controllers, you shouldn’t need LPE at all. I am working on a upstreamable external estimator interface, which I can share if you are willing to test experimental code which might not be ready yet.

I would love some help to make sure that it is ready for primetime so that everyone can benefit from it.

I am using the listen to transform option in mavros vision position plugin. Where frame id is world and child frame id is imu.
I will confirm the directions tomorrow.
Since ROVIO will start the local position at arbitrary position/attitude (I am using defaults 0,0,0) does that mean I have to initialise ROVIO when UAV points north?
It does not make sense, I would assume that first Vision position message would create local origin diff and use that in the future…

Testing your code sound good, opportunity for me to learn and help. Not mention it sound useful to my case.
I will ping you when we come back from the mine visit end of next week. Right now we have AI conference here and I am trying to prepare for the mine visit. Frantic time. It is past midnight now…
Thanks for your help.

One more thing re vision yaw fusion, I have the ATT_EXT_HDG_M = 1.0 and I assume the CBRK_NO_VISION does not exists anymore…

I have been doing more googling and it appears there is difference between ROS coordinates system and PX4 coordinate system. The vision yaw fusion fights between compass and vision heading, if I disable it I can see 90deg difference.
My visual pose has arbitrary origin, do I have to align it with North (NED) before sending it to PX4?

Mavros internally converts the ROS-PX4 frame, but the primary issue is that the mavros frame conversions are incorrect. I am currently using hand-crafted rotations to work around this.

With regards to the new interface, here is the WIP branch : https://github.com/mhkabir/Firmware/tree/uasys-devel I will let you know when it is ready to go. I am cleaning up the mavros side too, with correct frame rotations for Rovio.

My visual pose has arbitrary origin, do I have to align it with North (NED) before sending it to PX4?

Not if you set ATT_EXT_HDG_M to 2. (fuse vision heading)

@mhkabir @Maciej_Matuszak
I am trying to do Position Hold from Vision and meet the same problem. I use pixhawk. My drone turns circles every time I test it as well. I use /mavros/vision_position/pose to give the target position to the drone(not sure the exact topic name because I’m not around my drone right now) . And I listen to the feedback of the drone, the topic name is /mavros/local_position/pose. I found that the pose of the feedback is all about NED framework, totally nothing about external vision framework.
By the way I use “make px4fmu-v2_default” to make the project, I’m not sure if my way to build the project is right. I tried both ekf2 and LPE mode but they don’t make sense. I think maybe because I use a wrong way to build it. I am new to PX4 so now I have no idea what to do…Can you guys help me…

@zzzzzq sorry had not looked at this. Maybe @mhkabir can share the " hand-crafted rotations"…

Silly season is over lets go back to work :smile:
I had a look at the changes, once I got my head back into it, it is stright forward. I have couple of questins?

  1. There are 2 ways to send the vision estimates either via MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE or MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV + MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV. What’s the application of one v.s another?

  2. When MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV is processed in mavlink receiver it adds the global reference, this is not done when processing MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE. Is that by design or is it omission?

  3. I assume you use the MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN to set origin in case there is no GPS at all, i.e. to visualise the position in GCS?

  4. How you going with mavros side changes?


Please look here : https://github.com/PX4/Firmware/pull/6074

This is upstream-able version of the interface, and I have cut out a lot of crud and also the GPS/global origin stuff. It will come in on a new PR or in a couple of days time.

The mavros side is 99% ready, but with our custom plugin. I will push it tomorrow.