EKF2 and LPE Tuning and Consistancy
This section gives detail on how to analyze possible delays and EKF performance.
EKF2 Tuning
As discussed previously, certain delay parameters must be tuned in order to have the EKF2 working properly. This parameter is defined as EKF2_EV_DELAY which is the Vision Position Estimator delay relative to IMU measurements. The PX4 website describes its purpose:
Or in other words, it is the difference between the vision system timestamp and the "actual" capture time that would have been recorded by the IMU clock (the "base clock" for EKF2).
Technically this can be set to 0 if there is correct timestamping (not just arrival time) and timesync (e.g NTP) between MoCap and (for example) ROS computers. In reality, this needs some empirical tuning since delays in the entire MoCap->PX4 chain are very setup-specific. It is rare that a system is setup with an entirely synchronized chain!
It's important to make sure that EKF2 estimator provides accurate enough estimates of the states for your flight controller to perform well. One way to analyze is through the log files. These methods are presented in this link.
The default log file format in PX4 is Ulog. Usually, the default setting, is the logs start after arming the vehicle and stopped after disarm i.e. SDLOG_MODE parameter is set to 0. To enable logging of EV rates set bit 7 (Computer Vision and Avoidance) of SDLOG_PROFILE.
Setup the wifi connection between the Clover and OQroundControl (QGC) using TCP bridge so the flight logs can be downloaded for data analysis.
Use QGC to download a Ulog file you wish to analyze.
Download the FlightPlot to open your logs and analyze the topics of interest.
Plot the fields vehicle_attitude_0.q[0], and vehicle_vision_attitude_0.q[0]. This can be used to compare the external data vs. onboard estimate. Refer to the PX4 website to determine an approximate delay parameter from these plots.
While step 4. above gives a rough estimate of the delay, another method is plotting vehicle_visual_odometry_0.timestamp and vehicle_visual_odometry_0.timestamp_sample topics as shown:
The topic (timestamp) is the time PX4 received the message and (timestamp_sample) is the time you put on the sent message. The figure above gives about a 25 ms (the y-axis time is in microseconds) and therefore it is a reasonable delay that can be dealt with.
With the delay parameter set well enough for good performance, it can be finely tuned by checking the EKF2 innovations as described:
When a MoCap measurement arrives, the EKF calculates the difference between the predicted position and the position from the MoCap. This difference is called an ‘Innovation’.
Further tune the delay parameter by plotting the fields estimator_innovations_0.ev_pos_innov[3], ekf2_innovations_0.vel_pos_innov[4], and ekf2_innovations_0.vel_pos_innov[5]. Those are the innovations on the x/y/z position estimates reported by the EKF2. They should very small values, (ideally zero!), see the picture below for reasonable values. If those values are large, then EKF2 is not providing accurate estimation. This is most likely because of the inconsistency of timestamps of the fused measurements. For that, you will need to start adjusting the EKF2_EV_DELAY parameter that affects the position estimates. It should be decreased if mocap data is fed at a high rate (look into motive software and see what the frequency is set to, or use topic_tools function to throttle the frequency of a topic).
LPE Tuning
This tutorial is based around using the EKF2 although if the LPE was used, tuning its performance can be done from the following section on the PX4 website.
Last updated
