Skip to content

Implement okid #1031

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 2 commits into
base: main
Choose a base branch
from
Draft

Implement okid #1031

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
299dd67
Add okid, add okid example
KybernetikJo Jul 14, 2024
5abbfe1
Add sphinx doc
KybernetikJo Feb 4, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
228 changes: 226 additions & 2 deletions control/modelsimp.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,9 @@
from .timeresp import TimeResponseData

__all__ = ['hankel_singular_values', 'balanced_reduction', 'model_reduction',
'minimal_realization', 'eigensys_realization', 'markov', 'hsvd',
'balred', 'modred', 'minreal', 'era']
'minimal_realization', 'eigensys_realization',
'observer_kalman_identification', 'markov', 'hsvd', 'balred',
'modred', 'minreal', 'era', 'okid']


# Hankel Singular Value Decomposition
Expand Down Expand Up @@ -724,9 +725,232 @@ def markov(*args, m=None, transpose=False, dt=None, truncate=False):
# Return the first m Markov parameters
return H if not transpose else np.transpose(H)

def observer_kalman_identification(*args, m=None, transpose=False, dt=True, truncate=False):
"""observer_kalman_identification(Y, U, [, m])

Calculate the first `m` Markov parameters [D CB CAB ...]
from data.

This function computes the Markov parameters for a discrete time system

.. math::

x[k+1] &= A x[k] + B u[k] \\\\
y[k] &= C x[k] + D u[k]

given data for u and y. The algorithm assumes that that C A^k B = 0 for
k > m-2 (see [1]_, [2]_). Note that the problem is ill-posed if the length of
the input data is less than the desired number of Markov parameters (a
warning message is generated in this case).

The function can be called with either 1, 2 or 3 arguments:

* ``H = observer_kalman_identification(data)``
* ``H = observer_kalman_identification(data, m)``
* ``H = observer_kalman_identification(Y, U)``
* ``H = observer_kalman_identification(Y, U, m)``

where `data` is an `TimeResponseData` object, and `Y`, `U`, are 1D or 2D
array and m is an integer.

Parameters
----------
Y : array_like
Output data. If the array is 1D, the system is assumed to be
single input. If the array is 2D and transpose=False, the columns
of `Y` are taken as time points, otherwise the rows of `Y` are
taken as time points.
U : array_like
Input data, arranged in the same way as `Y`.
data : TimeResponseData
Response data from which the Markov parameters where estimated.
Input and output data must be 1D or 2D array.
m : int, optional
Number of Markov parameters to output. Defaults to len(U).
dt : True of float, optional
True indicates discrete time with unspecified sampling time and a
positive float is discrete time with the specified sampling time.
It can be used to scale the Markov parameters in order to match
the unit-area impulse response of python-control. Default is True
for array_like and dt=data.time[1]-data.time[0] for
TimeResponseData as input.
truncate : bool, optional
Do not use first m equation for least least squares. Default is False.
transpose : bool, optional
Assume that input data is transposed relative to the standard
:ref:`time-series-convention`. For TimeResponseData this parameter
is ignored. Default is False.

Returns
-------
H : ndarray
First m Markov parameters, [D CB CAB ...].

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should we add a "See Also" section and reference markov? Perhaps also a note here (and in markov) about how this differs from the markov command?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would that be enough?

See Also

markov

Notes

The :func:~control.markov command estimates the Markov parameters directly, which can be hard for slightly damped systems.
The :func:~control.observer_kalman_identification command uses a Kalman filter, which is better suited for slightly damped systems.

See Also
--------
markov

Notes
-----
The :func:`~control.markov` command estimates the Markov parameters directly, which can be hard for slightly damped systems.
The :func:`~control.observer_kalman_identification` command uses a Kalman filter, which is better suited for slightly damped systems.

References
----------
.. [1] J.-N. Juang, M. Phan, L. G. Horta, and R. W. Longman,
Identification of observer/Kalman filter Markov parameters - Theory
and experiments. Journal of Guidance Control and Dynamics, 16(2),
320-329, 2012. http://doi.org/10.2514/3.21006

.. [2] J.-N. Juang, Applied System Identification, 1994

Examples
--------
>>> T = np.linspace(0, 10, 100)
>>> U = np.ones((1, 100))
>>> T, Y = ct.forced_response(ct.tf([1], [1, 0.5], True), T, U)
>>> H = ct.okid(Y, U, 3, transpose=False)

"""
# Convert input parameters to 2D arrays (if they aren't already)

