Trapezoidal Motion Profiles and Kalman Filters

[BeepBot99]

• Add trapezoidal motion profiles as an InterpolatorElement
• Add Kalman filters as a Filter

[zachwaffle4]

LMAO I started a motion profile feature branch like an hour ago

[BeepBot99]

Wow thank you! Take a look at the WPILib one, you should probably base it off that.

[zachwaffle4]

conveniently (2) i have the entire allwpilib source code on my computer