Abstract: To solve the problem that filtering effect is incompatible with phase margin in servo control system, while eliminating high-frequency noise will lose useful low-frequency signal, we present a filtering method of anti-waves disturbance real-time control signal based on Kalman filter algorithm. In this method the system is described by state space method, the second-order autoregression model is used to calculate the system parameters, and recursive algorithm is adopted to achieve real-time process of computer, thus the high-frequency noise can be significantly removed, the gyro drift can be suppressed and the useful signal can be extracted. Simulation result shows that this method is simple and convenient in implementation, and has good disturbance suppression performance. Moreover, it effectively reduces the loss of system phase, eliminates the high-frequency interference, and greatly contribute improvement to the system bandwidth and the open loop gain.