Notebook

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.

In [11]:

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 aircraf 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:

In [12]:

# 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)

Object: pvtol
Inputs (2): F1, F2, 
Outputs (6): x0, x1, x2, x3, x4, x5, 
States (6): x0, x1, x2, x3, x4, x5,  

Object: 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,

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

In [13]:

# 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.

In [14]:

# 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)

Object: sys[3]
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],

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

In [15]:

# 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)

Object: control
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.31948924e-07  8.67680175e+00 -2.35855555e+00
      -6.98881806e-08  1.91220852e+00  1.00000000e+00  0.00000000e+00
       3.16227766e+00  1.31948924e-07 -8.67680175e+00  2.35855555e+00
       6.98881806e-08 -1.91220852e+00]
     [-1.31948923e-06  3.16227766e+00 -2.32324805e-07 -2.36396241e-06
       4.97998224e+00  7.90913288e-08  0.00000000e+00  1.00000000e+00
       1.31948923e-06 -3.16227766e+00  2.32324805e-07  2.36396241e-06
      -4.97998224e+00 -7.90913288e-08]]
 

Object: xh5
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[3]_x[0], sys[3]_x[1], sys[3]_x[2], sys[3]_x[3], sys[3]_x[4], sys[3]_x[5], sys[3]_x[6], sys[3]_x[7], sys[3]_x[8], sys[3]_x[9], sys[3]_x[10], sys[3]_x[11], sys[3]_x[12], sys[3]_x[13], sys[3]_x[14], sys[3]_x[15], sys[3]_x[16], sys[3]_x[17], sys[3]_x[18], sys[3]_x[19], sys[3]_x[20], sys[3]_x[21], sys[3]_x[22], sys[3]_x[23], sys[3]_x[24], sys[3]_x[25], sys[3]_x[26], sys[3]_x[27], sys[3]_x[28], sys[3]_x[29], sys[3]_x[30], sys[3]_x[31], sys[3]_x[32], sys[3]_x[33], sys[3]_x[34], sys[3]_x[35], sys[3]_x[36], sys[3]_x[37], sys[3]_x[38], sys[3]_x[39], sys[3]_x[40], sys[3]_x[41],

Simulations¶

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

In [16]:

# 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();

LQR with EKF¶

In [17]:

# 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:])

In [18]:

# 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)

Out[18]:

<matplotlib.legend.Legend at 0x7fa431d8b3a0>

Full state feedback¶

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

In [19]:

# 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);

In [ ]: