Incorrect velocity estimates when using VICON for MOCAP position.

[beaucolley]

Hi there

I'm attempting to fly in Off Board mode with position feedback from a VICON system. I'm running the LPE PX4 branch with a raspberry pi 3 companion computer running MAVROS, VRPN_CLIENT_NODE and another ROS Node sending a constant set point of (0,0,1.5).

With the default value for LPE_VIC_P we get highly unstable flight, log attached. The problem seems to be the velocity estimated, which looks to be following the position estimate, resulting in large estimated velocities as the drone moves away from the origin. Log attached

Reducing the LPE_VIC_P down to 0.0005 allows the drone to stabilize somewhat but it still oscillates around the 1.5 meter set point, log attached.

To me it seems for some reason the LPE is using the position from the VICON to update the velocity. With default weighting the velocity estimate is so large the drone over corrects and crashes drone crashes. With reduced weighting the position error is able to overcome the effect of incorrect velocity but it cannot track the set point well because at any non origin position the velocity estimate is pushed away from zero.

The VRPN node is publishing geometry_msgs/PoseStamped.msg which contains geometry_msgs/Point position geometry_msgs/Quaternion orientation

[LorenzMeier]

Thanks for sharing the logfile - as of now we're only officially supporting the EKF2 state estimator. Could you re-run your experiment with it and report back?

[beaucolley]

Sure can do that now, is that going to be the current master branch?

[beaucolley]

Thats working perfectly now. Thanks! https://review.px4.io/plot_app?log=aa944e78-073f-49fd-81ea-734d19aa0216 I would say there is defiantly a nasty bug in the LPE branch.