A system (18) for controlling a safety system (44) of a vehicle (10) is disclosed herein. The system includes a longitudinal acceleration sensor (36), a vehicle or wheel speed sensor(s) (20), a lateral acceleration sensor (32), a yaw rate sensor (28), and a controller (26). The controller determines a stability index and provides a first observer that determines a reference longitudinal velocity in response to the sensors. The controller determines a reference lateral velocity in response to the sensors. The controller provides a second observer that determines a second longitudinal velocity in response to the sensors and a first adjustment based on the reference longitudinal velocity. The controller determines a second lateral velocity in response to the sensors and a second adjustment based on the reference lateral velocity. The controller determines an output lateral velocity and an output longitudinal velocity in response to the first and second observers and the stability index and accordingly controls the safety system.