r"""lqe(A, G, C, QN, RN, [, NN])

# TODO: incorporate cross-covariance NN, something like this,

# which doesn't work for some reason

# if NN is None:

# NN = np.zeros(QN.size(0),RN.size(1))

# NG = G @ NN

#

# Process the arguments and figure out what inputs we received

#

# If we were passed a discrete-time system as the first arg, use dlqe()

if isinstance(args[0], LTI) and isdtime(args[0], strict=True):

# Call dlqe

return dlqe(*args, **kwargs)

# Get the method to use (if specified as a keyword)

method = kwargs.pop('method', None)

if kwargs:

raise TypeError("unrecognized keyword(s): ", str(kwargs))

# Get the system description

if (len(args) < 3):

raise ControlArgument("not enough input arguments")

# If we were passed a state space system, use that to get system matrices

if isinstance(args[0], StateSpace):

A = np.array(args[0].A, ndmin=2, dtype=float)

G = np.array(args[0].B, ndmin=2, dtype=float)

C = np.array(args[0].C, ndmin=2, dtype=float)

index = 1

elif isinstance(args[0], LTI):

# Don't allow other types of LTI systems

raise ControlArgument("LTI system must be in state space form")

else:

# Arguments should be A and B matrices

A = np.array(args[0], ndmin=2, dtype=float)

G = np.array(args[1], ndmin=2, dtype=float)

C = np.array(args[2], ndmin=2, dtype=float)

index = 3

# Get the weighting matrices (converting to matrices, if needed)

QN = np.array(args[index], ndmin=2, dtype=float)

RN = np.array(args[index+1], ndmin=2, dtype=float)

# Get the cross-covariance matrix, if given

if (len(args) > index + 2):

# NN = np.array(args[index+2], ndmin=2, dtype=float)

raise ControlNotImplemented("cross-covariance not implemented")

else:

pass

# For future use (not currently used below)

# NN = np.zeros((QN.shape[0], RN.shape[1]))

# Check dimensions of G (needed before calling care())

_check_shape(QN, G.shape[1], G.shape[1], name="QN")

# Compute the result (dimension and symmetry checking done in care())

P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN, method=method,

_Bs="C", _Qs="QN", _Rs="RN", _Ss="NN")

r"""dlqe(A, G, C, QN, RN, [, N])

#

# Process the arguments and figure out what inputs we received

#

# Get the method to use (if specified as a keyword)

method = kwargs.pop('method', None)

if kwargs:

raise TypeError("unrecognized keyword(s): ", str(kwargs))

# Get the system description

if (len(args) < 3):

raise ControlArgument("not enough input arguments")

# If we were passed a continuous time system as the first arg, raise error

if isinstance(args[0], LTI) and isctime(args[0], strict=True):

raise ControlArgument("dlqr() called with a continuous-time system")

# If we were passed a state space system, use that to get system matrices

if isinstance(args[0], StateSpace):

A = np.array(args[0].A, ndmin=2, dtype=float)

G = np.array(args[0].B, ndmin=2, dtype=float)

C = np.array(args[0].C, ndmin=2, dtype=float)

index = 1

elif isinstance(args[0], LTI):

# Don't allow other types of LTI systems

raise ControlArgument("LTI system must be in state space form")

else:

# Arguments should be A and B matrices

A = np.array(args[0], ndmin=2, dtype=float)

G = np.array(args[1], ndmin=2, dtype=float)

C = np.array(args[2], ndmin=2, dtype=float)

index = 3

# Get the weighting matrices (converting to matrices, if needed)

QN = np.array(args[index], ndmin=2, dtype=float)

RN = np.array(args[index+1], ndmin=2, dtype=float)

# TODO: incorporate cross-covariance NN, something like this,

# which doesn't work for some reason

# if NN is None:

# NN = np.zeros(QN.size(0),RN.size(1))

# NG = G @ NN

if len(args) > index + 2:

# NN = np.array(args[index+2], ndmin=2, dtype=float)

raise ControlNotImplemented("cross-covariance not yet implemented")

# Check dimensions of G (needed before calling care())

_check_shape(QN, G.shape[1], G.shape[1], name="QN")