# Get the system description
if len(args) < 1:
raise ControlArgument("not enough input arguments")

if isinstance(args[0], TimeResponseData):
data = args[0]
Umat = np.array(data.inputs, ndmin=2)
Ymat = np.array(data.outputs, ndmin=2)
if dt is None:
dt = data.time[1] - data.time[0]
if not np.allclose(np.diff(data.time), dt):
raise ValueError("response time values must be equally "
"spaced.")
transpose = data.transpose
if data.transpose and not data.issiso:
Umat, Ymat = np.transpose(Umat), np.transpose(Ymat)
if len(args) == 2:
m = args[1]
elif len(args) > 2:
raise ControlArgument("too many positional arguments")
else:
if len(args) < 2:
raise ControlArgument("not enough input arguments")
Umat = np.array(args[1], ndmin=2)
Ymat = np.array(args[0], ndmin=2)
if dt is None:
dt = True
if transpose:
Umat, Ymat = np.transpose(Umat), np.transpose(Ymat)
if len(args) == 3:
m = args[2]
elif len(args) > 3:
raise ControlArgument("too many positional arguments")

# Make sure the number of time points match
if Umat.shape[1] != Ymat.shape[1]:
raise ControlDimension(
"Input and output data are of differnent lengths")
l = Umat.shape[1]

# If number of desired parameters was not given, set to size of input data
if m is None:
m = l

# Paper equation 8, Page 8
# There is a mistake in the paper, but it is right in the book
t = 0
if truncate:
t = m

# The okid in the paper estimates `m + 1` Markov parameters
# Change to `m` to match control.markov,
# TODO:What is the best way to match control.markov
# m = m - 1

q = Ymat.shape[0] # number of outputs
p = Umat.shape[0] # number of inputs

# Make sure there is enough data to compute parameters
if m*p > (l-t):
warnings.warn("Not enough data for requested number of parameters")

# the algorithm - Construct a matrix of control virtual inputs to invert
#
# (q,l) = (q,(p+q)*m+p) @ ((p+q)*m+p,l)
# YY.T = Ybar @ VV.T
#
# This algorithm sets up the following problem and solves it for
# the observer Markov parameters
#
# (l,q) = (l,(p+q)*m+p) @ ((p+q)*m+p,q)
# YY = VV @ Ybar.T
#
# truncated version t=m, do not use first m equation
#
# Note: This algorithm assumes C A^{j} B = 0
# for j > m-2. See equation (3) in
#
# J.-N. Juang, M. Phan, L. G. Horta, and R. W. Longman, Identification
# of observer/Kalman filter Markov parameters - Theory and
# experiments. Journal of Guidance Control and Dynamics, 16(2),
# 320-329, 2012. http://doi.org/10.2514/3.21006
#

Vmat = np.concatenate((Umat, Ymat),axis=0)

VVT = np.zeros(((p + q)*m + p, l))

VVT[:p, :] = Umat
for i in range(m):
# Shift previous column down and keep zeros at the top
VVT[(p+q)*i+p:(p+q)*(i+1)+p, i+1:] = Vmat[:, :l-i-1]

YY = Ymat[:, t:].T
VV = VVT[:, t:].T

# Solve for the observer Markov parameters from YY = VV @ Ybar.T
YbarT, _, _, _ = np.linalg.lstsq(VV, YY, rcond=None)
Ybar = YbarT.T

# Paper equation 11, Page 9
D = Ybar[:, :p]

Ybar_r = Ybar[:, p:].reshape(q, m, p+q) # output, time*v_input -> output, time, v_input
Ybar_r = Ybar_r.transpose(0, 2, 1) # output, v_input, time

Ybar1 = Ybar_r[:, :p, :] # select observer Markov parameters generated by input
Ybar2 = Ybar_r[:, p:, :] # select observer Markov parameters generated by output

# Using recursive substitution to solve for Markov parameters
Y = np.zeros((q, p, m))
# Paper equation 12, Page 9
Y[:, :, 0] = Ybar1[:, :, 0] + Ybar2[:, :, 0] @ D

