[Jared Russell]

What does your state space look like?

Problem #1 (integral windup on a unit step) is a fundamental limitation of linear feedback control; you can implement some sort of anti-windup mechanism if you want (as you would with PID) but it will require additional logic and make your closed-loop system nonlinear. A common way to do this is by preprocessing your input (e.g. calculating a new instantaneous goal that will saturate your output but won't result in excessive windup).

For problem #2, if you formulate your state space and controller correctly, you should see the velocity state of your output behave as desired (matching the velocity reference if there is no position error, and adjusting +/- depending on position tracking). However, this requires designing a MIMO controller, so you need to use a method suitable for doing so, since you need to choose a K matrix that's makes tradeoffs between multiple objectives (LQR is one common approach). As a quick workaround, you can use apply separate position and velocity FSF controllers and sum their outputs.