State Space Control of a Swerve Module Steering Mechanism

I’ve included a sysId json file of a steering mechanism (powered by a NEO) spinning in place on carpet.

I need some help isolating out tuning knobs from obvious inputs to state space control a swerve module’s steering mechanism.

Here is my methodology. Please assume I’ve handled units accordingly.

Items to code up. I think I’ve got this covered based off of examples usage and docs

  1. I need a KalmanFilter <2,1, 1>
  2. I need an LQR<2, 1, 1>
  3. plant<2, 1, 1>
  4. linear system loop < 2, 1, 1> that ties them all together
  5. periodically follow nextR, predict, correct, getU sequence in docs along with a trapMotionProfile.

Numbers to determine and their uses, and questions on each.

  1. plant uses kV, and kA from sysId. See file.
  2. LQR controller uses plant, q elements and r elements. r element are 12.0V from the battery.
    q elements are maxPositionError and maxVelocityError. and dt seconds which is 0.020
  3. observer (Kalman Filter) needs plant, stateStdDevs and measurementStdDevs and dt at 0.020

Questions concerning numbers
maxPositionError: For accuracy sake should this be the smallest possible encoder position of the NEO? 1/42 rotations unit converted accordingly?

maxVelocityError: Should this just be as high as possible or should it also be 1/42 for the NEO?

measurementStdDevs: Again should this be the minimum resolution of the NEO? I only have position to worry about here.

stateStdDevs. I don’t know where to acquire this. I feel like sysId should be handling this since it’s job is to characterize the system, but I’m not sure which numbers to use or if these are just tuning knobs. I have position and velocity to worry about here.

So that gives me possible items to tune
LQR:
maxPositionError
maxVelocityError

KalmanFilter:
measurementStdDevs (for position)
stateStdsDevs (for position and velocity)

Five possible tuning knobs.

Also if I use position latency from sysId to compensate then 6 knobs.

ModuleTurnSwerve.json (1.4 MB)