# Compute the result (dimension and symmetry checking done in dare())

P, E, LT = dare(A.T, C.T, G @ QN @ G.T, RN, method=method,

_Bs="C", _Qs="QN", _Rs="RN", _Ss="NN")

sys, QN, RN, P0=None, G=None, C=None,

control_indices=None, disturbance_indices=None,

estimate_labels='xhat[{i}]', covariance_labels='P[{i},{j}]',

measurement_labels=None, control_labels=None,

inputs=None, outputs=None, states=None, **kwargs):

r"""Create an I/O system implementing a linear quadratic estimator.

# Make sure that we were passed an I/O system as an input

if not isinstance(sys, StateSpace):

raise ControlArgument("Input system must be a linear I/O system")

# Process legacy keywords

estimate_labels = _process_legacy_keyword(

kwargs, 'output_labels', 'estimate_labels', estimate_labels)

measurement_labels = _process_legacy_keyword(

kwargs, 'sensor_labels', 'measurement_labels', measurement_labels)

# Separate state_labels no longer supported => special processing required

if kwargs.get('state_labels'):

if estimate_labels is None:

estimate_labels = _process_legacy_keyword(

kwargs, 'state_labels', estimate_labels)

else:

warnings.warn(

"deprecated 'state_labels' ignored; use 'states' instead")

kwargs.pop('state_labels')

# Set the state matrix for later use

A = sys.A

# Determine the control and disturbance indices

ctrl_idx, dist_idx = _process_control_disturbance_indices(

sys, control_indices, disturbance_indices)

# Set the input and direct matrices

B = sys.B[:, ctrl_idx]

if not np.allclose(sys.D, 0):

raise NotImplementedError("nonzero 'D' matrix not yet implemented")

# Set the output matrices

if C is not None:

# Make sure we have full system output (allowing for numerical errors)

if sys.C.shape[0] != sys.nstates or \

not np.allclose(sys.C, np.eye(sys.nstates)):

raise ValueError("System output must be full state")

# Make sure that the output matches the size of RN

if C.shape[0] != RN.shape[0]:

raise ValueError("System output is the wrong size for C")

else:

# Use the system outputs as the measurements

C = sys.C

# Generate the disturbance matrix (G)

if G is None:

G = sys.B if len(dist_idx) == 0 else sys.B[:, dist_idx]

G = _check_shape(G, sys.nstates, len(dist_idx), name='G')

# Initialize the covariance matrix

if P0 is None:

# Initialize P0 to the steady state value

_, P0, _ = lqe(A, G, C, QN, RN)

P0 = _check_shape(P0, sys.nstates, sys.nstates, symmetric=True, name='P0')

# Figure out the labels to use

estimate_labels = _process_labels(

estimate_labels, 'estimate',

[f'xhat[{i}]' for i in range(sys.nstates)])

outputs = estimate_labels if outputs is None else outputs

if C is None:

# System outputs are the input to the estimator

measurement_labels = _process_labels(

measurement_labels, 'measurement', sys.output_labels)

else:

# Generate labels corresponding to measured values from C

measurement_labels = _process_labels(

measurement_labels, 'measurement',

[f'y[{i}]' for i in range(C.shape[0])])

control_labels = _process_labels(

control_labels, 'control',

[sys.input_labels[i] for i in ctrl_idx])

inputs = measurement_labels + control_labels if inputs is None \

else inputs

# Process the disturbance covariances and check size

QN = _check_shape(QN, G.shape[1], G.shape[1], square=True, name='QN')

RN = _check_shape(RN, C.shape[0], C.shape[0], square=True, name='RN')

if isinstance(covariance_labels, str):

# Generate the list of labels using the argument as a format string

covariance_labels = [

covariance_labels.format(i=i, j=j) \

for i in range(sys.nstates) for j in range(sys.nstates)]

states = estimate_labels + covariance_labels if states is None else states

if isctime(sys):

# Create an I/O system for the state feedback gains

# Note: reshape vectors into column vectors for legacy np.matrix

R_inv = np.linalg.inv(RN)

Reps_inv = C.T @ R_inv @ C

def _estim_update(t, x, u, params):

# See if we are estimating or predicting

correct = params.get('correct', True)

# Get the state of the estimator

xhat = x[0:sys.nstates].reshape(-1, 1)

P = x[sys.nstates:].reshape(sys.nstates, sys.nstates)

# Extract the inputs to the estimator

y = u[0:C.shape[0]].reshape(-1, 1)

u = u[C.shape[0]:].reshape(-1, 1)

# Compute the optimal gain

L = P @ C.T @ R_inv

# Update the state estimate

dxhat = A @ xhat + B @ u # prediction

if correct:

dxhat -= L @ (C @ xhat - y) # correction

# Update the covariance

dP = A @ P + P @ A.T + G @ QN @ G.T

if correct:

dP -= P @ Reps_inv @ P

# Return the update

return np.hstack([dxhat.reshape(-1), dP.reshape(-1)])

else:

def _estim_update(t, x, u, params):

# See if we are estimating or predicting

correct = params.get('correct', True)

# Get the state of the estimator

xhat = x[0:sys.nstates].reshape(-1, 1)

P = x[sys.nstates:].reshape(sys.nstates, sys.nstates)

# Extract the inputs to the estimator

y = u[0:C.shape[0]].reshape(-1, 1)

u = u[C.shape[0]:].reshape(-1, 1)

# Compute the optimal gain

Reps_inv = np.linalg.inv(RN + C @ P @ C.T)

L = A @ P @ C.T @ Reps_inv

# Update the state estimate

dxhat = A @ xhat + B @ u # prediction

if correct:

dxhat -= L @ (C @ xhat - y) # correction

# Update the covariance

dP = A @ P @ A.T + G @ QN @ G.T

if correct:

dP -= A @ P @ C.T @ Reps_inv @ C @ P @ A.T

# Return the update

return np.hstack([dxhat.reshape(-1), dP.reshape(-1)])

def _estim_output(t, x, u, params):

return x[0:sys.nstates]

# Define the estimator system

return NonlinearIOSystem(

_estim_update, _estim_output, dt=sys.dt,

"""Generate a white noise signal with specified intensity.

