Output feedback control using LQR and extended Kalman filtering

RMM, 14 Feb 2022

This notebook illustrates the implementation of an extended Kalman filter and the use of the estimated state for LQR feedback of a vectored thrust aircraft model.

[1]:

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as patches
import control as ct

System definition

We consider a (planar) vertical takeoff and landing aircraft model:

PVTOL diagram

The dynamics of the system with disturbances on the $x$ and $y$ variables are given by

$\begin{aligned} m \ddot x &= F_1 \cos\theta - F_2 \sin\theta - c \dot x + d_x, \\ m \ddot y &= F_1 \sin\theta + F_2 \cos\theta - c \dot y - m g + d_y, \\ J \ddot \theta &= r F_1. \end{aligned}$

The measured values of the system are the position and orientation, with added noise $n_x$ , $n_y$ , and $n_\theta$ :

$\vec y = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix} + \begin{bmatrix} n_x \\ n_y \\ n_z \end{bmatrix}.$

The dynamics are defined in the pvtol module:

[2]:

# pvtol = nominal system (no disturbances or noise)
# pvtol_noisy = pvtol w/ process disturbances and sensor noise
from pvtol import pvtol, pvtol_noisy, plot_results

# Find the equiblirum point corresponding to the origin
xe, ue = ct.find_eqpt(
    pvtol, np.zeros(pvtol.nstates),
    np.zeros(pvtol.ninputs), [0, 0, 0, 0, 0, 0],
    iu=range(2, pvtol.ninputs), iy=[0, 1])

x0, u0 = ct.find_eqpt(
    pvtol, np.zeros(pvtol.nstates),
    np.zeros(pvtol.ninputs), np.array([2, 1, 0, 0, 0, 0]),
    iu=range(2, pvtol.ninputs), iy=[0, 1])

# Extract the linearization for use in LQR design
pvtol_lin = pvtol.linearize(xe, ue)
A, B = pvtol_lin.A, pvtol_lin.B

print(pvtol, "\n")
print(pvtol_noisy)

<FlatSystem>: pvtol
Inputs (2): ['F1', 'F2']
Outputs (6): ['x0', 'x1', 'x2', 'x3', 'x4', 'x5']
States (6): ['x0', 'x1', 'x2', 'x3', 'x4', 'x5']
Parameters: ['m', 'J', 'r', 'g', 'c']

Update: <function _pvtol_update at 0x13771c0e0>
Output: <function _pvtol_output at 0x13771c360>

Forward: <function _pvtol_flat_forward at 0x13771c400>
Reverse: <function _pvtol_flat_reverse at 0x13771e2a0>

<NonlinearIOSystem>: pvtol_noisy
Inputs (7): ['F1', 'F2', 'Dx', 'Dy', 'Nx', 'Ny', 'Nth']
Outputs (6): ['x0', 'x1', 'x2', 'x3', 'x4', 'x5']
States (6): ['x0', 'x1', 'x2', 'x3', 'x4', 'x5']

Update: <function _noisy_update at 0x13771e7a0>
Output: <function _noisy_output at 0x13771e840>

We also define the properties of the disturbances, noise, and initial conditions:

[3]:

# Disturbance and noise intensities
Qv = np.diag([1e-2, 1e-2])
Qw = np.array([[2e-4, 0, 1e-5], [0, 2e-4, 1e-5], [1e-5, 1e-5, 1e-4]])
Qwinv = np.linalg.inv(Qw)

# Initial state covariance
P0 = np.eye(pvtol.nstates)

Control system design

We start be defining an extended Kalman filter to estimate the state of the system from the measured outputs.

[4]:

# Define the disturbance input and measured output matrices
F = np.array([[0, 0], [0, 0], [0, 0], [1, 0], [0, 1], [0, 0]])
C = np.eye(3, 6)

# Estimator update law
def estimator_update(t, x, u, params):
    # Extract the states of the estimator
    xhat = x[0:pvtol.nstates]
    P = x[pvtol.nstates:].reshape(pvtol.nstates, pvtol.nstates)

    # Extract the inputs to the estimator
    y = u[0:3]                  # just grab the first three outputs
    u = u[3:5]                  # get the inputs that were applied as well

    # Compute the linearization at the current state
    A = pvtol.A(xhat, u)        # A matrix depends on current state
    # A = pvtol.A(xe, ue)       # Fixed A matrix (for testing/comparison)

    # Compute the optimal again
    L = P @ C.T @ Qwinv

    # Update the state estimate
    xhatdot = pvtol.updfcn(t, xhat, u, params) - L @ (C @ xhat - y)

    # Update the covariance
    Pdot = A @ P + P @ A.T - P @ C.T @ Qwinv @ C @ P + F @ Qv @ F.T

    # Return the derivative
    return np.hstack([xhatdot, Pdot.reshape(-1)])

