Skip to content

Commit 0422c82

Browse files
murrayrmmurrayrm
authored
Merge pull request #877 from murrayrm/oep_mhe-26Feb2023
Optimization-based and moving horizon estimation
2 parents 2c5c2f0 + a329323 commit 0422c82

22 files changed

+2762
-209
lines changed

‎.gitignore‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
*.pyc
2+
__pycache__/
23
build/
34
dist/
45
htmlcov/

‎benchmarks/optestim_bench.py‎

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
# optestim_bench.py - benchmarks for optimal/moving horizon estimation
2+
# RMM, 14 Mar 2023
3+
#
4+
# This benchmark tests the timing for the optimal estimation routines and
5+
# is intended to be used for helping tune the performance of the functions
6+
# used for optimization-based estimation.
7+
8+
import numpy as np
9+
import math
10+
import control as ct
11+
import control.optimal as opt
12+
13+
minimizer_table = {
14+
'default': (None, {}),
15+
'trust': ('trust-constr', {}),
16+
'trust_bigstep': ('trust-constr', {'finite_diff_rel_step': 0.01}),
17+
'SLSQP': ('SLSQP', {}),
18+
'SLSQP_bigstep': ('SLSQP', {'eps': 0.01}),
19+
'COBYLA': ('COBYLA', {}),
20+
}
21+
22+
# Table to turn on and off process disturbances and measurement noise
23+
noise_table = {
24+
'noisy': (1e-1, 1e-3),
25+
'nodist': (0, 1e-3),
26+
'nomeas': (1e-1, 0),
27+
'clean': (0, 0)
28+
}
29+
30+
31+
# Assess performance as a function of optimization and integration methods
32+
def time_oep_minimizer_methods(minimizer_name, noise_name, initial_guess):
33+
# Use fixed system to avoid randome errors (was csys = ct.rss(4, 2, 5))
34+
csys = ct.ss(
35+
[[-0.5, 1, 0, 0], [0, -1, 1, 0], [0, 0, -2, 1], [0, 0, 0, -3]], # A
36+
[[0, 0.1], [0, 0.1], [0, 0.1], [1, 0.1]], # B
37+
[[1, 0, 0, 0], [0, 0, 1, 0]], # C
38+
0, dt=0)
39+
# dsys = ct.c2d(csys, dt)
40+
# sys = csys if dt == 0 else dsys
41+
sys = csys
42+
43+
# Decide on process disturbances and measurement noise
44+
dist_mag, meas_mag = noise_table[noise_name]
45+
46+
# Create disturbances and noise (fixed, to avoid random errors)
47+
Rv = 0.1 * np.eye(1) # scalar disturbance
48+
Rw = 0.01 * np.eye(sys.noutputs)
49+
timepts = np.arange(0, 10.1, 1)
50+
V = np.array(
51+
[0 if t % 2 == 1 else 1 if t % 4 == 0 else -1 for t in timepts]
52+
).reshape(1, -1) * dist_mag
53+
W = np.vstack([np.sin(2*timepts), np.cos(3*timepts)]) * meas_mag
54+
55+
# Generate system data
56+
U = np.sin(timepts).reshape(1, -1)
57+
res = ct.input_output_response(sys, timepts, [U, V])
58+
Y = res.outputs + W
59+
60+
# Decide on the initial guess to use
61+
if initial_guess == 'xhat':
62+
initial_guess = (res.states, V*0)
63+
elif initial_guess == 'both':
64+
initial_guess = (res.states, V)
65+
else:
66+
initial_guess = None
67+
68+
69+
# Set up optimal estimation function using Gaussian likelihoods for cost
70+
traj_cost = opt.gaussian_likelihood_cost(sys, Rv, Rw)
71+
init_cost = lambda xhat, x: (xhat - x) @ (xhat - x)
72+
oep = opt.OptimalEstimationProblem(
73+
sys, timepts, traj_cost, terminal_cost=init_cost)
74+
75+
# Noise and disturbances (the standard case)
76+
est = oep.compute_estimate(Y, U, initial_guess=initial_guess)
77+
assert est.success
78+
np.testing.assert_allclose(
79+
est.states[:, -1], res.states[:, -1], atol=1e-1, rtol=1e-2)
80+
81+
82+
# Parameterize the test against different choices of integrator and minimizer
83+
time_oep_minimizer_methods.param_names = ['minimizer', 'noise', 'initial']
84+
time_oep_minimizer_methods.params = (
85+
['default', 'trust', 'SLSQP', 'COBYLA'],
86+
['noisy', 'nodist', 'nomeas', 'clean'],
87+
['none', 'xhat', 'both'])

‎control/config.py‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010

1111
import collections
1212
import warnings
13+
from .exception import ControlArgument
1314

