Skip to content

Commit 5dc8aa0

Browse files
KybernetikJoKybernetikJo
committed
Add okid, add okid example
1 parent 220ace3 commit 5dc8aa0

2 files changed

Lines changed: 329 additions & 0 deletions

File tree

‎control/modelsimp.py‎

Lines changed: 219 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,15 +43,24 @@
4343
# External packages and modules
4444
import numpy as np
4545
import warnings
46+
<<<<<<< HEAD
4647
from .exception import ControlSlycot, ControlArgument, ControlDimension
48+
=======
49+
from .exception import ControlSlycot, ControlMIMONotImplemented, \
50+
ControlArgument, ControlDimension
51+
>>>>>>> ab66e5d (Add okid, add okid example)
4752
from .iosys import isdtime, isctime
4853
from .statesp import StateSpace
4954
from .statefbk import gram
5055
from .timeresp import TimeResponseData
5156

57+
<<<<<<< HEAD
5258
__all__ = ['hankel_singular_values', 'balanced_reduction', 'model_reduction',
5359
'minimal_realization', 'eigensys_realization', 'markov', 'hsvd',
5460
'balred', 'modred', 'minreal', 'era']
61+
=======
62+
__all__ = ['hsvd', 'balred', 'modred', 'era', 'markov', 'minreal', 'okid']
63+
>>>>>>> ab66e5d (Add okid, add okid example)
5564

5665

5766
# Hankel Singular Value Decomposition
@@ -690,9 +699,219 @@ def markov(*args, m=None, transpose=False, dt=None, truncate=False):
690699
# Return the first m Markov parameters
691700
return H if not transpose else np.transpose(H)
692701

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