Optimized Kalman_Filter
Added H2control