En algoritme til optimale kontrolproblemer