How to reduce 'jitter'/tilt with FOC w/NavX

I keep saying this on every one of my posts lol, but our team is doing swerve this year, and its actually coming together pretty well!

However, there is an issue with our Field-Oriented-Control:

when we try to move (often times at a straight line), there is a slight ‘tilt’ and it’ll go slightly to the left or right, after some time the robot will ‘jitter’ for a little bit, before correcting itself.

I checked SmartDashboard where we log all our devices and stuff, and saw that more often than not; the NavX readings will jitter. (for example: the angle reading might jitter between 3 to 5 degrees). I’m not exactly sure what’s causing this:

I had initially thought that vibrations from movement might’ve caused this issue, but even when still, the NavX gives us jittery readings.

Here are some other things that could be important:

  • We are using YAGSL
  • Our swerve modules are: 2 NEOs w/SparkMax and 1 CANCoder each