Hello,
I’ve got some problems with the EKF2 using high precision motion capture data as vision input.
I configured everything according to the [PX4 Dev Guide] concerning [Discuss 9154], [Issue 7408], [Issue 8520], [Issue 10146], [Issue 10172], and [Issue 10310].
So if anyone has any further input, be it another issue on GitHub or one’s solution to this, I’d really appreciate it.
What I tried:
- Lowering the rate of pose information from the mocap system.
- Trusting the mocap system more than normal (lowering the noise params and raising the gate size).
- Not trusting the IMU/Gyro data (raising the noise params).
What I didn’t (yet) try:
- Aligning the IMU center to the center of the mocap data via
EKF2_IMU_POS_X
,EKF2_IMU_POS_Y
, andEKF2_IMU_POS_Z
.
My results:
Note: plots are at the end
- While the position (
/mavros/local_position/pose
) generally follows the mocap pose, a lot of times, it overshoots. - Often it also starts oscillating while the pose is changed only a little, mostly to my inability to really fix an axis by hand.
- In general, the results are not stable as it seems that the EKF still does not trust the pose information from the mocap.
My setup:
- OptiTrack System with VRPN-client for ROS, noise is < 1 mm
- PX4 v1.8.2dev running_ on Aerocore 2 (build based on [v1.8.2 tag])
- Data streaming to EKF2 via remap of
/mavros/local_position/pose
at constant 50 Hz. (Data is coming from the VRPN at 200 Hz and throttled to 50 Hz via a [topic_tools/throttle]-node; confirmed the 50 Hz viarostopic hz
.) - Moving my quadrotor by hand, trying to only move it along one axis and fixing the other two (completely fixing its orientation).
My params:
-
EKF2_ACC_B_NOISE
: 0.01 -
EKF2_ACC_NOISE
: 1.00 -
EKF2_AID_MASK
: 24 -
EKF2_EVA_NOISE
: 0.01 -
EKF2_EVP_NOISE
: 0.01 -
EKF2_EV_DELAY
: 10.0 -
EKF2_EV_GATE
: 10 -
EKF2_GYR_B_NOISE
: 0.01 -
EKF2_GYR_NOISE
: 0.1 -
EKF2_HGT_MODE
: Vision
Plots: