Sim EKF2 Estimated Angle vs Ground Truth Angle Discrepancy

#1

Quick background, we have built a Matlab/Simulink simulation for a custom VTOL we are building.

The simulation runs fine and we are able to fly in SITL mode w/ QGroundControl.

We have an issue that we can’t quite track down where the EKF2 estimated angles for roll/yaw always slightly overshoot the ground truth angle from the sim, and we have been unable to tune this out with the EKF2 params (which honestly we don’t fully understand) or identify what else might be the cause.

The angular rates DO track perfectly with the ground truth, by the way.

Below is a Flight Review example for a simple multicopter roll back and forth which shows the angle discrepancy and the perfect angular rate match.

Can anyone offer any insight into what we might be missing or not understanding here? Any help would be greatly appreciated!

The full flight review log is here:
https://logs.px4.io/plot_app?log=3438834a-5728-4a5a-b479-1c538ab1cfcc

Thanks!
Nick

#2

Are you sure you are using the same rotation order to calculate the Euler angles?

1 Like
#3

Hey Carl, I’m still wrapping my head around this (and quaternions), but when you are talking about the rotation order, are you talking about the process to get the Euler angles from the Quaternions? If so, it’s the same code that translates both the vehicle_attitude and vehicle_attitude_groundtruth quaternions to Euler that occurs in the Flight Review code.

Or am I missing something in your question as well?

#4

Hi,
Yes exactly. It looked similar to when you compare e.g. the roll angle from a 321 and 312 Tait-Bryan Euler angle sequence.
I never really looked into the EKF performance compared to a ground truth since that is generally not available, and the simulation environment is usually not capable to simulate the EKF accurately due to timeing issues etc. (Maybe that have changed now with lockstep but not sure)

I quickly compared the results to our simulation and we have the same issue with not perfect angle tracking. I would guess it comes from a heading estimate mismatch which couples into the roll angle.

Depending on what you are trying to do, but if your goal is not to change the estimator I would not be too concerned about this. The EKF performance is much better to tune using replay logfiles collected from real flight data.

#5

Hey Carl, thanks again.

We’re looking into the Euler angle sequence, and I’ve been trying to see if I can get our simulation to work in lockstep. (And we definitely don’t want to get into the business of modifying the estimator)

It’s pretty straightforward to have our side wait for HIL_ACTUATOR_CONTROLS before taking another step, however, if I send just a single HIL_SENSOR message to trigger the next step in the PX4 loop, it never takes the next step. If I do continuously repeat the HIL_SENSOR (and other sim outputs) the loop continues, but things get real haywire. I’m wondering if it has to do with our time_usec stamp on our messages. Perhaps we’re not setting that right, or maybe there’s something else I’m missing. Any ideas on that front?

#6

Actually, I think I can figure out the timing by looking at how the timing is handled in jMAVsim’s lockstep mode.

#7

Once we got our sim in lockstep with PX4 the angle discrepancy cleared up!

#8

Nice! Thanks for sharing