# Convert input arguments to arrays

T = np.atleast_1d(T)

Q = np.atleast_2d(Q)

# Check the shape of the input arguments

if len(T.shape) != 1:

raise ValueError("Time vector T must be 1D")

if len(Q.shape) != 2 or Q.shape[0] != Q.shape[1]:

raise ValueError("Covariance matrix Q must be square")

# Figure out the time increment

if dt != 0:

# Discrete time system => white noise is not scaled

dt = 1

else:

dt = T[1] - T[0]

# Make sure data points are equally spaced

if not np.allclose(np.diff(T), T[1] - T[0]):

raise ValueError("Time values must be equally spaced.")

# Generate independent white noise sources for each input

W = np.array([

np.random.normal(0, 1/sqrt(dt), T.size) for i in range(Q.shape[0])])

# Return a linear combination of the noise sources

"""Compute the correlation of time signals.

T = np.atleast_1d(T)

X = np.atleast_2d(X)

Y = np.atleast_2d(Y) if Y is not None else X

# Check the shape of the input arguments

if len(T.shape) != 1:

raise ValueError("Time vector T must be 1D")

if len(X.shape) != 2 or len(Y.shape) != 2:

raise ValueError("Signals X and Y must be 2D arrays")

if T.shape[0] != X.shape[1] or T.shape[0] != Y.shape[1]:

raise ValueError("Signals X and Y must have same length as T")

# Figure out the time increment

dt = T[1] - T[0]

# Make sure data points are equally spaced

if not np.allclose(np.diff(T), T[1] - T[0]):

raise ValueError("Time values must be equally spaced.")

# Compute the correlation matrix

R = np.array(

[[sp.signal.correlate(X[i], Y[j])

for i in range(X.shape[0])] for j in range(Y.shape[0])]

) * dt / (T[-1] - T[0])

# From scipy.signal.correlation_lags (for use with older versions)

# tau = sp.signal.correlation_lags(len(X[0]), len(Y[0])) * dt

tau = np.arange(-len(Y[0]) + 1, len(X[0])) * dt