.. DO NOT EDIT.
.. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY.
.. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE:
.. "api/gallery/control_systems/optimal/plot_01_kalman_lqr.py"
.. LINE NUMBERS ARE GIVEN BELOW.

.. only:: html

    .. note::
        :class: sphx-glr-download-link-note

        :ref:`Go to the end <sphx_glr_download_api_gallery_control_systems_optimal_plot_01_kalman_lqr.py>`
        to download the full example code or to run this example in your browser via JupyterLite.

.. rst-class:: sphx-glr-example-title

.. _sphx_glr_api_gallery_control_systems_optimal_plot_01_kalman_lqr.py:


Kalman's linear-quadratic regulator (1960)
==========================================

Rudolf Kalman (1960) asked for the state feedback :math:`u = -Kx` that
minimises :math:`J = \int_0^\infty (x^\mathsf{T}Qx + u^\mathsf{T}Ru)\,dt`.
The answer is :math:`K = R^{-1}B^\mathsf{T}P`, where :math:`P` solves an
algebraic Riccati equation, and the minimum cost from :math:`x_0` is
:math:`x_0^\mathsf{T}Px_0` (:func:`~engineeringkit.control_systems.lqr`).
The weights replace pole locations as the designer's knobs: a cheaper
input (smaller :math:`R`) buys a faster response.

For the double integrator :math:`\ddot y = u` with :math:`Q =
\mathrm{diag}(1, 0)` and :math:`R = r`, the gain is
:math:`K = [r^{-1/2},\ \sqrt2\,r^{-1/4}]`: a closed-loop damping ratio of
:math:`1/\sqrt2` for every :math:`r`.

.. GENERATED FROM PYTHON SOURCE LINES 20-54

.. code-block:: Python

    import matplotlib.pyplot as plt
    import numpy as np
    from scipy.integrate import solve_ivp

    from engineeringkit.control_systems import discretize_zoh, dlqr, lqr

    A = np.array([[0.0, 1.0], [0.0, 0.0]])
    B = np.array([[0.0], [1.0]])
    Q = np.diag([1.0, 0.0])
    x0 = np.array([1.0, 0.0])

    fig, (ax1, ax2) = plt.subplots(2, 1, sharex=True, figsize=(7, 6))
    for r in (0.01, 0.1, 1.0):
        K, P, closed = lqr(A, B, Q, [[r]])
        assert np.allclose(K, [[r**-0.5, np.sqrt(2) * r**-0.25]])
        zeta = -closed.real[0] / np.abs(closed[0])
        assert np.isclose(zeta, 1 / np.sqrt(2))

        # Simulate the closed loop, integrating the cost alongside the state.
        def rhs(t, z, K=K, r=r):
            x = z[:2]
            u = -(K @ x)
            return np.concatenate((A @ x + B @ u, [x @ Q @ x + r * u[0] ** 2]))

        sol = solve_ivp(rhs, (0, 40), np.concatenate((x0, [0.0])), rtol=1e-10, atol=1e-12, dense_output=True)
        cost = sol.y[2, -1]
        print(f"r = {r:5.2f}: K = {K.round(3).tolist()}, J = {cost:.5f}, x0'Px0 = {x0 @ P @ x0:.5f}")
        assert np.isclose(cost, x0 @ P @ x0, rtol=1e-6)

        t = np.linspace(0, 8, 400)
        x = sol.sol(t)[:2]
        ax1.plot(t, x[0], label=f"r = {r}")
        ax2.plot(t, -(K @ x)[0], label=f"r = {r}")




.. image-sg:: /api/gallery/control_systems/optimal/images/sphx_glr_plot_01_kalman_lqr_001.png
   :alt: plot 01 kalman lqr
   :srcset: /api/gallery/control_systems/optimal/images/sphx_glr_plot_01_kalman_lqr_001.png
   :class: sphx-glr-single-img


.. rst-class:: sphx-glr-script-out

 .. code-block:: none

    r =  0.01: K = [[10.0, 4.472]], J = 0.44721, x0'Px0 = 0.44721
    r =  0.10: K = [[3.162, 2.515]], J = 0.79527, x0'Px0 = 0.79527
    r =  1.00: K = [[1.0, 1.414]], J = 1.41421, x0'Px0 = 1.41421




.. GENERATED FROM PYTHON SOURCE LINES 55-62

The discrete regulator converges to the continuous one
------------------------------------------------------

Bellman's dynamic programming over sampling periods gives the discrete
Riccati equation (:func:`~engineeringkit.control_systems.dlqr`). With the
weights scaled by the period, its gain tends to the continuous gain as the
period shrinks.

.. GENERATED FROM PYTHON SOURCE LINES 62-75

.. code-block:: Python


    K, _, _ = lqr(A, B, Q, [[0.1]])
    for dt in (0.1, 0.01, 0.001):
        Ad, Bd = discretize_zoh(A, B, dt)
        Kd, _, _ = dlqr(Ad, Bd, Q * dt, [[0.1 * dt]])
        print(f"dt = {dt}: discrete gain {Kd.round(4).tolist()}")
    assert np.allclose(Kd, K, rtol=2e-3)

    ax1.set(ylabel="position y", title="LQR from y(0) = 1")
    ax2.set(xlabel="time [s]", ylabel="input u")
    ax1.legend()

    plt.show()




.. rst-class:: sphx-glr-script-out

 .. code-block:: none

    dt = 0.1: discrete gain [[2.7889, 2.3617]]
    dt = 0.01: discrete gain [[3.1228, 2.4991]]
    dt = 0.001: discrete gain [[3.1583, 2.5133]]





.. rst-class:: sphx-glr-timing

   **Total running time of the script:** (0 minutes 0.069 seconds)


.. _sphx_glr_download_api_gallery_control_systems_optimal_plot_01_kalman_lqr.py:

.. only:: html

  .. container:: sphx-glr-footer sphx-glr-footer-example

    .. container:: lite-badge

      .. image:: images/jupyterlite_badge_logo.svg
        :target: ../../../../lite/lab/index.html?path=api/gallery/control_systems/optimal/plot_01_kalman_lqr.ipynb
        :alt: Launch JupyterLite
        :width: 150 px

    .. container:: sphx-glr-download sphx-glr-download-jupyter

      :download:`Download Jupyter notebook: plot_01_kalman_lqr.ipynb <plot_01_kalman_lqr.ipynb>`

    .. container:: sphx-glr-download sphx-glr-download-python

      :download:`Download Python source code: plot_01_kalman_lqr.py <plot_01_kalman_lqr.py>`

    .. container:: sphx-glr-download sphx-glr-download-zip

      :download:`Download zipped: plot_01_kalman_lqr.zip <plot_01_kalman_lqr.zip>`


.. only:: html

 .. rst-class:: sphx-glr-signature

    `Gallery generated by Sphinx-Gallery <https://sphinx-gallery.github.io>`_