I found one of the integrator was not working due to the insufficient soldering.
I remedied it, then the TF of the servo seems what I intented.

However, I found that the gain was too high for stable lock...
I will modify the servo or locking scheme.