estimator = ct.NonlinearIOSystem(
    estimator_update, lambda t, x, u, params: x[0:pvtol.nstates],
    states=pvtol.nstates + pvtol.nstates**2,
    inputs= pvtol_noisy.state_labels[0:3] \
        + pvtol_noisy.input_labels[0:pvtol.ninputs],
    outputs=[f'xh{i}' for i in range(pvtol.nstates)],
)
print(estimator)

<NonlinearIOSystem>: sys[1]
Inputs (5): ['x0', 'x1', 'x2', 'F1', 'F2']
Outputs (6): ['xh0', 'xh1', 'xh2', 'xh3', 'xh4', 'xh5']
States (42): ['x[0]', 'x[1]', 'x[2]', 'x[3]', 'x[4]', 'x[5]', 'x[6]', 'x[7]', 'x[8]', 'x[9]', 'x[10]', 'x[11]', 'x[12]', 'x[13]', 'x[14]', 'x[15]', 'x[16]', 'x[17]', 'x[18]', 'x[19]', 'x[20]', 'x[21]', 'x[22]', 'x[23]', 'x[24]', 'x[25]', 'x[26]', 'x[27]', 'x[28]', 'x[29]', 'x[30]', 'x[31]', 'x[32]', 'x[33]', 'x[34]', 'x[35]', 'x[36]', 'x[37]', 'x[38]', 'x[39]', 'x[40]', 'x[41]']

Update: <function estimator_update at 0x13771f6a0>
Output: <function <lambda> at 0x13771f7e0>

We now define an LQR controller, using a physically motivated weighting:

[5]:

# Shoot for 1 cm error in x, 10 cm error in y.  Try to keep the angle
# less than 5 degrees in making the adjustments.  Penalize side forces
# due to loss in efficiency.
#

Qx = np.diag([100, 10, (180/np.pi) / 5, 0, 0, 0])
Qu = np.diag([10, 1])
K, _, _ = ct.lqr(A, B, Qx, Qu)

#
# Control system construction: combine LQR w/ EKF
#
# Use the linearization around the origin to design the optimal gains
# to see how they compare to the final value of P for the EKF
#

# Construct the state feedback controller with estimated state as input
statefbk, _ = ct.create_statefbk_iosystem(pvtol, K, estimator=estimator)
print(statefbk, "\n")

# Reconstruct the control system with the noisy version of the process
# Create a closed loop system around the controller
clsys = ct.interconnect(
    [pvtol_noisy, statefbk, estimator],
    inplist = statefbk.input_labels[0:pvtol.ninputs + pvtol.nstates] + \
        pvtol_noisy.input_labels[pvtol.ninputs:],
    inputs = statefbk.input_labels[0:pvtol.ninputs + pvtol.nstates] + \
        pvtol_noisy.input_labels[pvtol.ninputs:],
    outlist = pvtol.output_labels + statefbk.output_labels + estimator.output_labels,
    outputs = pvtol.output_labels + statefbk.output_labels + estimator.output_labels
)
print(clsys)

<StateSpace>: sys[2]
Inputs (14): ['xd[0]', 'xd[1]', 'xd[2]', 'xd[3]', 'xd[4]', 'xd[5]', 'ud[0]', 'ud[1]', 'xh0', 'xh1', 'xh2', 'xh3', 'xh4', 'xh5']
Outputs (2): ['F1', 'F2']
States (0): []

A = []

B = []

C = []

D = [[-3.16227766e+00 -1.31948922e-07  8.67680175e+00 -2.35855555e+00
      -6.98881821e-08  1.91220852e+00  1.00000000e+00  0.00000000e+00
       3.16227766e+00  1.31948922e-07 -8.67680175e+00  2.35855555e+00
       6.98881821e-08 -1.91220852e+00]
     [-1.31948921e-06  3.16227766e+00 -2.32324826e-07 -2.36396240e-06
       4.97998224e+00  7.90913276e-08  0.00000000e+00  1.00000000e+00
       1.31948921e-06 -3.16227766e+00  2.32324826e-07  2.36396240e-06
      -4.97998224e+00 -7.90913276e-08]]

