initial implementation of optimal estimation problem

murrayrm · murrayrm · commit cf4510911f57 · 2023-03-18T10:36:19.000-07:00
diff --git a/control/optimal.py b/control/optimal.py
diff --git a/control/stochsys.py b/control/stochsys.py
@@ -354,8 +354,8 @@ def create_estimator_iosystem(
         Disturbance matrix describing how the disturbances enters the
         dynamics.  Defaults to sys.B.
     C : ndarray, optional
-        If the system has all full states output, define the measured values
-        to be used by the estimator.  Otherwise, use the system output as the
+        If the system has full state output, define the measured values to
+        be used by the estimator.  Otherwise, use the system output as the
         measured values.
     {state, covariance, sensor, output}_labels : str or list of str, optional
         Set the name of the signals to use for the internal state, covariance,
diff --git a/control/tests/kwargs_test.py b/control/tests/kwargs_test.py
@@ -196,6 +196,7 @@ def test_matplotlib_kwargs(function, nsysargs, moreargs, kwargs, mplcleanup):
         flatsys_test.TestFlatSys.test_solve_flat_ocp_errors,
     'optimal.create_mpc_iosystem': optimal_test.test_mpc_iosystem_rename,
     'optimal.solve_ocp': optimal_test.test_ocp_argument_errors,
+    'optimal.solve_oep': optimal_test.test_oep_argument_errors,
     'FrequencyResponseData.__init__':
         frd_test.TestFRD.test_unrecognized_keyword,
     'InputOutputSystem.__init__': test_unrecognized_kwargs,
@@ -217,7 +218,9 @@ def test_matplotlib_kwargs(function, nsysargs, moreargs, kwargs, mplcleanup):
     'optimal.OptimalControlProblem.compute_trajectory':
         optimal_test.test_ocp_argument_errors,
     'optimal.OptimalControlProblem.create_mpc_iosystem':
-        optimal_test.test_mpc_iosystem_rename,
+        optimal_test.test_ocp_argument_errors,
+    'optimal.OptimalEstimationProblem.__init__':
+        optimal_test.test_oep_argument_errors,
 }
 
 #
@@ -234,9 +237,6 @@ def test_matplotlib_kwargs(function, nsysargs, moreargs, kwargs, mplcleanup):
     control.freqplot._add_arrows_to_line2D,             # RMM, 18 Nov 2022
     control.namedio._process_dt_keyword,                # RMM, 13 Nov 2022
     control.namedio._process_namedio_keywords,          # RMM, 18 Nov 2022
-    control.optimal.OptimalControlProblem.__init__,     # RMM, 18 Nov 2022
-    control.optimal.solve_ocp,                          # RMM, 18 Nov 2022
-    control.optimal.create_mpc_iosystem,                # RMM, 18 Nov 2022
 }
 
 @pytest.mark.parametrize("module", [control, control.flatsys])
diff --git a/control/tests/optimal_test.py b/control/tests/optimal_test.py
@@ -529,7 +529,7 @@ def test_ocp_argument_errors():
     with pytest.raises(TypeError, match="unrecognized keyword"):
         ocp = opt.OptimalControlProblem(sys, time, cost, constraints)
         ocp.compute_trajectory(x0, unknown=None)
-            
+
     # Unrecognized trajectory constraint type
     constraints = [(None, np.eye(3), [0, 0, 0], [0, 0, 0])]
     with pytest.raises(TypeError, match="unknown trajectory constraint type"):
@@ -771,3 +771,18 @@ def vehicle_output(t, x, u, params):
                     np.testing.assert_almost_equal(y[:,-1], xf, decimal=1)
             else:
                 np.testing.assert_almost_equal(y[:,-1], xf, decimal=1)
+
+
+def test_oep_argument_errors():
+    sys = ct.rss(4, 2, 2)
+    timepts = np.linspace(0, 1, 10)
+    Y = np.zeros((2, timepts.size))
+    U = np.zeros_like(timepts)
+    cost = opt.gaussian_likelihood_cost(sys, np.eye(1), np.eye(2))
+
+    # Unrecognized arguments
+    with pytest.raises(TypeError, match="unrecognized keyword"):
+        res = opt.solve_oep(sys, timepts, Y, U, cost, unknown=True)
+
+    with pytest.raises(TypeError, match="unrecognized keyword"):
+        oep = opt.OptimalEstimationProblem(sys, timepts, cost, unknown=True)
diff --git a/control/tests/stochsys_test.py b/control/tests/stochsys_test.py
@@ -6,6 +6,7 @@
 from control.tests.conftest import asmatarrayout
 
 import control as ct
+import control.optimal as opt
 from control import lqe, dlqe, rss, drss, tf, ss, ControlArgument, slycot_check
 from math import log, pi
 
