In order to improve the control accuracy of vehicle electronic stability program, an Optimal Extended Kalman Filter (OEKF) algorithm is designed based on Extended Kalman Filter (EKF) and optimal control theory. The 7-DOF vehicle model, the linear two-degree-of-freedom vehicle model and the LQG control model of the body electronic stability program (ESP) are built by MATLAB/Simulink. Taking the vehicle sideslip angle and yaw rate as control variables, the OEKF sideslip angle observer is established and simulated in MATLAB/Simulink environment. By comparing with the traditional EKF sideslip angle observer, it is concluded that the OEKF sideslip angle observer can not only estimate the sideslip angle accurately and in real time, but also effectively reduce the lateral acceleration of the vehicle during sideslip and improve the handling stability of the vehicle in driving state. Finally, the effectiveness of the algorithm is further verified by real vehicle road test. In this paper, the algorithm is innovated, and the EKF algorithm and the optimal control theory are combined to study the vehicle sideslip angle estimation and stability control.