System and Method Suitable for Controlling Motion of an Ego Vehicle in an Environment Including Other Moving Agents

The present disclosure provides a system and a method for controlling an ego vehicle (EV) in an environment surrounding the EV and including at least one other agent (OA) representing a moving object. The method includes determining an independent trajectory for the EV independent from motion of the at least one OA based on a state of the EV, and an independent trajectory for the at least one OA independent from motion of the EV based on a state of the at least one OA. The method further includes determining jointly and interdependently joint trajectories of the EV and the at least one OA by optimizing a cost function of a difference between the joint trajectories and the independent trajectories. The method further includes controlling the motion of the EV based on the joint trajectory of the EV.