Skip to content

Commit

Permalink
use external package for matrix equations
Browse files Browse the repository at this point in the history
  • Loading branch information
olof3 committed Feb 3, 2021
1 parent 4a6d813 commit 1b44cd2
Show file tree
Hide file tree
Showing 6 changed files with 99 additions and 91 deletions.
2 changes: 1 addition & 1 deletion README.md
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ ss, tf, zpk
##### Analysis
pole, tzero, norm, hinfnorm, linfnorm, ctrb, obsv, gangoffour, margin, markovparam, damp, dampreport, zpkdata, dcgain, covar, gram, sigma, sisomargin
##### Synthesis
care, dare, dlyap, lqr, dlqr, place, leadlink, laglink, leadlinkat, rstd, rstc, dab, balreal, baltrunc
arec, ared, lyapc, lyapd, lqr, dlqr, place, leadlink, laglink, leadlinkat, rstd, rstc, dab, balreal, baltrunc
###### PID design
pid, stabregionPID, loopshapingPI, pidplots
##### Time and Frequency response
Expand Down
10 changes: 9 additions & 1 deletion src/ControlSystems.jl
Original file line number Diff line number Diff line change
Expand Up @@ -103,11 +103,15 @@ import Base: +, -, *, /, (==), (!=), isapprox, convert, promote_op
import Base: getproperty, getindex
import Base: exp # for exp(-s)
import LinearAlgebra: BlasFloat
export lyap # Make sure LinearAlgebra.lyap is available
import ControlMatrixEquations: ared, arec, lyapc, lyapd
export lyapd, lyapc
export ared, arec
import Printf, Colors
import DSP: conv
using MacroTools



abstract type AbstractSystem end

include("types/TimeEvolution.jl")
Expand Down Expand Up @@ -169,6 +173,10 @@ include("plotting.jl")
@deprecate norminf hinfnorm
@deprecate diagonalize(s::AbstractStateSpace, digits) diagonalize(s::AbstractStateSpace)

@deprecate dlyap lyapd
@deprecate dare ared
@deprecate care arec

function covar(D::Union{AbstractMatrix,UniformScaling}, R)
@warn "This call is deprecated due to ambiguity, use covar(ss(D), R) or covar(ss(D, Ts), R) instead"
D*R*D'
Expand Down
132 changes: 61 additions & 71 deletions src/matrix_comps.jl
Original file line number Diff line number Diff line change
Expand Up @@ -7,75 +7,65 @@ Algorithm taken from:
Laub, "A Schur Method for Solving Algebraic Riccati Equations."
http://dspace.mit.edu/bitstream/handle/1721.1/1301/R-0859-05666488.pdf
"""
function care(A, B, Q, R)
G = try
B*inv(R)*B'
catch y
if y isa SingularException
error("R must be non-singular in care.")
else
throw(y)
end
end

Z = [A -G;
-Q -A']

S = schur(Z)
S = ordschur(S, real(S.values).<0)
U = S.Z

(m, n) = size(U)
U11 = U[1:div(m, 2), 1:div(n,2)]
U21 = U[div(m,2)+1:m, 1:div(n,2)]
return U21/U11
end

"""`dare(A, B, Q, R)`
Compute `X`, the solution to the discrete-time algebraic Riccati equation,
defined as A'XA - X - (A'XB)(B'XB + R)^-1(B'XA) + Q = 0, where Q>=0 and R>0
Algorithm taken from:
Laub, "A Schur Method for Solving Algebraic Riccati Equations."
http://dspace.mit.edu/bitstream/handle/1721.1/1301/R-0859-05666488.pdf
"""
function dare(A, B, Q, R)
if !issemiposdef(Q)
error("Q must be positive-semidefinite.");
end
if (!isposdef(R))
error("R must be positive definite.");
end

n = size(A, 1);

E = [
Matrix{Float64}(I, n, n) B*(R\B');
zeros(size(A)) A'
];
F = [
A zeros(size(A));
-Q Matrix{Float64}(I, n, n)
];

QZ = schur(F, E);
QZ = ordschur(QZ, abs.(QZ.alpha./QZ.beta) .< 1);
# function care(A, B, Q, R)
# G = try
# B*inv(R)*B'
# catch y
# if y isa SingularException
# error("R must be non-singular in care.")
# else
# throw(y)
# end
# end
#
# Z = [A -G;
# -Q -A']
#
# S = schur(Z)
# S = ordschur(S, real(S.values).<0)
# U = S.Z
#
# (m, n) = size(U)
# U11 = U[1:div(m, 2), 1:div(n,2)]
# U21 = U[div(m,2)+1:m, 1:div(n,2)]
# return U21/U11
# end
#
# """`dare(A, B, Q, R)`
#
# Compute `X`, the solution to the discrete-time algebraic Riccati equation,
# defined as A'XA - X - (A'XB)(B'XB + R)^-1(B'XA) + Q = 0, where Q>=0 and R>0
#
# Algorithm taken from:
# Laub, "A Schur Method for Solving Algebraic Riccati Equations."
# http://dspace.mit.edu/bitstream/handle/1721.1/1301/R-0859-05666488.pdf
# """
# function dare(A, B, Q, R)
# if !ishermitian(Q)# !issemiposdef(Q)
# error("Q must be Hermitian");
# end
# if (!isposdef(R))
# error("R must be positive definite");
# end
#
# n = size(A, 1);
#
# E = [
# Matrix{Float64}(I, n, n) B*(R\B');
# zeros(size(A)) A'
# ];
# F = [
# A zeros(size(A));
# -Q Matrix{Float64}(I, n, n)
# ];
#
# QZ = schur(F, E);
# QZ = ordschur(QZ, abs.(QZ.alpha./QZ.beta) .< 1);
#
# return QZ.Z[(n+1):end, 1:n]/QZ.Z[1:n, 1:n];
# end

