Skip to content

Commit 299dd67

Browse files
KybernetikJoKybernetikJo
committed
Add okid, add okid example
1 parent f73e893 commit 299dd67

2 files changed

Lines changed: 336 additions & 2 deletions

File tree

‎control/modelsimp.py‎

Lines changed: 226 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -22,8 +22,9 @@
2222
from .timeresp import TimeResponseData
2323

2424
__all__ = ['hankel_singular_values', 'balanced_reduction', 'model_reduction',
25-
'minimal_realization', 'eigensys_realization', 'markov', 'hsvd',
26-
'balred', 'modred', 'minreal', 'era']
25+
'minimal_realization', 'eigensys_realization',
26+
'observer_kalman_identification', 'markov', 'hsvd', 'balred',
27+
'modred', 'minreal', 'era', 'okid']
2728

2829

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

728+
def observer_kalman_identification(*args, m=None, transpose=False, dt=True, truncate=False):
729+
"""observer_kalman_identification(Y, U, [, m])
730+
731+
Calculate the first `m` Markov parameters [D CB CAB ...]
732+
from data.
733+
734+
This function computes the Markov parameters for a discrete time system
735+
736+
.. math::
737+
738+
x[k+1] &= A x[k] + B u[k] \\\\
739+
y[k] &= C x[k] + D u[k]
740+
741+
given data for u and y. The algorithm assumes that that C A^k B = 0 for
742+
k > m-2 (see [1]_, [2]_). Note that the problem is ill-posed if the length of
743+
the input data is less than the desired number of Markov parameters (a
744+
warning message is generated in this case).
745+
746+
The function can be called with either 1, 2 or 3 arguments:
747+
748+
* ``H = observer_kalman_identification(data)``
749+
* ``H = observer_kalman_identification(data, m)``
750+
* ``H = observer_kalman_identification(Y, U)``
751+
* ``H = observer_kalman_identification(Y, U, m)``
752+
753+
where `data` is an `TimeResponseData` object, and `Y`, `U`, are 1D or 2D
754+
array and m is an integer.
755+
756+
Parameters
757+
----------
758+
Y : array_like
759+
Output data. If the array is 1D, the system is assumed to be
760+
single input. If the array is 2D and transpose=False, the columns
761+
of `Y` are taken as time points, otherwise the rows of `Y` are
762+
taken as time points.
763+
U : array_like
764+
Input data, arranged in the same way as `Y`.
765+
data : TimeResponseData
766+
Response data from which the Markov parameters where estimated.
767+
Input and output data must be 1D or 2D array.
768+
m : int, optional
769+
Number of Markov parameters to output. Defaults to len(U).
770+
dt : True of float, optional
771+
True indicates discrete time with unspecified sampling time and a
772+
positive float is discrete time with the specified sampling time.
773+
It can be used to scale the Markov parameters in order to match
774+
the unit-area impulse response of python-control. Default is True
775+
for array_like and dt=data.time[1]-data.time[0] for
776+
TimeResponseData as input.
777+
truncate : bool, optional
778+
Do not use first m equation for least least squares. Default is False.
779+
transpose : bool, optional
780+
Assume that input data is transposed relative to the standard
781+
:ref:`time-series-convention`. For TimeResponseData this parameter
782+
is ignored. Default is False.
783+
784+
Returns
785+
-------
786+
H : ndarray
787+
First m Markov parameters, [D CB CAB ...].
788+
789+
See Also
790+
--------
791+
markov
792+
793+
Notes
794+
-----
795+
The :func:`~control.markov` command estimates the Markov parameters directly, which can be hard for slightly damped systems.
796+
The :func:`~control.observer_kalman_identification` command uses a Kalman filter, which is better suited for slightly damped systems.
797+
798+
References
799+
----------
800+
.. [1] J.-N. Juang, M. Phan, L. G. Horta, and R. W. Longman,
801+
Identification of observer/Kalman filter Markov parameters - Theory
802+
and experiments. Journal of Guidance Control and Dynamics, 16(2),
803+
320-329, 2012. http://doi.org/10.2514/3.21006
804+
805+
.. [2] J.-N. Juang, Applied System Identification, 1994
806+
807+
Examples
808+
--------
809+
>>> T = np.linspace(0, 10, 100)
810+
>>> U = np.ones((1, 100))
811+
>>> T, Y = ct.forced_response(ct.tf([1], [1, 0.5], True), T, U)
812+
>>> H = ct.okid(Y, U, 3, transpose=False)
813+
814+
"""
815+
# Convert input parameters to 2D arrays (if they aren't already)
816+
817+
# Get the system description
818+
if len(args) < 1:
819+
raise ControlArgument("not enough input arguments")
820+
821+
if isinstance(args[0], TimeResponseData):
822+
data = args[0]
823+
Umat = np.array(data.inputs, ndmin=2)
824+
Ymat = np.array(data.outputs, ndmin=2)
825+
if dt is None:
826+
dt = data.time[1] - data.time[0]
827+
if not np.allclose(np.diff(data.time), dt):
828+
raise ValueError("response time values must be equally "
829+
"spaced.")
830+
transpose = data.transpose
831+
if data.transpose and not data.issiso:
832+
Umat, Ymat = np.transpose(Umat), np.transpose(Ymat)
833+
if len(args) == 2:
834+
m = args[1]
835+
elif len(args) > 2:
836+
raise ControlArgument("too many positional arguments")
837+
else:
838+
if len(args) < 2:
839+
raise ControlArgument("not enough input arguments")
840+
Umat = np.array(args[1], ndmin=2)
841+
Ymat = np.array(args[0], ndmin=2)
842+
if dt is None:
843+
dt = True
844+
if transpose:
845+
Umat, Ymat = np.transpose(Umat), np.transpose(Ymat)
846+
if len(args) == 3:
847+
m = args[2]
848+
elif len(args) > 3:
849+
raise ControlArgument("too many positional arguments")
850+
851+
# Make sure the number of time points match
852+
if Umat.shape[1] != Ymat.shape[1]:
853+
raise ControlDimension(
854+
"Input and output data are of differnent lengths")
855+
l = Umat.shape[1]
856+
857+
# If number of desired parameters was not given, set to size of input data
858+
if m is None:
859+
m = l
860+
861+
# Paper equation 8, Page 8
862+
# There is a mistake in the paper, but it is right in the book
863+
t = 0
864+
if truncate:
865+
t = m
866+
867+
# The okid in the paper estimates `m + 1` Markov parameters
868+
# Change to `m` to match control.markov,
869+
# TODO:What is the best way to match control.markov
870+
# m = m - 1
871+
872+
q = Ymat.shape[0] # number of outputs
873+
p = Umat.shape[0] # number of inputs
874+
875+
# Make sure there is enough data to compute parameters
876+
if m*p > (l-t):
877+
warnings.warn("Not enough data for requested number of parameters")
878+
879+
# the algorithm - Construct a matrix of control virtual inputs to invert
880+
#
881+
# (q,l) = (q,(p+q)*m+p) @ ((p+q)*m+p,l)
882+
# YY.T = Ybar @ VV.T
883+
#
884+
# This algorithm sets up the following problem and solves it for
885+
# the observer Markov parameters
886+
#
887+
# (l,q) = (l,(p+q)*m+p) @ ((p+q)*m+p,q)
888+
# YY = VV @ Ybar.T
889+
#
890+
# truncated version t=m, do not use first m equation
891+
#
892+
# Note: This algorithm assumes C A^{j} B = 0
893+
# for j > m-2. See equation (3) in
894+
#
895+
# J.-N. Juang, M. Phan, L. G. Horta, and R. W. Longman, Identification
896+
# of observer/Kalman filter Markov parameters - Theory and
897+
# experiments. Journal of Guidance Control and Dynamics, 16(2),
898+
# 320-329, 2012. http://doi.org/10.2514/3.21006
899+
#
900+
901+
Vmat = np.concatenate((Umat, Ymat),axis=0)
902+
903+
VVT = np.zeros(((p + q)*m + p, l))
904+
905+
VVT[:p, :] = Umat
906+
for i in range(m):
907+
# Shift previous column down and keep zeros at the top
908+
VVT[(p+q)*i+p:(p+q)*(i+1)+p, i+1:] = Vmat[:, :l-i-1]
909+
910+
YY = Ymat[:, t:].T
911+
VV = VVT[:, t:].T
912+
913+
# Solve for the observer Markov parameters from YY = VV @ Ybar.T
914+
YbarT, _, _, _ = np.linalg.lstsq(VV, YY, rcond=None)
915+
Ybar = YbarT.T
916+
917+
# Paper equation 11, Page 9
918+
D = Ybar[:, :p]
919+
920+
Ybar_r = Ybar[:, p:].reshape(q, m, p+q) # output, time*v_input -> output, time, v_input
921+
Ybar_r = Ybar_r.transpose(0, 2, 1) # output, v_input, time
922+
923+
Ybar1 = Ybar_r[:, :p, :] # select observer Markov parameters generated by input
924+
Ybar2 = Ybar_r[:, p:, :] # select observer Markov parameters generated by output
925+
926+
# Using recursive substitution to solve for Markov parameters
927+
Y = np.zeros((q, p, m))
928+
# Paper equation 12, Page 9
929+
Y[:, :, 0] = Ybar1[:, :, 0] + Ybar2[:, :, 0] @ D
930+
931+
# Paper equation 15, Page 10
932+
for k in range(1, m):
933+
Y[:, :, k] = Ybar1[:, :, k] + Ybar2[:, :, k] @ D
934+
for i in range(k-1):
935+
Y[:, :, k] += Ybar2[:, :, i] @ Y[:, :, k-i-1]
936+
937+
# Paper equation 11, Page 9
938+
H = np.zeros((q, p, m+1))
939+
H[:, :, 0] = D
940+
H[:, :, 1:] = Y[:, :, :]
941+
H = H/dt # scaling
942+
943+
# for siso return a 1D array instead of a 3D array
944+
if q == 1 and p == 1:
945+
H = np.squeeze(H)
946+
947+
# Return the first m Markov parameters
948+
return H if not transpose else np.transpose(H)
949+
727950
# Function aliases
728951
hsvd = hankel_singular_values
729952
balred = balanced_reduction
730953
modred = model_reduction
731954
minreal = minimal_realization
732955
era = eigensys_realization
956+
okid = observer_kalman_identification

‎examples/okid_msd.py‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
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+
97+
98+
H_est = ct.okid(response, m, dt=dt)
99+
# helper function for plotting only
100+
ir_est = create_impulse_response(H_est,
101+
ir_true.time,
102+
ir_true.transpose,
103+
dt)
104+
105+
ir_true.plot(title=xixo)
106+
ir_est.plot(color='orange',linestyle='dashed')
107+
plt.show()
108+
109+
if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
110+
plt.show()

0 commit comments

Comments
 (0)