diff --git a/control/config.py b/control/config.py
index afd7615ca..8acdf28e2 100644
--- a/control/config.py
+++ b/control/config.py
@@ -103,6 +103,9 @@ def reset_defaults():
     from .iosys import _iosys_defaults
     defaults.update(_iosys_defaults)
 
+    from .optimal import _optimal_defaults
+    defaults.update(_optimal_defaults)
+
 
 def _get_param(module, param, argval=None, defval=None, pop=False, last=False):
     """Return the default value for a configuration option.
diff --git a/control/iosys.py b/control/iosys.py
index 2f63a7e8b..d5a7b755b 100644
--- a/control/iosys.py
+++ b/control/iosys.py
@@ -1571,7 +1571,7 @@ def __init__(self, io_sys, ss_sys=None):
 def input_output_response(
         sys, T, U=0., X0=0, params={},
         transpose=False, return_x=False, squeeze=None,
-        solve_ivp_kwargs={}, **kwargs):
+        solve_ivp_kwargs={}, t_eval='T', **kwargs):
     """Compute the output response of a system to a given input.
 
     Simulate a dynamical system with a given input and return its output
@@ -1654,50 +1654,57 @@ def input_output_response(
     if kwargs.get('solve_ivp_method', None):
         if kwargs.get('method', None):
             raise ValueError("ivp_method specified more than once")
-        solve_ivp_kwargs['method'] = kwargs['solve_ivp_method']
+        solve_ivp_kwargs['method'] = kwargs.pop('solve_ivp_method')
 
     # Set the default method to 'RK45'
     if solve_ivp_kwargs.get('method', None) is None:
         solve_ivp_kwargs['method'] = 'RK45'
 
+    # Make sure all input arguments got parsed
+    if kwargs:
+        raise TypeError("unknown parameters %s" % kwargs)
+
     # Sanity checking on the input
     if not isinstance(sys, InputOutputSystem):
         raise TypeError("System of type ", type(sys), " not valid")
 
     # Compute the time interval and number of steps
     T0, Tf = T[0], T[-1]
-    n_steps = len(T)
+    ntimepts = len(T)
+
+    # Figure out simulation times (t_eval)
+    if solve_ivp_kwargs.get('t_eval'):
+        if t_eval == 'T':
+            # Override the default with the solve_ivp keyword
+            t_eval = solve_ivp_kwargs.pop('t_eval')
+        else:
+            raise ValueError("t_eval specified more than once")
+    if isinstance(t_eval, str) and t_eval == 'T':
+        # Use the input time points as the output time points
+        t_eval = T
 
     # Check and convert the input, if needed
     # TODO: improve MIMO ninputs check (choose from U)
     if sys.ninputs is None or sys.ninputs == 1:
-        legal_shapes = [(n_steps,), (1, n_steps)]
+        legal_shapes = [(ntimepts,), (1, ntimepts)]
     else:
-        legal_shapes = [(sys.ninputs, n_steps)]
+        legal_shapes = [(sys.ninputs, ntimepts)]
     U = _check_convert_array(U, legal_shapes,
                              'Parameter ``U``: ', squeeze=False)
-    U = U.reshape(-1, n_steps)
-
-    # Check to make sure this is not a static function
-    nstates = _find_size(sys.nstates, X0)
-    if nstates == 0:
-        # No states => map input to output
-        u = U[0] if len(U.shape) == 1 else U[:, 0]
-        y = np.zeros((np.shape(sys._out(T[0], X0, u))[0], len(T)))
-        for i in range(len(T)):
-            u = U[i] if len(U.shape) == 1 else U[:, i]
-            y[:, i] = sys._out(T[i], [], u)
-        return TimeResponseData(
-            T, y, None, U, issiso=sys.issiso(),
-            output_labels=sys.output_index, input_labels=sys.input_index,
-            transpose=transpose, return_x=return_x, squeeze=squeeze)
+    U = U.reshape(-1, ntimepts)
+    ninputs = U.shape[0]
 
     # create X0 if not given, test if X0 has correct shape
+    nstates = _find_size(sys.nstates, X0)
     X0 = _check_convert_array(X0, [(nstates,), (nstates, 1)],
                               'Parameter ``X0``: ', squeeze=True)
 
-    # Update the parameter values
-    sys._update_params(params)
+    # Figure out the number of outputs
+    if sys.noutputs is None:
+        # Evaluate the output function to find number of outputs
+        noutputs = np.shape(sys._out(T[0], X0, U[:, 0]))[0]
+    else:
+        noutputs = sys.noutputs
 
     #
     # Define a function to evaluate the input at an arbitrary time
@@ -1714,6 +1721,31 @@ def ufun(t):
         dt = (t - T[idx-1]) / (T[idx] - T[idx-1])
         return U[..., idx-1] * (1. - dt) + U[..., idx] * dt
 
+    # Check to make sure this is not a static function
+    if nstates == 0:            # No states => map input to output
+        # Make sure the user gave a time vector for evaluation (or 'T')
+        if t_eval is None:
+            # User overrode t_eval with None, but didn't give us the times...
+            warn("t_eval set to None, but no dynamics; using T instead")
+            t_eval = T
+
+        # Allocate space for the inputs and outputs
+        u = np.zeros((ninputs, len(t_eval)))
+        y = np.zeros((noutputs, len(t_eval)))
+
+        # Compute the input and output at each point in time
+        for i, t in enumerate(t_eval):
+            u[:, i] = ufun(t)
+            y[:, i] = sys._out(t, [], u[:, i])
+
+        return TimeResponseData(
+            t_eval, y, None, u, issiso=sys.issiso(),
+            output_labels=sys.output_index, input_labels=sys.input_index,
+            transpose=transpose, return_x=return_x, squeeze=squeeze)
+
+    # Update the parameter values
+    sys._update_params(params)
+
     # Create a lambda function for the right hand side
     def ivp_rhs(t, x):
         return sys._rhs(t, x, ufun(t))
@@ -1724,27 +1756,27 @@ def ivp_rhs(t, x):
             raise NameError("scipy.integrate.solve_ivp not found; "
                             "use SciPy 1.0 or greater")
         soln = sp.integrate.solve_ivp(
-            ivp_rhs, (T0, Tf), X0, t_eval=T,
+            ivp_rhs, (T0, Tf), X0, t_eval=t_eval,
             vectorized=False, **solve_ivp_kwargs)
+        if not soln.success:
+            raise RuntimeError("solve_ivp failed: " + soln.message)
 
-        if not soln.success or soln.status != 0:
-            # Something went wrong
-            warn("sp.integrate.solve_ivp failed")
-            print("Return bunch:", soln)
-
-        # Compute the output associated with the state (and use sys.out to
-        # figure out the number of outputs just in case it wasn't specified)
-        u = U[0] if len(U.shape) == 1 else U[:, 0]
-        y = np.zeros((np.shape(sys._out(T[0], X0, u))[0], len(T)))
-        for i in range(len(T)):
-            u = U[i] if len(U.shape) == 1 else U[:, i]
-            y[:, i] = sys._out(T[i], soln.y[:, i], u)
+        # Compute inputs and outputs for each time point
+        u = np.zeros((ninputs, len(soln.t)))
+        y = np.zeros((noutputs, len(soln.t)))
+        for i, t in enumerate(soln.t):
+            u[:, i] = ufun(t)
+            y[:, i] = sys._out(t, soln.y[:, i], u[:, i])
 
     elif isdtime(sys):
+        # If t_eval was not specified, use the sampling time
+        if t_eval is None:
+            t_eval = np.arange(T[0], T[1] + sys.dt, sys.dt)
+
         # Make sure the time vector is uniformly spaced
-        dt = T[1] - T[0]
-        if not np.allclose(T[1:] - T[:-1], dt):
-            raise ValueError("Parameter ``T``: time values must be "
+        dt = t_eval[1] - t_eval[0]
+        if not np.allclose(t_eval[1:] - t_eval[:-1], dt):
+            raise ValueError("Parameter ``t_eval``: time values must be "
                              "equally spaced.")
 
         # Make sure the sample time matches the given time
@@ -1764,21 +1796,23 @@ def ivp_rhs(t, x):
 
         # Compute the solution
         soln = sp.optimize.OptimizeResult()
-        soln.t = T                      # Store the time vector directly
-        x = X0                          # Initilize state
+        soln.t = t_eval                 # Store the time vector directly
+        x = np.array(X0)                # State vector (store as floats)
         soln.y = []                     # Solution, following scipy convention
-        y = []                          # System output
-        for i in range(len(T)):
-            # Store the current state and output
+        u, y = [], []                   # System input, output
+        for t in t_eval:
+            # Store the current input, state, and output
             soln.y.append(x)
-            y.append(sys._out(T[i], x, ufun(T[i])))
+            u.append(ufun(t))
+            y.append(sys._out(t, x, u[-1]))
 
             # Update the state for the next iteration
-            x = sys._rhs(T[i], x, ufun(T[i]))
+            x = sys._rhs(t, x, u[-1])
 
         # Convert output to numpy arrays
         soln.y = np.transpose(np.array(soln.y))
         y = np.transpose(np.array(y))
+        u = np.transpose(np.array(u))
 
         # Mark solution as successful
         soln.success = True     # No way to fail
@@ -1787,7 +1821,7 @@ def ivp_rhs(t, x):
         raise TypeError("Can't determine system type")
 
     return TimeResponseData(
-        soln.t, y, soln.y, U, issiso=sys.issiso(),
+        soln.t, y, soln.y, u, issiso=sys.issiso(),
         output_labels=sys.output_index, input_labels=sys.input_index,
         state_labels=sys.state_index,
         transpose=transpose, return_x=return_x, squeeze=squeeze)
diff --git a/control/optimal.py b/control/optimal.py
index 493b6bc3d..cb9f57ded 100644
--- a/control/optimal.py
+++ b/control/optimal.py
@@ -16,10 +16,21 @@
 import logging
 import time
 
+from . import config
+from .exception import ControlNotImplemented
 from .timeresp import TimeResponseData
 
 __all__ = ['find_optimal_input']
 
+# Define module default parameter values
+_optimal_defaults = {
+    'optimal.minimize_method': None,
+    'optimal.minimize_options': {},
+    'optimal.minimize_kwargs': {},
+    'optimal.solve_ivp_method': None,
+    'optimal.solve_ivp_options': {},
+}
+
 
 class OptimalControlProblem():
     """Description of a finite horizon, optimal control problem.
@@ -110,6 +121,10 @@ class OptimalControlProblem():
     values of the input at the specified times (using linear interpolation for
     continuous systems).
 
+    The default values for ``minimize_method``, ``minimize_options``,
+    ``minimize_kwargs``, ``solve_ivp_method``, and ``solve_ivp_options`` can
+    be set using config.defaults['optimal.<keyword>'].
+
     """
     def __init__(
             self, sys, timepts, integral_cost, trajectory_constraints=[],
@@ -126,17 +141,22 @@ def __init__(
 
         # Process keyword arguments
         self.solve_ivp_kwargs = {}
-        self.solve_ivp_kwargs['method'] = kwargs.pop('solve_ivp_method', None)
-        self.solve_ivp_kwargs.update(kwargs.pop('solve_ivp_kwargs', {}))
+        self.solve_ivp_kwargs['method'] = kwargs.pop(
+            'solve_ivp_method', config.defaults['optimal.solve_ivp_method'])
+        self.solve_ivp_kwargs.update(kwargs.pop(
+            'solve_ivp_kwargs', config.defaults['optimal.solve_ivp_options']))
 
         self.minimize_kwargs = {}
-        self.minimize_kwargs['method'] = kwargs.pop('minimize_method', None)
-        self.minimize_kwargs['options'] = kwargs.pop('minimize_options', {})
-        self.minimize_kwargs.update(kwargs.pop('minimize_kwargs', {}))
+        self.minimize_kwargs['method'] = kwargs.pop(
+            'minimize_method', config.defaults['optimal.minimize_method'])
+        self.minimize_kwargs['options'] = kwargs.pop(
+            'minimize_options', config.defaults['optimal.minimize_options'])
+        self.minimize_kwargs.update(kwargs.pop(
+            'minimize_kwargs', config.defaults['optimal.minimize_kwargs']))
 
-        if len(kwargs) > 0:
-            raise ValueError(
-                f'unrecognized keyword(s): {list(kwargs.keys())}')
+        # Make sure all input arguments got parsed
+        if kwargs:
+            raise TypeError("unrecognized keyword(s): ", str(kwargs))
 
         # Process trajectory constraints
         if isinstance(trajectory_constraints, tuple):
@@ -271,9 +291,10 @@ def _cost_function(self, coeffs):
                 logging.debug("initial input[0:3] =\n" + str(inputs[:, 0:3]))
 
             # Simulate the system to get the state
+            # TODO: try calling solve_ivp directly for better speed?
             _, _, states = ct.input_output_response(
                 self.system, self.timepts, inputs, x, return_x=True,
-                solve_ivp_kwargs=self.solve_ivp_kwargs)
+                solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts)
             self.system_simulations += 1
             self.last_x = x
             self.last_coeffs = coeffs
@@ -393,7 +414,7 @@ def _constraint_function(self, coeffs):
             # Simulate the system to get the state
             _, _, states = ct.input_output_response(
                 self.system, self.timepts, inputs, x, return_x=True,
-                solve_ivp_kwargs=self.solve_ivp_kwargs)
+                solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts)
             self.system_simulations += 1
             self.last_x = x
             self.last_coeffs = coeffs
@@ -475,7 +496,7 @@ def _eqconst_function(self, coeffs):
             # Simulate the system to get the state
             _, _, states = ct.input_output_response(
                 self.system, self.timepts, inputs, x, return_x=True,
-                solve_ivp_kwargs=self.solve_ivp_kwargs)
+                solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts)
             self.system_simulations += 1
             self.last_x = x
             self.last_coeffs = coeffs
@@ -548,7 +569,7 @@ def _process_initial_guess(self, initial_guess):
             initial_guess = np.atleast_1d(initial_guess)
 
             # See whether we got entire guess or just first time point
-            if len(initial_guess.shape) == 1:
+            if initial_guess.ndim == 1:
                 # Broadcast inputs to entire time vector
                 try:
                     initial_guess = np.broadcast_to(
@@ -804,6 +825,15 @@ class OptimalControlResult(sp.optimize.OptimizeResult):
         Whether or not the optimizer exited successful.
     problem : OptimalControlProblem
         Optimal control problem that generated this solution.
+    cost : float
+        Final cost of the return solution.
+    system_simulations, {cost, constraint, eqconst}_evaluations : int
+        Number of system simulations and evaluations of the cost function,
+        (inequality) constraint function, and equality constraint function
+        performed during the optimzation.
+    {cost, constraint, eqconst}_process_time : float
+        If logging was enabled, the amount of time spent evaluating the cost
+        and constraint functions.
 
     """
     def __init__(
@@ -833,15 +863,19 @@ def __init__(
                 "unable to solve optimal control problem\n"
                 "scipy.optimize.minimize returned " + res.message, UserWarning)
 
+        # Save the final cost
+        self.cost = res.fun
+
         # Optionally print summary information
         if print_summary:
             ocp._print_statistics()
+            print("* Final cost:", self.cost)
 
         if return_states and inputs.shape[1] == ocp.timepts.shape[0]:
             # Simulate the system if we need the state back
             _, _, states = ct.input_output_response(
                 ocp.system, ocp.timepts, inputs, ocp.x, return_x=True,
-                solve_ivp_kwargs=ocp.solve_ivp_kwargs)
+                solve_ivp_kwargs=ocp.solve_ivp_kwargs, t_eval=ocp.timepts)
             ocp.system_simulations += 1
         else:
             states = None
@@ -858,7 +892,7 @@ def __init__(
 
 # Compute the input for a nonlinear, (constrained) optimal control problem
 def solve_ocp(
-        sys, horizon, X0, cost, trajectory_constraints=[], terminal_cost=None,
+        sys, horizon, X0, cost, trajectory_constraints=None, terminal_cost=None,
         terminal_constraints=[], initial_guess=None, basis=None, squeeze=None,
         transpose=None, return_states=False, log=False, **kwargs):
 
@@ -960,12 +994,18 @@ def solve_ocp(
     # Process keyword arguments
     if trajectory_constraints is None:
         # Backwards compatibility
-        trajectory_constraints = kwargs.pop('constraints', None)
+        trajectory_constraints = kwargs.pop('constraints', [])
 
     # Allow 'return_x` as a synonym for 'return_states'
     return_states = ct.config._get_param(
         'optimal', 'return_x', kwargs, return_states, pop=True)
 
+    # Process (legacy) method keyword
+    if kwargs.get('method'):
+        if kwargs.get('minimize_method'):
+            raise ValueError("'minimize_method' specified more than once")
+        kwargs['minimize_method'] = kwargs.pop('method')
+
     # Set up the optimal control problem
     ocp = OptimalControlProblem(
         sys, horizon, cost, trajectory_constraints=trajectory_constraints,
diff --git a/control/statefbk.py b/control/statefbk.py
index a866af725..6598eeeb8 100644
--- a/control/statefbk.py
+++ b/control/statefbk.py
@@ -734,7 +734,7 @@ def dlqr(*args, **kwargs):
     The dlqr() function computes the optimal state feedback controller
     u[n] = - K x[n] that minimizes the quadratic cost
 
-    .. math:: J = \\Sum_0^\\infty (x[n]' Q x[n] + u[n]' R u[n] + 2 x[n]' N u[n])
+    .. math:: J = \\sum_0^\\infty (x[n]' Q x[n] + u[n]' R u[n] + 2 x[n]' N u[n])
 
     The function can be called with either 3, 4, or 5 arguments:
 
diff --git a/control/tests/iosys_test.py b/control/tests/iosys_test.py
index bdc5ba31a..e76a6be55 100644
--- a/control/tests/iosys_test.py
+++ b/control/tests/iosys_test.py
@@ -1648,7 +1648,9 @@ def test_interconnect_unused_output():
                           outputs=['u'],
                           name='k')
 
-    with pytest.warns(UserWarning, match=r"Unused output\(s\) in InterconnectedSystem:") as record:
+    with pytest.warns(
+            UserWarning,
+            match=r"Unused output\(s\) in InterconnectedSystem:") as record:
         h = ct.interconnect([g,s,k],
                             inputs=['r'],
                             outputs=['y'])
@@ -1679,13 +1681,17 @@ def test_interconnect_unused_output():
             pytest.fail(f'Unexpected warning: {r.message}')
 
     # warn if explicity ignored output in fact used
-    with pytest.warns(UserWarning, match=r"Output\(s\) specified as ignored is \(are\) used:"):
+    with pytest.warns(
+            UserWarning,
+            match=r"Output\(s\) specified as ignored is \(are\) used:"):
         h = ct.interconnect([g,s,k],
                             inputs=['r'],
                             outputs=['y'],
                             ignore_outputs=['dy','u'])
 
-    with pytest.warns(UserWarning, match=r"Output\(s\) specified as ignored is \(are\) used:"):
+    with pytest.warns(
+            UserWarning,
+            match=r"Output\(s\) specified as ignored is \(are\) used:"):
         h = ct.interconnect([g,s,k],
                             inputs=['r'],
                             outputs=['y'],
@@ -1697,3 +1703,25 @@ def test_interconnect_unused_output():
                             inputs=['r'],
                             outputs=['y'],
                             ignore_outputs=['v'])
+
+def test_nonuniform_timepts():
+    """Test non-uniform time points for simulations"""
+    sys = ct.LinearIOSystem(ct.rss(2, 1, 1))
+
+    # Start with a uniform set of times
+    unifpts = [0, 1, 2, 3, 4,  5,  6,  7,  8,  9, 10]
+    uniform = [1, 2, 3, 2, 1, -1, -3, -5, -7, -3,  1]
+    t_unif, y_unif = ct.input_output_response(sys, unifpts, uniform)
+
+    # Create a non-uniform set of inputs
+    noufpts = [0, 2, 4,  8, 10]
+    nonunif = [1, 3, 1, -7,  1]
+    t_nouf, y_nouf = ct.input_output_response(sys, noufpts, nonunif)
+
+    # Make sure the outputs agree at common times
+    np.testing.assert_almost_equal(y_unif[noufpts], y_nouf, decimal=6)
+
+    # Resimulate using a new set of evaluation points
+    t_even, y_even = ct.input_output_response(
+        sys, noufpts, nonunif, t_eval=unifpts)
+    np.testing.assert_almost_equal(y_unif, y_even, decimal=6)
diff --git a/control/tests/optimal_test.py b/control/tests/optimal_test.py
index f059c4fc6..1aa307b60 100644
--- a/control/tests/optimal_test.py
+++ b/control/tests/optimal_test.py
@@ -45,6 +45,9 @@ def test_finite_horizon_simple():
     np.testing.assert_almost_equal(
         u_openloop, [-1, -1, 0.1393, 0.3361, -5.204e-16], decimal=4)
 
+    # Make sure the final cost is correct
+    assert math.isclose(res.cost, 32.4898, rel_tol=1e-5)
+
     # Convert controller to an explicit form (not implemented yet)
     # mpc_explicit = opt.explicit_mpc();
 
@@ -117,7 +120,7 @@ def test_discrete_lqr():
     assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
 
 
-def test_mpc_iosystem():
+def test_mpc_iosystem_aircraft():
     # model of an aircraft discretized with 0.2s sampling time
     # Source: https://www.mpt3.org/UI/RegulationProblem
     A = [[0.99, 0.01, 0.18, -0.09,   0],
@@ -171,6 +174,21 @@ def test_mpc_iosystem():
         xout[0:sys.nstates, -1], xd, atol=0.1, rtol=0.01)
 
 
+def test_mpc_iosystem_continuous():
+    # Create a random state space system
+    sys = ct.rss(2, 1, 1)
+    T, _ = ct.step_response(sys)
+
+    # provide penalties on the system signals
+    Q = np.eye(sys.nstates)
+    R = np.eye(sys.ninputs)
+    cost = opt.quadratic_cost(sys, Q, R)
+
+    # Continuous time MPC controller not implemented
+    with pytest.raises(NotImplementedError):
+        ctrl = opt.create_mpc_iosystem(sys, T, cost)
+
+
 # Test various constraint combinations; need to use a somewhat convoluted
 # parametrization due to the need to define sys instead the test function
 @pytest.mark.parametrize("constraint_list", [
@@ -437,7 +455,7 @@ def test_ocp_argument_errors():
             sys, time, x0, cost, constraints, initial_guess=np.zeros((4,1,1)))
 
     # Unrecognized arguments
-    with pytest.raises(ValueError, match="unrecognized keyword"):
+    with pytest.raises(TypeError, match="unrecognized keyword"):
         res = opt.solve_ocp(
             sys, time, x0, cost, constraints, terminal_constraint=None)
 
@@ -549,3 +567,59 @@ def final_point_eval(x, u):
         optctrl = opt.OptimalControlProblem(
             sys, time, cost, terminal_constraints=final_point)
         res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
+
+
+def test_optimal_doc():
+    """Test optimal control problem from documentation"""
+    def vehicle_update(t, x, u, params):
+        # Get the parameters for the model
+        l = params.get('wheelbase', 3.)         # vehicle wheelbase
+        phimax = params.get('maxsteer', 0.5)    # max steering angle (rad)
+
+        # Saturate the steering input
+        phi = np.clip(u[1], -phimax, phimax)
+
+        # Return the derivative of the state
+        return np.array([
+            np.cos(x[2]) * u[0],            # xdot = cos(theta) v
+            np.sin(x[2]) * u[0],            # ydot = sin(theta) v
+            (u[0] / l) * np.tan(phi)        # thdot = v/l tan(phi)
+        ])
+
+    def vehicle_output(t, x, u, params):
+        return x                            # return x, y, theta (full state)
+
+    # Define the vehicle steering dynamics as an input/output system
+    vehicle = ct.NonlinearIOSystem(
+        vehicle_update, vehicle_output, states=3, name='vehicle',
+        inputs=('v', 'phi'), outputs=('x', 'y', 'theta'))
+
+    # Define the initial and final points and time interval
+    x0 = [0., -2., 0.]; u0 = [10., 0.]
+    xf = [100., 2., 0.]; uf = [10., 0.]
+    Tf = 10
+
+    # Define the cost functions
+    Q = np.diag([0, 0, 0.1])          # don't turn too sharply
+    R = np.diag([1, 1])               # keep inputs small
+    P = np.diag([1000, 1000, 1000])   # get close to final point
+    traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
+    term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf)
+
+    # Define the constraints
+    constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
+
+    # Solve the optimal control problem
+    horizon = np.linspace(0, Tf, 3, endpoint=True)
+    result = opt.solve_ocp(
+        vehicle, horizon, x0, traj_cost, constraints,
+        terminal_cost= term_cost, initial_guess=u0)
+
+    # Make sure the resulting trajectory generate a good solution
+    resp = ct.input_output_response(
+        vehicle, horizon, result.inputs, x0,
+        t_eval=np.linspace(0, Tf, 10))
+    t, y = resp
+    assert (y[0, -1] - xf[0]) / xf[0] < 0.01
+    assert (y[1, -1] - xf[1]) / xf[1] < 0.01
+    assert y[2, -1] < 0.1
diff --git a/doc/control.rst b/doc/control.rst
index 87c1151eb..8bd6f7a32 100644
--- a/doc/control.rst
+++ b/doc/control.rst
@@ -114,7 +114,7 @@ Control system synthesis
     lqe
     mixsyn
     place
-    rlocus_pid_designer
+    rootlocus_pid_designer
 
 Model simplification tools
 ==========================
diff --git a/doc/examples.rst b/doc/examples.rst
index 91476bc9d..89a2b16a1 100644
--- a/doc/examples.rst
+++ b/doc/examples.rst
@@ -29,6 +29,7 @@ other sources.
    robust_mimo
    cruise-control
    steering-gainsched
+   steering-optimal
    kincar-flatsys
 
 Jupyter notebooks
diff --git a/doc/optimal.rst b/doc/optimal.rst
index e173e430b..8da08e7af 100644
--- a/doc/optimal.rst
+++ b/doc/optimal.rst
@@ -79,7 +79,7 @@ Every :math:`\Delta T` seconds, an optimal control problem is solved over a
 :math:`T` second horizon, starting from the current state.  The first
 :math:`\Delta T` seconds of the optimal control :math:`u_T^{\*}(\cdot;
 x(t))` is then applied to the system. If we let :math:`x_T^{\*}(\cdot;
-x(t))` represent the optimal trajectory starting from :math:`x(t)`$ then the
+x(t))` represent the optimal trajectory starting from :math:`x(t)` then the
 system state evolves from :math:`x(t)` at current time :math:`t` to
 :math:`x_T^{*}(\delta T, x(t))` at the next sample time :math:`t + \Delta
 T`, assuming no model uncertainty.
@@ -219,9 +219,11 @@ with a starting and ending velocity of 10 m/s::
 To set up the optimal control problem we design a cost function that
 penalizes the state and input using quadratic cost functions::
 
-  Q = np.diag([0.1, 10, .1])    # keep lateral error low
-  R = np.eye(2) * 0.1
-  cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
+  Q = np.diag([0, 0, 0.1])          # don't turn too sharply
+  R = np.diag([1, 1])               # keep inputs small
+  P = np.diag([1000, 1000, 1000])   # get close to final point
+  traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
+  term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf)
 
 We also constraint the maximum turning rate to 0.1 radians (about 6 degees)
 and constrain the velocity to be in the range of 9 m/s to 11 m/s::
@@ -230,20 +232,19 @@ and constrain the velocity to be in the range of 9 m/s to 11 m/s::
 
 Finally, we solve for the optimal inputs::
 
-  horizon = np.linspace(0, Tf, 20, endpoint=True)
-  bend_left = [10, 0.01]	# slight left veer
-
+  horizon = np.linspace(0, Tf, 3, endpoint=True)
   result = opt.solve_ocp(
-      vehicle, horizon, x0, cost, constraints, initial_guess=bend_left,
-      options={'eps': 0.01})    # set step size for gradient calculation
-
-  # Extract the results
-  u = result.inputs
-  t, y = ct.input_output_response(vehicle, horizon, u, x0)
+      vehicle, horizon, x0, traj_cost, constraints,
+      terminal_cost=term_cost, initial_guess=u0)
 
 Plotting the results::
 
-  # Plot the results
+  # Simulate the system dynamics (open loop)
+  resp = ct.input_output_response(
+      vehicle, horizon, result.inputs, x0,
+      t_eval=np.linspace(0, Tf, 100))
+  t, y, u = resp.time, resp.outputs, resp.inputs
+
   plt.subplot(3, 1, 1)
   plt.plot(y[0], y[1])
   plt.plot(x0[0], x0[1], 'ro', xf[0], xf[1], 'ro')
@@ -252,15 +253,13 @@ Plotting the results::
 
   plt.subplot(3, 1, 2)
   plt.plot(t, u[0])
-  plt.axis([0, 10, 8.5, 11.5])
-  plt.plot([0, 10], [9, 9], 'k--', [0, 10], [11, 11], 'k--')
+  plt.axis([0, 10, 9.9, 10.1])
   plt.xlabel("t [sec]")
   plt.ylabel("u1 [m/s]")
 
   plt.subplot(3, 1, 3)
   plt.plot(t, u[1])
-  plt.axis([0, 10, -0.15, 0.15])
-  plt.plot([0, 10], [-0.1, -0.1], 'k--', [0, 10], [0.1, 0.1], 'k--')
+  plt.axis([0, 10, -0.01, 0.01])
   plt.xlabel("t [sec]")
   plt.ylabel("u2 [rad/s]")
 
@@ -272,6 +271,47 @@ yields
 
 .. image:: steering-optimal.png
 
+Optimization Tips
+=================
+
+The python-control optimization module makes use of the SciPy optimization
+toolbox and it can sometimes be tricky to get the optimization to converge.
+If you are getting errors when solving optimal control problems or your
+solutions do not seem close to optimal, here are a few things to try:
+
+* Less is more: try using a smaller number of time points in your
+  optimiation.  The default optimal control problem formulation uses the
+  value of the inputs at each time point as a free variable and this can
+  generate a large number of parameters quickly.  Often you can find very
+  good solutions with a small number of free variables (the example above
+  uses 3 time points for 2 inputs, so a total of 6 optimization variables).
+  Note that you can "resample" the optimal trajectory by running a
+  simulation of the sytem and using the `t_eval` keyword in
+  `input_output_response` (as done above).
+
+* Use a smooth basis: as an alternative to parameterizing the optimal
+  control inputs using the value of the control at the listed time points,
+  you can specify a set of basis functions using the `basis` keyword in
+  :func:`~control.solve_ocp` and then parameterize the controller by linear
+  combination of the basis functions.  The :mod:`!control.flatsys` module
+  defines several sets of basis functions that can be used.
+
+* Tweak the optimizer: by using the `minimize_method`, `minimize_options`,
+  and `minimize_kwargs` keywords in :func:`~control.solve_ocp`, you can
+  choose the SciPy optimization function that you use and set many
+  parameters.  See :func:`scipy.optimize.minimize` for more information on
+  the optimzers that are available and the options and keywords that they
+  accept.
+
+* Walk before you run: try setting up a simpler version of the optimization,
+  remove constraints or simplifying the cost to get a simple version of the
+  problem working and then add complexity.  Sometimes this can help you find
+  the right set of options or identify situations in which you are being too
+  aggressive in what your are trying to get the system to do.
+
+See :ref:`steering-optimal` for some examples of different problem
+formulations.
+
 
 Module classes and functions
 ============================
diff --git a/doc/steering-optimal.png b/doc/steering-optimal.png
index 6ff50c0f4..f847923b5 100644
Binary files a/doc/steering-optimal.png and b/doc/steering-optimal.png differ
diff --git a/doc/steering-optimal.py b/doc/steering-optimal.py
new file mode 120000
index 000000000..506033ec1
--- /dev/null
+++ b/doc/steering-optimal.py
@@ -0,0 +1 @@
+../examples/steering-optimal.py
\ No newline at end of file
diff --git a/doc/steering-optimal.rst b/doc/steering-optimal.rst
new file mode 100644
index 000000000..777278c1c
--- /dev/null
+++ b/doc/steering-optimal.rst
@@ -0,0 +1,14 @@
+.. _steering-optimal:
+
+Optimal control for vehicle steeering (lane change)
+---------------------------------------------------
+
+Code
+....
+.. literalinclude:: steering-optimal.py
+   :language: python
+   :linenos:
+
+
+Notes
+.....