Source: https://homepages.laas.fr/arzelier/polycop/n6k3/Commande_Optimale/C1_CO.pdf