<InterconnectedSystem>: sys[3]
Inputs (13): ['xd[0]', 'xd[1]', 'xd[2]', 'xd[3]', 'xd[4]', 'xd[5]', 'ud[0]', 'ud[1]', 'Dx', 'Dy', 'Nx', 'Ny', 'Nth']
Outputs (14): ['x0', 'x1', 'x2', 'x3', 'x4', 'x5', 'F1', 'F2', 'xh0', 'xh1', 'xh2', 'xh3', 'xh4', 'xh5']
States (48): ['pvtol_noisy_x0', 'pvtol_noisy_x1', 'pvtol_noisy_x2', 'pvtol_noisy_x3', 'pvtol_noisy_x4', 'pvtol_noisy_x5', 'sys[1]_x[0]', 'sys[1]_x[1]', 'sys[1]_x[2]', 'sys[1]_x[3]', 'sys[1]_x[4]', 'sys[1]_x[5]', 'sys[1]_x[6]', 'sys[1]_x[7]', 'sys[1]_x[8]', 'sys[1]_x[9]', 'sys[1]_x[10]', 'sys[1]_x[11]', 'sys[1]_x[12]', 'sys[1]_x[13]', 'sys[1]_x[14]', 'sys[1]_x[15]', 'sys[1]_x[16]', 'sys[1]_x[17]', 'sys[1]_x[18]', 'sys[1]_x[19]', 'sys[1]_x[20]', 'sys[1]_x[21]', 'sys[1]_x[22]', 'sys[1]_x[23]', 'sys[1]_x[24]', 'sys[1]_x[25]', 'sys[1]_x[26]', 'sys[1]_x[27]', 'sys[1]_x[28]', 'sys[1]_x[29]', 'sys[1]_x[30]', 'sys[1]_x[31]', 'sys[1]_x[32]', 'sys[1]_x[33]', 'sys[1]_x[34]', 'sys[1]_x[35]', 'sys[1]_x[36]', 'sys[1]_x[37]', 'sys[1]_x[38]', 'sys[1]_x[39]', 'sys[1]_x[40]', 'sys[1]_x[41]']

Subsystems (3):
 * <NonlinearIOSystem pvtol_noisy: ['F1', 'F2', 'Dx', 'Dy', 'Nx', 'Ny', 'Nth']
    -> ['x0', 'x1', 'x2', 'x3', 'x4', 'x5']>
 * <StateSpace sys[2]: ['xd[0]', 'xd[1]', 'xd[2]', 'xd[3]', 'xd[4]', 'xd[5]',
    'ud[0]', 'ud[1]', 'xh0', 'xh1', 'xh2', 'xh3', 'xh4', 'xh5'] -> ['F1',
    'F2']>
 * <NonlinearIOSystem sys[1]: ['x0', 'x1', 'x2', 'F1', 'F2'] -> ['xh0', 'xh1',
    'xh2', 'xh3', 'xh4', 'xh5']>

Connections:
 * pvtol_noisy.F1 <- sys[2].F1
 * pvtol_noisy.F2 <- sys[2].F2
 * pvtol_noisy.Dx <- Dx
 * pvtol_noisy.Dy <- Dy
 * pvtol_noisy.Nx <- Nx
 * pvtol_noisy.Ny <- Ny
 * pvtol_noisy.Nth <- Nth
 * sys[2].xd[0] <- xd[0]
 * sys[2].xd[1] <- xd[1]
 * sys[2].xd[2] <- xd[2]
 * sys[2].xd[3] <- xd[3]
 * sys[2].xd[4] <- xd[4]
 * sys[2].xd[5] <- xd[5]
 * sys[2].ud[0] <- ud[0]
 * sys[2].ud[1] <- ud[1]
 * sys[2].xh0 <- sys[1].xh0
 * sys[2].xh1 <- sys[1].xh1
 * sys[2].xh2 <- sys[1].xh2
 * sys[2].xh3 <- sys[1].xh3
 * sys[2].xh4 <- sys[1].xh4
 * sys[2].xh5 <- sys[1].xh5
 * sys[1].x0 <- pvtol_noisy.x0
 * sys[1].x1 <- pvtol_noisy.x1
 * sys[1].x2 <- pvtol_noisy.x2
 * sys[1].F1 <- sys[2].F1
 * sys[1].F2 <- sys[2].F2

Outputs:
 * x0 <- pvtol_noisy.x0
 * x1 <- pvtol_noisy.x1
 * x2 <- pvtol_noisy.x2
 * x3 <- pvtol_noisy.x3
 * x4 <- pvtol_noisy.x4
 * x5 <- pvtol_noisy.x5
 * F1 <- sys[2].F1
 * F2 <- sys[2].F2
 * xh0 <- sys[1].xh0
 * xh1 <- sys[1].xh1
 * xh2 <- sys[1].xh2
 * xh3 <- sys[1].xh3
 * xh4 <- sys[1].xh4
 * xh5 <- sys[1].xh5

Simulations

We now simulate the response of the system, starting with an instantiation of the noise:

[6]:

# Create the time vector for the simulation
Tf = 10
T = np.linspace(0, Tf, 1000)

