Investigate using 2nd Order Kinematics

[legoguy1000]

Odometry can become inaccurate while moving and rotating at the same time when using first order kinematics. Some teams have switched to 2nd Order Kinematics to improve their Position Estimation.

https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5?u=legoguy1000
https://github.com/FRC-4481-Team-Rembrandts/4481-Stock-Robot-2023/tree/2023.0
https://www.chiefdelphi.com/t/frc-3181-pittsford-robotics-2023-build-thread/420726/4

[legoguy1000]

https://www.chiefdelphi.com/t/swerve-drive-goes-in-an-arc-while-rotating/424669/7?u=legoguy1000

[legoguy1000]

Might be good to try this first since it's a much simpler change https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5?u=legoguy1000

[legoguy1000]

Another implementation of 2nd order kinematics https://github.com/BroncBotz3481/YAGSL-Example/tree/main/src%2Fmain%2Fjava%2Ffrc%2Frobot%2Fsubsystems%2Fswervedrive%2Fswerve%2Fkinematics

[legoguy1000]

@agrinmanriv0537 do we want to close this due to #44 or keep it in the backlog to look more later??

[wilsonwatson]

#44 Doesn't actually use 2nd-order kinematics. Rather it just accounts for the discretization of control inputs. Probably should be put on backlog.