Skip to content

Commit ae2aea7

Browse files
KybernetikJoKybernetikJo
committed
Add okid, add okid example
1 parent 6406868 commit ae2aea7

2 files changed

Lines changed: 304 additions & 2 deletions

File tree

‎control/modelsimp.py‎

Lines changed: 193 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -44,12 +44,13 @@
4444
import numpy as np
4545
import warnings
4646
from .exception import ControlSlycot, ControlMIMONotImplemented, \
47-
ControlDimension
47+
ControlArgument, ControlDimension
4848
from .iosys import isdtime, isctime
4949
from .statesp import StateSpace
5050
from .statefbk import gram
51+
from .timeresp import TimeResponseData
5152

52-
__all__ = ['hsvd', 'balred', 'modred', 'era', 'markov', 'minreal']
53+
__all__ = ['hsvd', 'balred', 'modred', 'era', 'markov', 'minreal', 'okid']
5354

5455

5556
# Hankel Singular Value Decomposition
@@ -556,3 +557,193 @@ def markov(Y, U, m=None, transpose=False):
556557

557558
# Return the first m Markov parameters
558559
return H if transpose else np.transpose(H)
560+
561+
def okid(*args, m=None, transpose=False, dt=True, truncate=False):
562+
"""okid(Y, U, [, m])
563+
564+
Calculate the first `m+1` Markov parameters [D CB CAB ...]
565+
from data.
566+
567+
This function computes the Markov parameters for a discrete time system
568+
569+
.. math::
570+
571+
x[k+1] &= A x[k] + B u[k] \\\\
572+
y[k] &= C x[k] + D u[k]
573+
574+
given data for u and y. The algorithm assumes that that C A^k B = 0 for
575+
k > m-2 (see [1]_). Note that the problem is ill-posed if the length of
576+
the input data is less than the desired number of Markov parameters (a
577+
warning message is generated in this case).
578+
579+
The function can be called with either 1, 2 or 3 arguments:
580+
581+
* ``H = okid(data)``
582+
* ``H = okid(data, m)``
583+
* ``H = okid(Y, U)``
584+
* ``H = okid(Y, U, m)``
585+
586+
where `data` is an `TimeResponseData` object, and `Y`, `U`, are 1D or 2D
587+
array and m is an integer.
588+
589+
Parameters
590+
----------
591+
Y : array_like
592+
Output data. If the array is 1D, the system is assumed to be single
593+
input. If the array is 2D and transpose=False, the columns of `Y`
594+
are taken as time points, otherwise the rows of `Y` are taken as
595+
time points.
596+
U : array_like
597+
Input data, arranged in the same way as `Y`.
598+
data : TimeResponseData
599+
Response data from which the Markov parameters where estimated.
600+
Input and output data must be 1D or 2D array.
601+
m : int, optional
602+
Number of Markov parameters to output. Defaults to len(U).
603+
dt : True of float, optional
604+
True indicates discrete time with unspecified sampling time and a
605+
positive float is discrete time with the specified sampling time.
606+
It can be used to scale the Markov parameters in order to match
607+
the unit-area impulse response of python-control. Default is True
608+
truncate : bool, optional
609+
Do not use first m equation for least least squares. Default is False.
610+
transpose : bool, optional
611+
Assume that input data is transposed relative to the standard
612+
:ref:`time-series-convention`. For TimeResponseData this parameter
613+
is ignored. Default is False.
614+
615+
Returns
616+
-------
617+
H : ndarray
618+
First m Markov parameters, [D CB CAB ...].
619+
620+
References
621+
----------
622+
.. [1] J.-N. Juang, M. Phan, L. G. Horta, and R. W. Longman,
623+
Identification of observer/Kalman filter Markov parameters - Theory
624+
and experiments. Journal of Guidance Control and Dynamics, 16(2),
625+
320-329, 2012. http://doi.org/10.2514/3.21006
626+
627+
Examples
628+
--------
629+
>>> T = np.linspace(0, 10, 100)
630+
>>> U = np.ones((1, 100))
631+
>>> T, Y = ct.forced_response(ct.tf([1], [1, 0.5], True), T, U)
632+
>>> H = ct.okid(Y, U, 3, transpose=False)
633+
634+
"""
635+
# Convert input parameters to 2D arrays (if they aren't already)
636+
637+
# Get the system description
638+
if (len(args) < 1):
639+
raise ControlArgument("not enough input arguments")
640+
641+
if isinstance(args[0], TimeResponseData):
642+
Umat = np.array(args[0].inputs, ndmin=2)
643+
Ymat = np.array(args[0].outputs, ndmin=2)
644+
transpose = args[0].transpose
645+
if args[0].transpose and not args[0].issiso:
646+
Umat, Ymat = np.transpose(Umat), np.transpose(Ymat)
647+
if (len(args) == 2):
648+
m = args[1]
649+
elif (len(args) > 2):
650+
raise ControlArgument("too many positional arguments")
651+
else:
652+
if (len(args) < 2):
653+
raise ControlArgument("not enough input arguments")
654+
Umat = np.array(args[1], ndmin=2)
655+
Ymat = np.array(args[0], ndmin=2)
656+
if transpose:
657+
Umat, Ymat = np.transpose(Umat), np.transpose(Ymat)
658+
if (len(args) == 3):
659+
m = args[2]
660+
elif (len(args) > 3):
661+
raise ControlArgument("too many positional arguments")
662+
663+
# Make sure the number of time points match
664+
if Umat.shape[1] != Ymat.shape[1]:
665+
raise ControlDimension(
666+
"Input and output data are of differnent lengths")
667+
l = Umat.shape[1]
668+
669+
# If number of desired parameters was not given, set to size of input data
670+
if m is None:
671+
m = l
672+
673+
t = 0
674+
if truncate:
675+
t = m
676+
677+
q = Ymat.shape[0] # number of outputs
678+
p = Umat.shape[0] # number of inputs
679+
680+
# Make sure there is enough data to compute parameters
681+
if m*p > (l-t):
682+
warnings.warn("Not enough data for requested number of parameters")
683+
684+
# the algorithm - Construct a matrix of control inputs to invert
685+
#
686+
# (q,l) = (q,(p+q)*m+p) @ ((p+q)*m+p,l)
687+
# YY.T = Ybar @ VV.T
688+
#
689+
# This algorithm sets up the following problem and solves it for
690+
# the Markov parameters
691+
#
692+
# (l,q) = (l,(p+q)*m+p) @ ((p+q)*m+p,q)
693+
# YY = VV @ Ybar.T
694+
#
695+
# truncated version t=m, do not use first m equation
696+
#
697+
# Note: This algorithm assumes C A^{j} B = 0
698+
# for j > m-2. See equation (3) in
699+
#
700+
# J.-N. Juang, M. Phan, L. G. Horta, and R. W. Longman, Identification
701+
# of observer/Kalman filter Markov parameters - Theory and
702+
# experiments. Journal of Guidance Control and Dynamics, 16(2),
703+
# 320-329, 2012. http://doi.org/10.2514/3.21006
704+
#
705+
706+
Vmat = np.concatenate((Umat, Ymat),axis=0)
707+
708+
VVT = np.zeros(((p + q)*m + p, l))
709+
710+
VVT[:p,:] = Umat
711+
for i in range(m):
712+
# Shift previous column down and keep zeros at the top
713+
VVT[(p+q)*i+p:(p+q)*(i+1)+p, i+1:] = Vmat[:, :l-i-1]
714+
715+
YY = Ymat[:,t:].T
716+
VV = VVT[:,t:].T
717+
718+
# Solve for the observer Markov parameters from YY = VV @ Ybar.T
719+
YbarT, _, _, _ = np.linalg.lstsq(VV, YY, rcond=None)
720+
Ybar = YbarT.T
721+
722+
# Equation 11, Page 9
723+
D = Ybar[:,:p]
724+
725+
Ybar_r = Ybar[:,p:].reshape(q,m,p+q) # output, time*v_input -> output, time, v_input
726+
Ybar_r = Ybar_r.transpose(0,2,1) # output, v_input, time
727+
728+
Ybar1 = Ybar_r[:,:p,:] # select observer Markov parameters generated by input
729+
Ybar2 = Ybar_r[:,p:,:] # select observer Markov parameters generated by output
730+
731+
# Using recursive substitution to solve for Markov parameters
732+
Y = np.zeros((q,p,m))
733+
# Equation 12, Page 9
734+
Y[:,:,0] = Ybar1[:,:,0] + Ybar2[:,:,0]@D
735+
736+
# Equation 15, Page 10
737+
for k in range(1,m):
738+
Y[:,:,k] = Ybar1[:,:,k] + Ybar2[:,:,k]@D
739+
for i in range(k-1):
740+
Y[:,:,k] += Ybar2[:,:,i]@Y[:,:,k-i-1]
741+
742+
# Equation 11, Page 9
743+
H = np.zeros((q,p,m+1))
744+
H[:,:,0] = D
745+
H[:,:,1:] = Y[:,:,:]
746+
H = H/dt # scaling
747+
748+
# Return the first m Markov parameters
749+
return H if not transpose else np.transpose(H)