# Create representative process disturbance and sensor noise vectors
np.random.seed(117)           # avoid figures changing from run to run
V = ct.white_noise(T, Qv) # smaller disturbances and noise then design
W = ct.white_noise(T, Qw)
plt.plot(T, V[0], label="V[0]")
plt.plot(T, W[0], label="W[0]")
plt.legend();

../_images/examples_pvtol-outputfbk_12_0.png

LQR with EKF

[7]:

# Put together the input for the system
U = np.vstack([
    np.outer(xe, np.ones_like(T)),      # xd
    np.outer(ue, np.ones_like(T)),      # ud
    V, W                                # disturbances and noise
])
X0 = np.hstack([x0, np.zeros(pvtol.nstates), P0.reshape(-1)])

# Initial condition response
resp = ct.input_output_response(clsys, T, U, X0)

# Plot the response
plot_results(T, resp.states, resp.outputs[pvtol.nstates:])

../_images/examples_pvtol-outputfbk_14_0.png

[8]:

# Response of the first two states, including internal estimates
plt.figure()
h1, = plt.plot(resp.time, resp.outputs[0], 'b-', linewidth=0.75)
h2, = plt.plot(resp.time, resp.outputs[1], 'r-', linewidth=0.75)

# Add on the internal estimator states
xh0 = clsys.find_output('xh0')
xh1 = clsys.find_output('xh1')
h3, = plt.plot(resp.time, resp.outputs[xh0], 'k--')
h4, = plt.plot(resp.time, resp.outputs[xh1], 'k--')

plt.plot([0, 10], [0, 0], 'k--', linewidth=0.5)
plt.ylabel("Position $x$, $y$ [m]")
plt.xlabel("Time $t$ [s]")
plt.legend(
    [h1, h2, h3, h4], ['$x$', '$y$', '$\hat{x}$', '$\hat{y}$'],
    loc='upper right', frameon=False, ncol=2)

<>:16: SyntaxWarning: invalid escape sequence '\h'
<>:16: SyntaxWarning: invalid escape sequence '\h'
<>:16: SyntaxWarning: invalid escape sequence '\h'
<>:16: SyntaxWarning: invalid escape sequence '\h'
/var/folders/3h/8vlrqzts6wnd_p5xvy01zclc0000gn/T/ipykernel_62492/1696903767.py:16: SyntaxWarning: invalid escape sequence '\h'
  [h1, h2, h3, h4], ['$x$', '$y$', '$\hat{x}$', '$\hat{y}$'],
/var/folders/3h/8vlrqzts6wnd_p5xvy01zclc0000gn/T/ipykernel_62492/1696903767.py:16: SyntaxWarning: invalid escape sequence '\h'
  [h1, h2, h3, h4], ['$x$', '$y$', '$\hat{x}$', '$\hat{y}$'],

[8]:

<matplotlib.legend.Legend at 0x137f30190>

../_images/examples_pvtol-outputfbk_15_2.png

Full state feedback

As a comparison, we can investigate the response of the system if the exact state was available:

[9]:

# Compute the full state feedback solution
lqr_ctrl, _ = ct.create_statefbk_iosystem(pvtol, K)

lqr_clsys = ct.interconnect(
    [pvtol_noisy, lqr_ctrl],
    inplist = lqr_ctrl.input_labels[0:pvtol.ninputs + pvtol.nstates] + \
        pvtol_noisy.input_labels[pvtol.ninputs:],
    inputs = lqr_ctrl.input_labels[0:pvtol.ninputs + pvtol.nstates] + \
        pvtol_noisy.input_labels[pvtol.ninputs:],
    outlist = pvtol.output_labels + lqr_ctrl.output_labels,
    outputs = pvtol.output_labels + lqr_ctrl.output_labels
)

# Put together the input for the system
U = np.vstack([
    np.outer(xe, np.ones_like(T)),      # xd
    np.outer(ue, np.ones_like(T)),      # ud
    V, W * 0                            # disturbances and noise
])

# Run a simulation with full state feedback
lqr_resp = ct.input_output_response(lqr_clsys, T, U, x0)

# Compare the results
plt.plot(resp.states[0], resp.states[1], 'b-', label="Extended KF")
plt.plot(lqr_resp.states[0], lqr_resp.states[1], 'r-', label="Full state")

plt.xlabel('$x$ [m]')
plt.ylabel('$y$ [m]')
plt.axis('equal')
plt.legend(frameon=False);

/Users/murray/Library/CloudStorage/Dropbox/macosx/src/python-control/murrayrm/control/statefbk.py:788: UserWarning: cannot verify system output is system state
  warnings.warn("cannot verify system output is system state")

../_images/examples_pvtol-outputfbk_17_1.png

[ ]: