use MatrixEquations.jl (#494)

* use MatrixEquations.jl

* add ME to Project.toml

* care/dare for scalars

* args and kwargs forwarding to ME

* ME handles vector B

Author: baggepinnen

Project.toml

@@ -13,6 +13,7 @@ IterTools = "c8e1da08-722c-5040-9ed9-7db0dc04731e"
 LaTeXStrings = "b964fa9f-0449-5b57-a5c2-d3ea65f4040f"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 MacroTools = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09"
+MatrixEquations = "99c1a7ee-ab34-5fd5-8076-27c950a045f4"
 MatrixPencils = "48965c70-4690-11ea-1f13-43a2532b2fa8"
 OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
@@ -29,6 +30,7 @@ DiffEqCallbacks = "2.16"
 IterTools = "1.0"
 LaTeXStrings = "1.0"
 MacroTools = "0.5"
+MatrixEquations = "1"
 MatrixPencils = "1.6"
 OrdinaryDiffEq = "5.2, 6.0"
 Plots = "0.24, 0.25, 0.26, 0.27, 0.28, 0.29, 1.0"

src/ControlSystems.jl

@@ -114,6 +114,7 @@ import DiffEqCallbacks: SavingCallback, SavedValues
 import MatrixPencils
 using DelayDiffEq
 using MacroTools
+using MatrixEquations
 
 abstract type AbstractSystem end
 

src/matrix_comps.jl

@@ -3,78 +3,34 @@
 Compute 'X', the solution to the continuous-time algebraic Riccati equation,
 defined as A'X + XA - (XB)R^-1(B'X) + Q = 0, where R is non-singular.
 
-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
+Uses `MatrixEquations.arec`.
 """
 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
+    arec(A, B, R, Q)[1]
 end
 
+care(A::Number, B::Number, Q::Number, R::Number) = care(fill(A,1,1),fill(B,1,1),fill(Q,1,1),fill(R,1,1))
+
 """`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
+Uses `MatrixEquations.ared`.
 """
 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);
-
-    return QZ.Z[(n+1):end, 1:n]/QZ.Z[1:n, 1:n];
+    ared(A, B, R, Q)[1]
 end
 
+dare(A::Number, B::Number, Q::Number, R::Number) = dare(fill(A,1,1),fill(B,1,1),fill(Q,1,1),fill(R,1,1))
+
 """`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))
+    lyapd(A, Q)
 end
 
 """`gram(sys, opt)`
@@ -86,12 +42,11 @@ function gram(sys::AbstractStateSpace, opt::Symbol)
     if !isstable(sys)
         error("gram only valid for stable A")
     end
-    func = iscontinuous(sys) ? lyap : dlyap
+    lyapf = 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
+        return lyapf(sys.A, sys.B*sys.B')
     elseif opt === :o
-        return func(Matrix(sys.A'), sys.C'*sys.C)#::Array{numeric_type(sys),2} # lyap is type-unstable
+        return lyapf(Matrix(sys.A'), sys.C'*sys.C)
     else
         error("opt must be either :c for controllability grammian, or :o for
                 observability grammian")

src/synthesis.jl

@@ -1,5 +1,5 @@
 """
-    lqr(A, B, Q, R)
+    lqr(A, B, Q, R, args...; kwargs...)
 
 Calculate the optimal gain matrix `K` for the state-feedback law `u = -K*x` that
 minimizes the cost function:
@@ -13,6 +13,9 @@ For the continuous time model `dx = Ax + Bu`.
 Solve the LQR problem for state-space system `sys`. Works for both discrete
 and continuous time systems.
 
+The `args...; kwargs...` are sent to the Riccati solver, allowing specification of cross-covariance etc. See `?MatrixEquations.arec` for more help.
+
+See also `LQG`
 Usage example:
 ```julia
 using LinearAlgebra # For identity matrix I
@@ -31,9 +34,8 @@ y, t, x, uout = lsim(sys,u,t,x0=x0)
 plot(t,x', lab=["Position" "Velocity"], xlabel="Time [s]")
 ```
 """
-function lqr(A, B, Q, R)
-    S = care(A, B, Q, R)
-    K = R\B'*S
+function lqr(A, B, Q, R, args...; kwargs...)
+    S, _, K = arec(A, B, R, Q, args...; kwargs...)
     return K
 end
 
@@ -42,29 +44,33 @@ end
     kalman(sys, R1, R2)
 
 Calculate the optimal Kalman gain
+
+The `args...; kwargs...` are sent to the Riccati solver, allowing specification of cross-covariance etc. See `?MatrixEquations.arec/ared` for more help.
+
+See also `LQG`
 """