‎examples/okid_msd.py‎

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
# okid_msd.py
2+
# Johannes Kaisinger, 13 July 2024
3+
#
4+
# Demonstrate estimation of markov parameters via okid.
5+
# SISO, SIMO, MISO, MIMO case
6+
7+
import numpy as np
8+
import matplotlib.pyplot as plt
9+
import os
10+
11+
import control as ct
12+
13+
def create_impulse_response(H, time, transpose, dt):
14+
"""Helper function to use TimeResponseData type for plotting"""
15+
16+
H = np.array(H, ndmin=3)
17+
18+
if transpose:
19+
H = np.transpose(H)
20+
21+
q, p, m = H.shape
22+
inputs = np.zeros((p,p,m))
23+
24+
issiso = True if (q == 1 and p == 1) else False
25+
26+
input_labels = []
27+
trace_labels, trace_types = [], []
28+
for i in range(p):
29+
inputs[i,i,0] = 1/dt # unit area impulse
30+
input_labels.append(f"u{[i]}")
31+
trace_labels.append(f"From u{[i]}")
32+
trace_types.append('impulse')
33+
34+
output_labels = []
35+
for i in range(q):
36+
output_labels.append(f"y{[i]}")
37+
38+
return ct.TimeResponseData(time=time[:m],
39+
outputs=H,
40+
output_labels=output_labels,
41+
inputs=inputs,
42+
input_labels=input_labels,
43+
trace_labels=trace_labels,
44+
trace_types=trace_types,
45+
sysname="H_est",
46+
transpose=transpose,
47+
plot_inputs=False,
48+
issiso=issiso)
49+
50+
# set up a mass spring damper system (2dof, MIMO case)
51+
# Mechanical Vibrations: Theory and Application, SI Edition, 1st ed.
52+
# Figure 6.5 / Example 6.7
53+
# m q_dd + c q_d + k q = f
54+
m1, k1, c1 = 1., 4., 1.
55+
m2, k2, c2 = 2., 2., 1.
56+
k3, c3 = 6., 1.
57+
58+
A = np.array([
59+
[0., 0., 1., 0.],
60+
[0., 0., 0., 1.],
61+
[-(k1+k2)/m1, (k2)/m1, -(c1+c2)/m1, c2/m1],
62+
[(k2)/m2, -(k2+k3)/m2, c2/m2, -(c2+c3)/m2]
63+
])
64+
B = np.array([[0.,0.],[0.,0.],[1/m1,0.],[0.,1/m2]])
65+
C = np.array([[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0]])
66+
D = np.zeros((2,2))
67+
68+
69+
xixo_list = ["SISO","SIMO","MISO","MIMO"]
70+
xixo = xixo_list[3] # choose a system for estimation
71+
match xixo:
72+
case "SISO":
73+
sys = ct.StateSpace(A, B[:,0], C[0,:], D[0,0])
74+
case "SIMO":
75+
sys = ct.StateSpace(A, B[:,:1], C, D[:,:1])
76+
case "MISO":
77+
sys = ct.StateSpace(A, B, C[:1,:], D[:1,:])
78+
case "MIMO":
79+
sys = ct.StateSpace(A, B, C, D)
80+
81+
dt = 0.1
82+
sysd = sys.sample(dt, method='zoh')
83+
sysd.name = "H_true"
84+
85+
# random forcing input
86+
t = np.arange(0,100,dt)
87+
u = np.random.randn(sysd.B.shape[-1], len(t))
88+
89+
response = ct.forced_response(sysd, U=u)
90+
response.plot()
91+
plt.show()
92+
93+
m = 100
94+
ir_true = ct.impulse_response(sysd, T=t[:m+1])
95+
H_true = ir_true.outputs
96+
print(H_true.shape)
97+
98+
H_est = ct.okid(response, m, dt=dt)
99+
print(H_est.shape)
100+
# Helper function for plotting only
101+
ir_est = create_impulse_response(H_est,
102+
ir_true.time,
103+
ir_true.transpose,
104+
dt)
105+
106+
ir_true.plot(title=xixo)
107+
ir_est.plot(color='orange',linestyle='dashed')
108+
plt.show()
109+
110+
if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
111+
plt.show()

0 commit comments

Comments
 (0)