EK3 offset on vertical position with vicon measurement


I’m using arducopter 4.1 for indoor flying with vicon system. I used the method here to send vicon information via mavproxy to the pixhawk as if the location and velocity are provided from GPS.

My parameters are set as below:

To avoid the pixhawk using barometer for z position estimate, I banned the barometer using the following parameters:

The procedure is that we placed the drone on the ground near the vicon’s origin and then plugged in the battery. We then turned on mavproxy and send vicon information using the vicon module. Then we handheld the drone to three random locations and read the vicon’s data. We later compared the recorded data with ardupilot log’s XKF1 (specifically, PN, PE, PD).

Attached is the data we collect for this trial and I show the picture of the offset.
log_33_2021-12-14-17-30-40.bin (1008 KB)

The plot shows that the vicon data were taken by pixhawk near 30 s (I think it’s at the time of ekf_yaw-reset). But before this happened, the XKF1.PD seemed drifting, which could be caused by integrating noisy accelerometer reading. What I don’t understand is that the offset on XKF1.PD is not corrected after the event set_home (around 40 s). The vicon data is very reliable and I think the XKF.PD should be reset to very close to vicon’s reading which is 0.022 m (on the ground). The parameter EK3_ACC_P_NSE has been set to 1 to maximize the trust on vicon data (I even tried 50 but it didn’t seem to solve the problem). What’s worse is that this offset has been kept later. We’re doing some trajectory following tasks that have high requirements on the EKF fused position. But with the offset that changes every time (we had a couple of more tests and the offsets therein were all different), we can not reliably perform our trajectories.

We also had tests that simply reboot the vehicle when vicon data were streamed through mavproxy. In this way, once the pixhawk finished rebooting, it can immediately use the vicon data. Below is the log I had. The procedure is similar to the above: the drone would sit on the ground for a while and then be hand-held to three different locations.
log_34_2021-12-14-17-31-50.bin (932 KB)

Again, you can see the drift on XKF1.PD before set_home and the resulting offset is kept since then.

So I wonder what’s not correctly configured on my side or is it revealing some bug in the EKF3? Any feedback would be appreciated! Thank you!

Just to give an update on the issue. We finally resolved the initial offset in EKF by switching to the localization packet in the form of T265 from fake GPS.