Control system for a vehicle and method thereof

A control system and method for controlling a vehicle 200 comprising: one or more controller; a memory for storing a model associated with a vehicle; and at least one processing means arranged to: receive, from at least one sensor, sensor data indicative of a location of one or more objects 300 detected in an environment of the vehicle; determine, in dependence on the received sensor data, at least one probability density function 320, wherein each probability density function is indicative of an expected location of each of the one or more objects up to a predetermined time horizon; determine, in dependence on the determined probability density function and the model, a likelihood of intersection of the expected location of the one or more objects and the model 230 of the vehicle up to the predetermined time horizon; and determine, in dependence on the determined likelihood of intersection, one or more control inputs for the vehicle up to the predetermined time horizon; and output means configured to output one or more control signals indicative of the one or more control inputs.