# Paper equation 15, Page 10
for k in range(1, m):
Y[:, :, k] = Ybar1[:, :, k] + Ybar2[:, :, k] @ D
for i in range(k-1):
Y[:, :, k] += Ybar2[:, :, i] @ Y[:, :, k-i-1]

# Paper equation 11, Page 9
H = np.zeros((q, p, m+1))
H[:, :, 0] = D
H[:, :, 1:] = Y[:, :, :]
H = H/dt # scaling

# for siso return a 1D array instead of a 3D array
if q == 1 and p == 1:
H = np.squeeze(H)

# Return the first m Markov parameters
return H if not transpose else np.transpose(H)

# Function aliases
hsvd = hankel_singular_values
balred = balanced_reduction
modred = model_reduction
minreal = minimal_realization
era = eigensys_realization
okid = observer_kalman_identification
2 changes: 1 addition & 1 deletion doc/functions.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -211,7 +211,7 @@ System ID and Model Reduction
model_reduction
eigensys_realization
markov

observer_kalman_identification

Nonlinear System Support
========================
Expand Down
110 changes: 110 additions & 0 deletions examples/okid_msd.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,110 @@
# okid_msd.py
# Johannes Kaisinger, 13 July 2024
#
# Demonstrate estimation of Markov parameters via okid.
# SISO, SIMO, MISO, MIMO case

import numpy as np
import matplotlib.pyplot as plt
import os

import control as ct

def create_impulse_response(H, time, transpose, dt):
"""Helper function to use TimeResponseData type for plotting"""

H = np.array(H, ndmin=3)

if transpose:
H = np.transpose(H)

q, p, m = H.shape
inputs = np.zeros((p,p,m))

issiso = True if (q == 1 and p == 1) else False

input_labels = []
trace_labels, trace_types = [], []
for i in range(p):
inputs[i,i,0] = 1/dt # unit area impulse
input_labels.append(f"u{[i]}")
trace_labels.append(f"From u{[i]}")
trace_types.append('impulse')

output_labels = []
for i in range(q):
output_labels.append(f"y{[i]}")

return ct.TimeResponseData(time=time[:m],
outputs=H,
output_labels=output_labels,
inputs=inputs,
input_labels=input_labels,
trace_labels=trace_labels,
trace_types=trace_types,
sysname="H_est",
transpose=transpose,
plot_inputs=False,
issiso=issiso)

# set up a mass spring damper system (2dof, MIMO case)
# Mechanical Vibrations: Theory and Application, SI Edition, 1st ed.
# Figure 6.5 / Example 6.7
# m q_dd + c q_d + k q = f
m1, k1, c1 = 1., 4., 1.
m2, k2, c2 = 2., 2., 1.
k3, c3 = 6., 1.

A = np.array([
[0., 0., 1., 0.],
[0., 0., 0., 1.],
[-(k1+k2)/m1, (k2)/m1, -(c1+c2)/m1, c2/m1],
[(k2)/m2, -(k2+k3)/m2, c2/m2, -(c2+c3)/m2]
])
B = np.array([[0.,0.],[0.,0.],[1/m1,0.],[0.,1/m2]])
C = np.array([[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0]])
D = np.zeros((2,2))


xixo_list = ["SISO","SIMO","MISO","MIMO"]
xixo = xixo_list[3] # choose a system for estimation
match xixo:
case "SISO":
sys = ct.StateSpace(A, B[:,0], C[0,:], D[0,0])
case "SIMO":
sys = ct.StateSpace(A, B[:,:1], C, D[:,:1])
case "MISO":
sys = ct.StateSpace(A, B, C[:1,:], D[:1,:])
case "MIMO":
sys = ct.StateSpace(A, B, C, D)

dt = 0.1
sysd = sys.sample(dt, method='zoh')
sysd.name = "H_true"

# random forcing input
t = np.arange(0,100,dt)
u = np.random.randn(sysd.B.shape[-1], len(t))

response = ct.forced_response(sysd, U=u)
response.plot()
plt.show()

m = 100
ir_true = ct.impulse_response(sysd, T=t[:m+1])
H_true = ir_true.outputs


H_est = ct.okid(response, m, dt=dt)
# helper function for plotting only
ir_est = create_impulse_response(H_est,
ir_true.time,
ir_true.transpose,
dt)

ir_true.plot(title=xixo)
ir_est.plot(color='orange',linestyle='dashed')
plt.show()

if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
plt.show()
Loading