-kalman(A, C, R1,R2) = Matrix(lqr(A',C',R1,R2)')
+kalman(A, C, R1,R2, args...; kwargs...) = Matrix(lqr(A',C',R1,R2, args...; kwargs...)')
 
-function lqr(sys::AbstractStateSpace, Q, R)
+function lqr(sys::AbstractStateSpace, Q, R, args...; kwargs...)
     if iscontinuous(sys)
-        return lqr(sys.A, sys.B, Q, R)
+        return lqr(sys.A, sys.B, Q, R, args...; kwargs...)
     else
         return dlqr(sys.A, sys.B, Q, R)
     end
 end
 
-function kalman(sys::AbstractStateSpace, R1,R2)
+function kalman(sys::AbstractStateSpace, R1, R2, args...; kwargs...)
     if iscontinuous(sys)
-        return Matrix(lqr(sys.A', sys.C', R1,R2)')
+        return Matrix(lqr(sys.A', sys.C', R1,R2, args...; kwargs...)')
     else
-        return Matrix(dlqr(sys.A', sys.C', R1,R2)')
+        return Matrix(dlqr(sys.A', sys.C', R1,R2, args...; kwargs...)')
     end
 end
 
 
 """
-    dlqr(A, B, Q, R)
-    dlqr(sys, Q, R)
+    dlqr(A, B, Q, R, args...; kwargs...)
+    dlqr(sys, Q, R, args...; kwargs...)
 
 Calculate the optimal gain matrix `K` for the state-feedback law `u[k] = -K*x[k]` that
 minimizes the cost function:
@@ -75,6 +81,8 @@ For the discrte time model `x[k+1] = Ax[k] + Bu[k]`.
 
 See also `lqg`
 
+The `args...; kwargs...` are sent to the Riccati solver, allowing specification of cross-covariance etc. See `?MatrixEquations.ared` for more help.
+
 Usage example:
 ```julia
 using LinearAlgebra # For identity matrix I
@@ -94,15 +102,14 @@ y, t, x, uout = lsim(sys,u,t,x0=x0)
 plot(t,x', lab=["Position"  "Velocity"], xlabel="Time [s]")
 ```
 """
-function dlqr(A, B, Q, R)
-    S = dare(A, B, Q, R)
-    K = (B'*S*B + R)\(B'S*A)
+function dlqr(A, B, Q, R, args...; kwargs...)
+    S, _, K = ared(A, B, R, Q, args...; kwargs...)
     return K
 end
 
-function dlqr(sys::AbstractStateSpace, Q, R)
+function dlqr(sys::AbstractStateSpace, Q, R, args...; kwargs...)
     !isdiscrete(sys) && throw(ArgumentError("Input argument sys must be discrete-time system"))
-    return dlqr(sys.A, sys.B, Q, R)
+    return dlqr(sys.A, sys.B, Q, R, args...; kwargs...)
 end
 
 """
@@ -111,8 +118,9 @@ end
 
 Calculate the optimal Kalman gain for discrete time systems
 
+The `args...; kwargs...` are sent to the Riccati solver, allowing specification of cross-covariance etc. See `?MatrixEquations.ared` for more help.
 """
-dkalman(A, C, R1,R2) = Matrix(dlqr(A',C',R1,R2)')
+dkalman(A, C, R1,R2, args...; kwargs...) = Matrix(dlqr(A',C',R1,R2, args...; kwargs...)')
 
 """
     place(A, B, p, opt=:c)

test/test_synthesis.jl

@@ -126,10 +126,10 @@ end
     L = dlqr(A,B,Q,R)
     @test L ≈ [0.5890881713787511 0.7118839434795103]
 
-    B = [0;1]   # Note B is vector, B'B is scalar and INcompatible with matrix
+    B = [0;1]   # Note B is vector, B'B is scalar 
     Q = eye_(2)
     R = eye_(1)
-    @test_throws MethodError L ≈ dlqr(A,B,Q,R)
+    L ≈ dlqr(A,B,Q,R)
     #L ≈ [0.5890881713787511 0.7118839434795103]
 end