local position unstable (ekf2 with vision fusion)

[chungshan]

Hi, I use ekf2 vision fusion( with motion captures system)
Sometimes, local_position/pose is correct.

But sometimes, it's uncorrect. The pose will be like this:

pose: 
  position: 
    x: -1.52471171309e-17
    y: 3.06921519898e-33
    z: 0.124502159655
  orientation: 
    x: 0.00153229214155
    y: 0.0453438805825
    z: 0.0199263977732
    w: -0.998771528692

I might reboot the px4 several times to get a correct value of local_position/pose.
What's the problem about this?
My motion capture data streaming rate is at 120Hz.
And I see that the receiving data stream rate of vision_pose/pose is only at 10Hz(in QGroundControl Analyze)
Is that the problem about the large difference between the above two data stream rate(120Hz&10Hz)?
Thanks!

flight log file:
https://logs.px4.io/plot_app?log=84096440-6f9c-4610-a5d3-8c3bc592d78e
This log file is the successful flight, because if I see that the local_position/pose is not correct, I won't let the drone take off.

ROSBag file:
https://drive.google.com/file/d/1JmP7I9GZqVxmh8uSSukeY--itedRVQMn/view?usp=sharing
the respective topic: /drone3/mavros/local_position/pose
You can see that the pose is very strange. The postion of x, y is very close to zero.
like this:

header: 
  seq: 4414
  stamp: 
    secs: 1533536785
    nsecs: 942267648
  frame_id: "map"
pose: 
  position: 
    x: -1.72637007448e-17
    y: 3.47514958151e-33
    z: 0.140968814492
  orientation: 
    x: -0.0248491935548
    y: 0.0221236218929
    z: -0.482939474001
    w: -0.875021532601

However at very few timestamps, it shows the correct position:

header: 
  seq: 4413
  stamp: 
    secs: 1533536785
    nsecs: 934267648
  frame_id: "map"
pose: 
  position: 
    x: 0.791218042374
    y: -1.84485912323
    z: 0.140622586012
  orientation: 
    x: -0.0249051236576
    y: 0.0221258913196
    z: -0.482943182924
    w: -0.875017823679

[EdXian]

Hi all,
I have the same problem as the following figure.

[ChristophTobler]

Can you provide a log? A ROSbag with the respective topic would also help

[AlexisTM]

The "very strange very close to 0 position" is an actual zero that went through multiple conversions and finishes being something.
It happens when the position is lost/EKF Filter fails.

It could be due to:

• driver: Update frequency
• EKF2: Yaw not correlated and not fused
• WiFi: latency spikes
• ...

[chungshan]

So, it's probably because of low frequency(50hz) of the IMU in px4?
And what "Yaw not correlated " means?
Does it mean that the yaw of vision pose doesn't align with the yaw from IMU?

[AlexisTM]

The MAVLink frequency of the IMU is 50Hz, it's higher internally.

Yaw not correlated means not aligned with the IMU yaw.

[priseborough]

The ekf2 module is unable to accept observations faster than set by EKF2_MIN_OBS_DT without risk of data in the buffer being overwritten before it falls behind the EKF's fusion time horizon. This results in the data not being used. Also because the EKF runs on a 8msec internal update rate for the covariance prediction and observation fusion cycle (IMU data is used for state prediction at the full rate), any observation faster than about 100Hz can run into problems due to timing jitter regardless of the setting of EKF2_MIN_OBS_DT

[priseborough]

The estimator status and ekf2 innovation data is not being logged fast enough to determine if data is being lost.

[priseborough]

@virtualclone Are you able to get full rate logging data? The data rate looks like it is being limited in the setExtVisionData function, but timing jitter and time offsets can still cause problems. Can you drop the rate you are sending data and also get a log of the innovation messages a the full published rate?

[diyajaddu]

Same with me #10146

[diyajaddu]

I tried even publish the vision pose at 80 Hz and 10 Hz and didn't make any difference with me,
The EKF keep restarting after a little bit of time

[diyajaddu]

I think this #10310 is connected to this issue too.

[tiralonghipol]

Any improvements on this?
following

[chungshan]

@priseborough I drop the rate from 50Hz to 10Hz (vision_pose), but the problem is still happened.
Another question, how can I get a log of the innovation messages a the full published rate?

[priseborough]

@virtualclone Remove the specified time step from this line and it should log every message:

https://github.com/PX4/Firmware/blob/master/src/modules/logger/logger.cpp#L616

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[ssingh19]

Is there any solution to this? I'm getting the same issue.

[priseborough]

Logs please with any reported problem. Logs need to be from boot and without dropouts that prevent ekf2 replay .

[YCH188]

@virtualclone @EdXian @ssingh19 Have you solve the problem? I am also using offboard mode to control Pixhawk. When I use rostopic echo /mavros/local_position/pose, the x,y coordinate is close to 0, even I move the drone. But the z coordinate changes as I moved the done. What is the sloution???

[mhkabir]

Closing this as no one got back with logs. Please open a new issue with logs if you still have issues.

[ssingh19]

@YCH188 I found the issues to be here: https://github.com/PX4/Firmware/blob/82aa24adfca29321cfd1209e287eab6c2b16780e/src/modules/mavlink/mavlink_receiver.cpp#L1239

https://github.com/PX4/Firmware/blob/82aa24adfca29321cfd1209e287eab6c2b16780e/src/modules/mavlink/mavlink_receiver.cpp#L1251

This is where it attempts to sync the vision message timestamp with PX4 clock. As a temporary workaround (which seems to be working reliably - we are able to do pretty aggressive offboard control), this is our fix.

You can optionally also incorporate your estimate of the delay between motion capture and the px4 board like this.

Note that this last fix is hacky and should be done using the EKF2_EV_DELAY parameter, but you get the idea.

[tiralonghipol]

@virtualclone @kawasakiDaichi @YCH188
I solved this problem on the ros side, px4 works perfectly
assuming that your position estimator is ok, try to check the timestamps: I had an almost 500ms difference between my (rovio) pose estimator and the /mavros/local_position/pose
forcing a ros time reference worked for me and the results are amazing!

https://www.youtube.com/watch?v=U-ro2bNXKRk&t=11s

(px4 1.8.2 stable release)
Hope this helps

[Seanmatthews]

@tiralonghipol Can you expand on how you solved this problem? Did you use the method that @ssingh19 described?