ordering
make ch7 -> Kp , removed servo output filter