1415
__all__ = ['defaults', 'set_defaults', 'reset_defaults',
1516
'use_matlab_defaults', 'use_fbs_defaults',
@@ -360,3 +361,25 @@ def use_legacy_defaults(version):
360361
set_defaults('nyquist', mirror_style='-')
361362

362363
return (major, minor, patch)
364+
365+
366+
#
367+
# Utility function for processing legacy keywords
368+
#
369+
# Use this function to handle a legacy keyword that has been renamed. This
370+
# function pops the old keyword off of the kwargs dictionary and issues a
371+
# warning. if both the old and new keyword are present, a ControlArgument
372+
# exception is raised.
373+
#
374+
def _process_legacy_keyword(kwargs, oldkey, newkey, newval):
375+
if kwargs.get(oldkey) is not None:
376+
warnings.warn(
377+
f"keyworld '{oldkey}' is deprecated; use '{newkey}'",
378+
DeprecationWarning)
379+
if newval is not None:
380+
raise ControlArgument(
381+
f"duplicate keywords '{oldkey}' and '{newkey}'")
382+
else:
383+
return kwargs.pop(oldkey)
384+
else:
385+
return newval

‎control/iosys.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -82,7 +82,7 @@ class for a set of subclasses that are used to implement specific
8282
System name (used for specifying signals). If unspecified, a generic
8383
name <sys[id]> is generated with a unique integer id.
8484
params : dict, optional
85-
Parameter values for the systems. Passed to the evaluation functions
85+
Parameter values for the system. Passed to the evaluation functions
8686
for the system as default values, overriding internal defaults.
8787
8888
Attributes

‎control/namedio.py‎

Lines changed: 102 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -22,8 +22,8 @@
2222
'namedio.sampled_system_name_prefix': '',
2323
'namedio.sampled_system_name_suffix': '$sampled'
2424
}
25-
26-
25+
26+
2727
class NamedIOSystem(object):
2828
def __init__(
2929
self, name=None, inputs=None, outputs=None, states=None, **kwargs):
@@ -584,3 +584,103 @@ def _process_signal_list(signals, prefix='s'):
584584

585585
else:
586586
raise TypeError("Can't parse signal list %s" % str(signals))
587+
588+
589+
#
590+
# Utility functions to process signal indices
591+
#
592+
# Signal indices can be specified in one of four ways:
593+
#
594+
# 1. As a positive integer 'm', in which case we return a list
595+
# corresponding to the first 'm' elements of a range of a given length
596+
#
597+
# 2. As a negative integer '-m', in which case we return a list
598+
# corresponding to the last 'm' elements of a range of a given length
599+
#
600+
# 3. As a slice, in which case we return the a list corresponding to the
601+
# indices specified by the slice of a range of a given length
602+
#
603+
# 4. As a list of ints or strings specifying specific indices. Strings are
604+
# compared to a list of labels to determine the index.
605+
#
606+
def _process_indices(arg, name, labels, length):
607+
# Default is to return indices up to a certain length
608+
arg = length if arg is None else arg
609+
610+
if isinstance(arg, int):
611+
# Return the start or end of the list of possible indices
612+
return list(range(arg)) if arg > 0 else list(range(length))[arg:]
613+
614+
elif isinstance(arg, slice):
615+
# Return the indices referenced by the slice
616+
return list(range(length))[arg]
617+
618+
elif isinstance(arg, list):
619+
# Make sure the length is OK
620+
if len(arg) > length:
621+
raise ValueError(
622+
f"{name}_indices list is too long; max length = {length}")
623+
624+
# Return the list, replacing strings with corresponding indices
625+
arg=arg.copy()
626+
for i, idx in enumerate(arg):
627+
if isinstance(idx, str):
628+
arg[i] = labels.index(arg[i])
629+
return arg
630+
631+
raise ValueError(f"invalid argument for {name}_indices")
632+
633+
#
634+
# Process control and disturbance indices
635+
#
636+
# For systems with inputs and disturbances, the control_indices and
637+
# disturbance_indices keywords are used to specify which is which. If only
638+
# one is given, the other is assumed to be the remaining indices in the
639+
# system input. If neither is given, the disturbance inputs are assumed to
640+
# be the same as the control inputs.
641+
#
642+
def _process_control_disturbance_indices(
643+
sys, control_indices, disturbance_indices):
644+
645+
if control_indices is None and disturbance_indices is None:
646+
# Disturbances enter in the same place as the controls
647+
dist_idx = ctrl_idx = list(range(sys.ninputs))
648+
649+
elif control_indices is not None:
650+
# Process the control indices
651+
ctrl_idx = _process_indices(
652+
control_indices, 'control', sys.input_labels, sys.ninputs)
653+
654+
# Disturbance indices are the complement of control indices
655+
dist_idx = [i for i in range(sys.ninputs) if i not in ctrl_idx]
656+
657+
else: # disturbance_indices is not None
658+
# If passed an integer, count from the end of the input vector
659+
arg = -disturbance_indices if isinstance(disturbance_indices, int) \
660+
else disturbance_indices
661+
662+
dist_idx = _process_indices(
663+
arg, 'disturbance', sys.input_labels, sys.ninputs)
664+
665+
# Set control indices to complement disturbance indices
666+
ctrl_idx = [i for i in range(sys.ninputs) if i not in dist_idx]
667+
668+
return ctrl_idx, dist_idx
669+
670+
671+
# Process labels
672+
def _process_labels(labels, name, default):
673+
if isinstance(labels, str):
674+
labels = [labels.format(i=i) for i in range(len(default))]
675+
676+
if labels is None:
677+
labels = default
678+
elif isinstance(labels, list):
679+
if len(labels) != len(default):
680+
raise ValueError(
681+
f"incorrect length of {name}_labels: {len(labels)}"
682+
f" instead of {len(default)}")
683+
else:
684+
raise ValueError(f"{name}_labels should be a string or a list")
685+
686+
return labels

0 commit comments

Comments
 (0)