Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
58 changes: 47 additions & 11 deletions control/statefbk.py
Original file line number Diff line number Diff line change
Expand Up @@ -113,18 +113,32 @@ def place(A, B, p):
return K


def place_varga(A, B, p):
def place_varga(A, B, p, dtime=False, alpha=None):
"""Place closed loop eigenvalues
K = place_varga(A, B, p)
K = place_varga(A, B, p, dtime=False, alpha=None)

Parameters
Required Parameters
----------
A : 2-d array
Dynamics matrix
B : 2-d array
Input matrix
p : 1-d list
Desired eigenvalue locations

Optional Parameters
---------------
dtime: False for continuous time pole placement or True for discrete time.
The default is dtime=False.
alpha: double scalar
If DICO='C', then place_varga will leave the eigenvalues with real
real part less than alpha untouched.
If DICO='D', the place_varga will leave eigenvalues with modulus
less than alpha untouched.

By default (alpha=None), place_varga computes alpha such that all
poles will be placed.

Returns
-------
K : 2-d array
Expand All @@ -146,7 +160,7 @@ def place_varga(A, B, p):
--------
>>> A = [[-1, -1], [0, 1]]
>>> B = [[0], [1]]
>>> K = place(A, B, [-2, -5])
>>> K = place_varga(A, B, [-2, -5])

See Also:
--------
Expand All @@ -160,24 +174,46 @@ def place_varga(A, B, p):
raise ControlSlycot("can't find slycot module 'sb01bd'")

# Convert the system inputs to NumPy arrays
A_mat = np.array(A);
B_mat = np.array(B);
A_mat = np.array(A)
B_mat = np.array(B)
if (A_mat.shape[0] != A_mat.shape[1] or
A_mat.shape[0] != B_mat.shape[0]):
raise ControlDimension("matrix dimensions are incorrect")

# Compute the system eigenvalues and convert poles to numpy array
system_eigs = np.linalg.eig(A_mat)[0]
placed_eigs = np.array(p);
placed_eigs = np.array(p)

# Need a character parameter for SB01BD
if dtime:
DICO = 'D'
else:
DICO = 'C'

if alpha is None:
# SB01BD ignores eigenvalues with real part less than alpha
# (if DICO='C') or with modulus less than alpha
# (if DICO = 'D').
if dtime:
# For discrete time, slycot only cares about modulus, so just make
# alpha the smallest it can be.
alpha = 0.0
else:
# Choosing alpha=min_eig is insufficient and can lead to an
# error or not having all the eigenvalues placed that we wanted.
# Evidently, what python thinks are the eigs is not precisely
# the same as what slicot thinks are the eigs. So we need some
# numerical breathing room. The following is pretty heuristic,
# but does the trick
alpha = -2*abs(min(system_eigs.real))
elif dtime and alpha < 0.0:
raise ValueError("Need alpha > 0 when DICO='D'")

# SB01BD sets eigenvalues with real part less than alpha
# We want to place all poles of the system => set alpha to minimum
alpha = min(system_eigs.real);

# Call SLICOT routine to place the eigenvalues
A_z,w,nfp,nap,nup,F,Z = \
sb01bd(B_mat.shape[0], B_mat.shape[1], len(placed_eigs), alpha,
A_mat, B_mat, placed_eigs, 'C');
A_mat, B_mat, placed_eigs, DICO)

# Return the gain matrix, with MATLAB gain convention
return -F
Expand Down
78 changes: 76 additions & 2 deletions control/tests/statefbk_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
from __future__ import print_function
import unittest
import numpy as np
from control.statefbk import ctrb, obsv, place, lqr, gram, acker
from control.statefbk import ctrb, obsv, place, place_varga, lqr, gram, acker
from control.matlab import *
from control.exception import slycot_check, ControlDimension

Expand Down Expand Up @@ -186,7 +186,10 @@ def testPlace(self):
np.testing.assert_raises(ValueError, place, A, B, P_repeated)

@unittest.skipIf(not slycot_check(), "slycot not installed")
def testPlace_varga(self):
def testPlace_varga_continuous(self):
"""
Check that we can place eigenvalues for dtime=False
"""
A = np.array([[1., -2.], [3., -4.]])
B = np.array([[5.], [7.]])

Expand All @@ -202,6 +205,77 @@ def testPlace_varga(self):
np.testing.assert_raises(ControlDimension, place, A[1:, :], B, P)
np.testing.assert_raises(ControlDimension, place, A, B[1:, :], P)

# Regression test against bug #177
# https://github.com/python-control/python-control/issues/177
A = np.array([[0, 1], [100, 0]])
B = np.array([[0], [1]])
P = np.array([-20 + 10*1j, -20 - 10*1j])
K = place_varga(A, B, P)
P_placed = np.linalg.eigvals(A - B.dot(K))

# No guarantee of the ordering, so sort them
P.sort()
P_placed.sort()
np.testing.assert_array_almost_equal(P, P_placed)

@unittest.skipIf(not slycot_check(), "slycot not installed")
def testPlace_varga_continuous_partial_eigs(self):
"""
Check that we are able to use the alpha parameter to only place
a subset of the eigenvalues, for the continous time case.
"""
# A matrix has eigenvalues at s=-1, and s=-2. Choose alpha = -1.5
# and check that eigenvalue at s=-2 stays put.
A = np.array([[1., -2.], [3., -4.]])
B = np.array([[5.], [7.]])

P = np.array([-3.])
P_expected = np.array([-2.0, -3.0])
alpha = -1.5
K = place_varga(A, B, P, alpha=alpha)

P_placed = np.linalg.eigvals(A - B.dot(K))
# No guarantee of the ordering, so sort them
P_expected.sort()
P_placed.sort()
np.testing.assert_array_almost_equal(P_expected, P_placed)

@unittest.skipIf(not slycot_check(), "slycot not installed")
def testPlace_varga_discrete(self):
"""
Check that we can place poles using dtime=True (discrete time)
"""
A = np.array([[1., 0], [0, 0.5]])
B = np.array([[5.], [7.]])

P = np.array([0.5, 0.5])
K = place_varga(A, B, P, dtime=True)
P_placed = np.linalg.eigvals(A - B.dot(K))
# No guarantee of the ordering, so sort them
P.sort()
P_placed.sort()
np.testing.assert_array_almost_equal(P, P_placed)

@unittest.skipIf(not slycot_check(), "slycot not installed")
def testPlace_varga_discrete_partial_eigs(self):
""""
Check that we can only assign a single eigenvalue in the discrete
time case.
"""
# A matrix has eigenvalues at 1.0 and 0.5. Set alpha = 0.51, and
# check that the eigenvalue at 0.5 is not moved.
A = np.array([[1., 0], [0, 0.5]])
B = np.array([[5.], [7.]])
P = np.array([0.2, 0.6])
P_expected = np.array([0.5, 0.6])
alpha = 0.51
K = place_varga(A, B, P, dtime=True, alpha=alpha)
P_placed = np.linalg.eigvals(A - B.dot(K))
P_expected.sort()
P_placed.sort()
np.testing.assert_array_almost_equal(P_expected, P_placed)


def check_LQR(self, K, S, poles, Q, R):
S_expected = np.array(np.sqrt(Q * R))
K_expected = S_expected / R
Expand Down