return QZ.Z[(n+1):end, 1:n]/QZ.Z[1:n, 1:n];
end

"""`dlyap(A, Q)`

Compute the solution `X` to the discrete Lyapunov equation
`AXA' - X + Q = 0`.
"""
function dlyap(A::AbstractMatrix, Q)
lhs = kron(A, conj(A))
lhs = I - lhs
x = lhs\reshape(Q, prod(size(Q)), 1)
return reshape(x, size(Q))
end

"""`gram(sys, opt)`
Expand All @@ -86,7 +76,7 @@ function gram(sys::AbstractStateSpace, opt::Symbol)
if !isstable(sys)
error("gram only valid for stable A")
end
func = iscontinuous(sys) ? lyap : dlyap
func = iscontinuous(sys) ? lyapc : lyapd
if opt === :c
# TODO probably remove type check in julia 0.7.0
return func(sys.A, sys.B*sys.B')#::Array{numeric_type(sys),2} # lyap is type-unstable
Expand Down Expand Up @@ -162,14 +152,14 @@ function covar(sys::AbstractStateSpace, W)
if !isstable(sys)
return fill(Inf,(size(C,1),size(C,1)))
end
func = iscontinuous(sys) ? lyap : dlyap

Q = try
func(A, B*W*B')
iscontinuous(sys) ? lyapc(A, B*W*B') : lyapd(A, B*W*B')
catch
error("No solution to the Lyapunov equation was found in covar")
end
P = C*Q*C'
if !isdiscrete(sys)
if iscontinuous(sys)
#Variance and covariance infinite for direct terms
direct_noise = D*W*D'
for i in 1:size(C,1)
Expand Down
4 changes: 2 additions & 2 deletions src/synthesis.jl
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ plot(t,x, lab=["Position" "Velocity"], xlabel="Time [s]")
```
"""
function lqr(A, B, Q, R)
S = care(A, B, Q, R)
S = care(A, B, Q, R)[1]
K = R\B'*S
return K
end
Expand Down Expand Up @@ -99,7 +99,7 @@ plot(t,x, lab=["Position" "Velocity"], xlabel="Time [s]")
```
"""
function dlqr(A, B, Q, R)
S = dare(A, B, Q, R)
S = dare(A, B, Q, R)[1]
K = (B'*S*B + R)\(B'S*A)
return K
end
Expand Down
2 changes: 1 addition & 1 deletion test/runtests.jl
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ eye_(n) = Matrix{Int64}(I, n, n)
my_tests = [
"test_timeevol",
"test_statespace",
"test_transferfunction",
"test_transferfunction",
"test_zpk",
"test_promotion",
"test_connections",
Expand Down
40 changes: 25 additions & 15 deletions test/test_linalg.jl
Original file line number Diff line number Diff line change
Expand Up @@ -4,34 +4,34 @@ b = [0 1]'
c = [1 -1]
r = 3

@test care(a, b, c'*c, r) [0.5895174372762604 1.8215747248860816; 1.8215747248860823 8.818839806923107]
@test dare(a, b, c'*c, r) [240.73344504496302 -131.09928700089387; -131.0992870008943 75.93413176750603]
@test care(a, b, c'*c, r)[1] [0.5895174372762604 1.8215747248860816; 1.8215747248860823 8.818839806923107]
@test dare(a, b, c'*c, r)[1] [240.73344504496302 -131.09928700089387; -131.0992870008943 75.93413176750603]

## Test dare method for non invertible matrices
A = [0 1; 0 0];
B0 = [0; 1];
Q = Matrix{Float64}(I, 2, 2);
A = [0 1; 0 0]
B0 = [0; 1]
Q = Matrix{Float64}(I, 2, 2)
R0 = 1
X = dare(A, B0, Q, R0);
X = dare(A, B0, Q, R0)[1]
# Reshape for matrix expression
B = reshape(B0, 2, 1)
R = fill(R0, 1, 1)
@test norm(A'X*A - X - (A'X*B)*((B'X*B + R)\(B'X*A)) + Q) < 1e-14
## Test dare for scalars
A = 1.0;
B = 1.0;
Q = 1.0;
R = 1.0;
X0 = dare(A, B, Q, R);
A = 1.0
B = 1.0
Q = 1.0
R = 1.0
X0 = dare(A, B, Q, R)[1]
X = X0[1]
@test norm(A'X*A - X - (A'X*B)*((B'X*B + R)\(B'X*A)) + Q) < 1e-14
# Test for complex matrices
I2 = Matrix{Float64}(I, 2, 2)
@test dare([1.0 im; im 1.0], I2, I2, I2) [1 + sqrt(2) 0; 0 1 + sqrt(2)]
@test dare([1.0 im; im 1.0], I2, I2, I2)[1] [1 + sqrt(2) 0; 0 1 + sqrt(2)]
# And complex scalars
@test dare(1.0, 1, 1, 1) fill((1 + sqrt(5))/2, 1, 1)
@test dare(1.0im, 1, 1, 1) fill((1 + sqrt(5))/2, 1, 1)
@test dare(1.0, 1im, 1, 1) fill((1 + sqrt(5))/2, 1, 1)
@test dare(1.0, 1, 1, 1)[1] fill((1 + sqrt(5))/2, 1, 1)
@test dare(1.0im, 1, 1, 1)[1] fill((1 + sqrt(5))/2, 1, 1)
@test dare(1.0, 1im, 1, 1)[1] fill((1 + sqrt(5))/2, 1, 1)

## Test gram, ctrb, obsv
a_2 = [-5 -3; 2 -9]
Expand Down Expand Up @@ -109,6 +109,16 @@ D_static = ss([0.704473 1.56483; -1.6661 -2.16041], 0.07)
@test norm(D_221) 3.4490083195926426
@test norm(ss([1],[2],[3],[4])) == Inf


# Test discrete time 2 norms
c = [1,2,-1,3,5,-2]
@test norm(DemoSystems.ssfir(c)) == norm(c)
c = 1:100
@test norm(DemoSystems.ssfir(c)) == norm(c)
c = 1:300 # typically enough to break a "naive" Lyapunov solver
@test norm(DemoSystems.ssfir(c)) == norm(c)


#
## Test Hinfinity norm computations
#
Expand Down

0 comments on commit 1b44cd2

Please sign in to comment.