@@ -317,3 +318,180 @@ def test_correlation():
     with pytest.raises(ValueError, match="Time values must be equally"):
         T = np.logspace(0, 2, T.size)
         tau, Rtau = ct.correlation(T, V)
+
+@pytest.mark.parametrize('dt', [0, 1])
+def test_oep(dt):
+    # Define the system to test, with additional input
+    csys = ct.ss(
+        [[-0.5, 1, 0, 0], [0, -1, 1, 0], [0, 0, -2, 1], [0, 0, 0, -3]], # A
+        [[0, 0.1], [0, 0.1], [0, 0.1], [1, 0.1]],                       # B
+        [[1, 0, 0, 0], [0, 0, 1, 0]],                                   # C
+        0, dt=0)
+    dsys = ct.c2d(csys, dt)
+    sys = csys if dt == 0 else dsys
+
+    # Create disturbances and noise (fixed, to avoid random errors)
+    Rv = 0.1 * np.eye(1)                # scalar disturbance
+    Rw = 0.01 * np.eye(sys.noutputs)
+    timepts = np.arange(0, 10.1, 1)
+    V = np.array(
+        [0 if t % 2 == 1 else 1 if t % 4 == 0 else -1 for t in timepts]
+    ).reshape(1, -1) * 0.1
+    W = np.vstack([np.sin(2*timepts), np.cos(3*timepts)]) * 1e-3
+
+    # Generate system data
+    U = np.sin(timepts)
+
+    # No disturbances
+    res0 = ct.input_output_response(sys, timepts, [U, V*0])
+    Y0 = res0.outputs
+
+    # With disturbances and noise
+    res1 = ct.input_output_response(sys, timepts, [U, V])
+    Y1 = res1.outputs + W
+
+    #
+    # Internal testing to make sure all our functions are OK
+    #
+
+    # Set up optimal estimation function using Gaussian likelihoods for cost
+    traj_cost = opt.gaussian_likelihood_cost(sys, Rv, Rw)
+    init_cost = lambda xhat, x: (xhat - x) @ (xhat - x)
+    oep = opt.OptimalEstimationProblem(
+        sys, timepts, traj_cost, terminal_cost=init_cost)
+
+    # _cost_function
+    oep.compute_estimate(res0.outputs, U, X0=0)
+    assert oep._cost_function(np.hstack(
+        [res0.states.reshape(-1), V.reshape(-1) * 0])) == 0
+    assert oep._cost_function(np.hstack(
+        [res0.states.reshape(-1), V.reshape(-1)])) != 0
+    if sys.isdtime():
+        # Collocation contstraint should be satisified for discrete time
+        np.testing.assert_allclose(oep._collocation_constraint(
+            np.hstack([res0.states.reshape(-1), V.reshape(-1) * 0])), 0)
+
+    # _compute_states_inputs: states and inputs with no noise
+    oep.compute_estimate(Y0, U)
+    xhat, u, v, w = oep._compute_states_inputs(
+        np.hstack([res0.states.reshape(-1), V.reshape(-1) * 0]))
+    np.testing.assert_allclose(xhat, res0.states)
+    np.testing.assert_allclose(u, U.reshape(1, -1))
+    np.testing.assert_allclose(v, 0)
+    np.testing.assert_allclose(w, 0)
+
+    # _compute_states_inputs: states and inputs with no noise
+    oep.compute_estimate(Y1, U)
+    xhat, u, v, w = oep._compute_states_inputs(
+        np.hstack([res1.states.reshape(-1), V.reshape(-1)]))
+    np.testing.assert_allclose(xhat, res1.states)
+    np.testing.assert_allclose(u, U.reshape(1, -1))
+    np.testing.assert_allclose(v, V)
+    np.testing.assert_allclose(w, W)
+
+    #
+    # oep.compute_estimate testing
+    #
+
+    # Noise free and disturbance free
+    nonoise_cost = opt.gaussian_likelihood_cost(sys, Rv, Rw/1e6)
+    oep0 = opt.OptimalEstimationProblem(
+        sys, timepts, nonoise_cost, terminal_cost=init_cost)
+    est0 = oep0.compute_estimate(Y0, U)
+    if sys.isdtime():
+         # Full state estimate should be near perfect
+        np.testing.assert_allclose(
+            est0.states[:, -1], res0.states[:, -1], atol=1e-3, rtol=1e-3)
+        np.testing.assert_allclose(est0.inputs, 0, atol=1e-2, rtol=1e-3)
+        np.testing.assert_allclose(est0.outputs, 0, atol=1e-2, rtol=1e-3)
+    else:
+        # Estimate at end of trajectory should be very close
+        assert est0.success
+        np.testing.assert_allclose(
+            est0.states[:, -1], res0.states[:, -1], atol=1e-2, rtol=1e-2)
+
+    # Noise free, but with disturbances and good initial guess
+    oep1 = opt.OptimalEstimationProblem(
+        sys, timepts, nonoise_cost, terminal_cost=init_cost)
+    est1 = oep1.compute_estimate(
+        res1.outputs, U, initial_guess=(res1.states, V), X0=0)
+    np.testing.assert_allclose(
+          est1.states[:, -1], res1.states[:, -1], atol=1e-2, rtol=1e-2)
+    if sys.isdtime():
+        # For discrete time, estimated disturbance and noise should be close
+        np.testing.assert_allclose(
+            est1.inputs[:-1], V[:-1], atol=1e-2, rtol=1e-2)
+        np.testing.assert_allclose(est1.outputs, 0, atol=1e-2, rtol=1e-2)
+
+    # Noise and disturbances (the standard case)
+    est2 = oep.compute_estimate(Y1, U)         # back to original OEP
+    assert est2.success
+    np.testing.assert_allclose(
+        est2.states[:, -1], res1.states[:, -1], atol=1e-1, rtol=1e-2)
+
+    #
+    # Disturbance constraints
+    #
+
+    V3 = np.clip(V, 0.5, 1)
+    traj_constraint = opt.disturbance_range_constraint(sys, 0.5, 1)
+    oep3 = opt.OptimalEstimationProblem(
+        sys, timepts, traj_cost, terminal_cost=init_cost,
+        trajectory_constraints=traj_constraint)
+
+    res3 = ct.input_output_response(sys, timepts, [U, V3])
+    Y3 = res3.outputs + W
+
+    # Make sure the constraint screws up the estimation problem
+    with pytest.raises(AssertionError):
+        est3 = oep.compute_estimate(Y3, U)
+        np.testing.assert_allclose(
+            est3.states[:, -1], res3.states[:, -1], atol=1e-1, rtol=1e-2)
+
+    # Make sure estimation is correct with constraint in place
+    est3 = oep3.compute_estimate(Y3, U)
+    assert est3.success
+    np.testing.assert_allclose(
+        est3.states[:, -1], res3.states[:, -1], atol=1e-1, rtol=1e-2)
+
+
+def test_mhe():
+    # Define the system to test, with additional input
+    csys = ct.ss(
+        [[-0.5, 1, 0, 0], [0, -1, 1, 0], [0, 0, -2, 1], [0, 0, 0, -3]], # A
+        [[0, 0.1], [0, 0.1], [0, 0.1], [1, 0.1]],                       # B
+        [[1, 0, 0, 0], [0, 0, 1, 0]],                                   # C
+        0, dt=0)
+    dt = 0.1
+    sys = ct.c2d(csys, dt)
+
+    # Create disturbances and noise (fixed, to avoid random errors)
+    Rv = 0.1 * np.eye(1)                # scalar disturbance
+    Rw = 1e-6 * np.eye(sys.noutputs)
+    P0 = 0.1 * np.eye(sys.nstates)
+
+    timepts = np.arange(0, 10*dt, dt)
+    mhe_timepts = np.arange(0, 5*dt, dt)
+    V = np.array(
+        [0 if i % 2 == 1 else 1 if i % 4 == 0 else -1
+         for i, t in enumerate(timepts)]).reshape(1, -1) * 0.1
+    W = np.sin(timepts / dt) * 1e-3
+
+    # Create a moving horizon estimator
+    traj_cost = opt.gaussian_likelihood_cost(sys, Rv, Rw)
+    init_cost = lambda xhat, x: (xhat - x) @ P0 @ (xhat - x)
+    oep = opt.OptimalEstimationProblem(
+        sys, mhe_timepts, traj_cost, terminal_cost=init_cost)
+    mhe = oep.create_mhe_iosystem(1)
+
+    # Generate system data
+    U = 10 * np.sin(timepts / (4*dt))
+    inputs = np.vstack([U, V])
+    resp = ct.input_output_response(sys, timepts, inputs)
+
+    # Run the estimator
+    estp = ct.input_output_response(
+        mhe, timepts, [resp.outputs, resp.inputs[0:1]])
+
+    # Make sure the estimated state is close to the actual state
+    np.testing.assert_allclose(estp.outputs, resp.states, atol=1e-2, rtol=1e-4)
diff --git a/doc/conf.py b/doc/conf.py
@@ -127,7 +127,10 @@
 # Add any paths that contain custom static files (such as style sheets) here,
 # relative to this directory. They are copied after the builtin static files,
 # so a file named "default.css" will overwrite the builtin "default.css".
-# html_static_path = ['_static']
+
+html_static_path = ['_static']
+def setup(app):
+    app.add_css_file('css/custom.css')
 
 # Custom sidebar templates, must be a dictionary that maps document names
 # to template names.
diff --git a/doc/optimal.rst b/doc/optimal.rst
