Implementation of place for MIMO systems (#854)

* WIP implementation of palce for MIMO systems

* finish MIMO place

lib/ControlSystemsBase/ext/ControlSystemsBaseImplicitDifferentiationExt.jl

@@ -215,7 +215,7 @@ const implicit_hinfnorm = ImplicitFunction(forward_hinfnorm, conditions_hinfnorm
 """
     hinfnorm(sys::StateSpace{Continuous, <:Dual}; kwargs)
 
-The H∞ norm can be differentiated through using ForwardDiff.jl, but at the time of writing, is limited to systems with *either* a signel input *or* a signle output. 
+The H∞ norm can be differentiated through using ForwardDiff.jl, but at the time of writing, is limited to systems with *either* a signel input *or* a single output. 
 
 A reverse-differention rule is defined in RobustAndOptimalControl.jl, which means that hinfnorm is differentiable using, e.g., Zygote in reverse mode.
 """

lib/ControlSystemsBase/src/synthesis.jl

@@ -121,23 +121,23 @@ eigvals(A-B*K2)
 
 Please note that this function can be numerically sensitive, solving the placement problem in extended precision might be beneficial.
 """
-function place(A, B, p, opt=:c)
+function place(A, B, p, opt=:c; kwargs...)
     n = length(p)
     n != size(A,1) && error("Must specify as many poles as states")
     if opt === :c
         n != size(B,1) && error("A and B must have same number of rows")
         if size(B,2) == 1
             acker(A, B, p)
         else
-            error("place only implemented for SISO systems")
+            place_knvd(A, B, p; kwargs...)
         end
     elseif opt === :o
         C = B # B is really the "C matrix"
         n != size(C,2) && error("A and C must have same number of columns")
         if size(C,1) == 1
             acker(A', C', p)'
         else
-            error("place only implemented for SISO systems")
+            place_knvd(A', C', p; kwargs...)'
         end
     else
         error("fourth argument must be :c or :o")
@@ -170,3 +170,136 @@ function acker(A,B,P)
     end
     return [zeros(1,n-1) 1]*(S\q)
 end
+
+
+"""
+    place_knvd(A::AbstractMatrix, B, λ; verbose = false, init = :s)
+
+Robust pole placement using the algorithm from
+> "Robust Pole Assignment in Linear State Feedback", Kautsky, Nichols, Van Dooren
+
+This implementation uses "method 0" for the X-step and the QR factorization for all factorizations.
+
+This function will be called automatically when [`place`](@ref) is called with a MIMO system.
+
+# Arguments:
+- `init`: Determines the initialization strategy for the iterations for find the `X` matrix. Possible choices are `:id` (default), `:rand`, `:s`. 
+"""
+function place_knvd(A::AbstractMatrix, B, λ; verbose=false, init=:s, method = 0)
+    n, m = size(B)
+    T = float(promote_type(eltype(A), eltype(B)))
+    CT = Complex{real(T)}
+    λ = sort(λ, by=LinearAlgebra.eigsortby)
+    length(λ) == size(A, 1) == n || error("Must specify as many poles as states")
+    Λ = diagm(λ)
+    QRB = qr(B)
+    U0, U1 = QRB.Q[:, 1:m], QRB.Q[:, m+1:end] # TODO: check dimension
+    Z = QRB.R
+    R = svdvals(B)
+    m = count(>(100*eps()*R[1]), R) # Rank of B
+    if m == n # Easy case, B is full rank
+        r = count(e->imag(e) == 0, λ)
+        ABF = diagm(real(λ))
+        j = r+1
+        while j <= n-1
+            ABF[j, j+1] = imag(λ[j])
+            ABF[j+1, j] = - imag(λ[j])
+            j += 2
+        end;
+        return B\(A - ABF) # Solve for F in (A - BF) = Λ
+    end
+
+
+    S = Matrix{CT}[]
+    for j = 1:n
+        qj = qr((U1'*(A- λ[j]*I))')
+        # @assert nr == m
+        # Ŝj = qj.Q[:, 1:n-m] # Needed for method 2
+        Sj = qj.Q[:, n-m+1:n]
+        # nr = size(qj.R, 1)
+        # Sj = qj.Q[:, nr+1:n]
+
+        push!(S, Sj)
+    end
+    if m == 1 # Shortcut
+        verbose && @info "Shortcut"
+        X = S
+        M = real(X*Λ/X)
+        F = real((Z\U0')*(M-A))
+        return -F
+    end
+
+    # Init
+    if init === :id
+        X = Matrix(one(CT)*I(n))
+    elseif init === :rand
+        X = randn(CT, n, n)
+    elseif init === :s
+        X = zeros(CT, n, n)
+        @views for j = 1:n
+            X[:,j] = sum(S[j], dims=2)
+            X[:,j] ./= norm(X[:,j])
+        end
+    elseif init === :acker
+        P = randn(m,1) # Random projection matrix
+        K = place(A,B*P,λ)
+        L = P*K
+        M = A - B*L
+        X = complex.(eigen(M).vectors)
+    else
+        error("Unknown init method")
+    end
+
+    cond_old = float(T)(Inf)
+    if method == 0
+        for i = 1:200
+            verbose && @info "Iteration $i"
+            for j = 1:n
+                Xj = qr(X[:, setdiff(1:n, j)])
+                ỹ = Xj.Q[:, end]
+                STy = S[j]'ỹ
+                xj = S[j]*(STy ./ norm(STy))
+                any(!isfinite, xj) && error("Not finite")
+                X[:, j] = xj
+            end
+            c = cond(X)
+            verbose && @info "cond(X) = $c"
+            if cond_old - c < 1e-14
+                break
+            end
+            cond_old = c
+            i == 200 && @warn "Max iterations reached"
+        end
+    # elseif method == 1
+    #     for i = 1:200
+    #         verbose && @info "Iteration $i"
+    #         for j = 1:n
+    #             Xj = qr(X[:, setdiff(1:n, j)])
+    #             Rj = Xj.R
+    #             Qj = Xj.Q[:, 1:end-1]
+    #             qj = Xj.Q[:, end]
+    #             σpt = qj'S[j]
+    #             σ = norm(σpt)
+    #             p = vec(σpt) ./ σ
+    #             ρ = sqrt(σ^(-2)*(w̃'w̃ + 1))
+    #             xj = inv(ρ*σ)*S[j]
+
+    #             any(!isfinite, xj) && error("Not finite")
+    #             X[:, j] = xj
+    #         end
+    #         c = cond(X)
+    #         verbose && @info "cond(X) = $c"
+    #         if cond_old - c < 1e-14
+    #             break
+    #         end
+    #         cond_old = c
+    #         i == 200 && @warn "Max iterations reached"
+    #     end
+    else
+        error("Only method 0 is implemented")
+    end
+    verbose && @info "norm X = $(norm(X))"
+    M = X*Λ/X
+    F = real((Z\U0')*(M-A))
+    -F # Paper assumes positive feedback
+end

lib/ControlSystemsBase/test/test_synthesis.jl

@@ -101,6 +101,76 @@ K = ControlSystemsBase.acker(A,B,p)
 @test ControlSystemsBase.eigvalsnosort(A-B*K) ≈ p
 end
 
+
+@testset "MIMO place" begin
+
+    function allin(a, b; tol = 1e-2)
+        all(minimum(abs, a .- b', dims=2)[:] .< tol)
+    end
+
+    for i = 1:10
+        # B smaller than A
+        sys = ssrand(2,2,3)
+        # @show cond(gram(sys, :c))
+        (; A, B) = sys
+        p = [-1.0, -2, -3]
+        L = place(A, B, p)
+        @test eltype(L) <: Real
+        @test allin(eigvals(A - B*L), p)
+
+        # p = [-3.0, -1-im, -1+im]
+        # L = place(A, B, p)
+        # @test eltype(L) <: Real
+
+        # cond(gram(sys, :c))
+        # @test allin(eigvals(A - B*L), p)
+
+
+        # B same size as A
+        sys = ssrand(2,3,3)
+        (; A, B) = sys
+        p = [-1.0, -2, -3]
+        L = place(A, B, p)
+        @test eltype(L) <: Real
+        @test allin(eigvals(A - B*L), p)
+
+        p = [-3.0, -1-im, -1+im]
+        L = place(A, B, p)
+        @test eltype(L) <: Real
+        @test allin(eigvals(A - B*L), p)
+
+        # deadbeat
+        A = [0 1; 0 0]
+        B = I(2)
+        sys = ss(A, B, I, 0)
+        sysd = c2d(sys, 0.1)
+
+        p = [0,0]
+        L = place(sysd, p)
+        @test eltype(L) <: Real
+        @test allin(eigvals(sysd.A - sysd.B*L), p)
+
+    end
+
+    A = [
+        -0.1094 0.0628 0 0 0
+        1.306 -2.132 0.9807 0 0
+        0 1.595 -3.149 1.547 0
+        0 0.0355 2.632 -4.257 1.855
+        0 0.00227 0 0.1636 -0.1625
+    ]
+    B = [
+        0 0.0638 0.0838 0.1004 0.0063
+        0 0 -0.1396 -0.206 -0.0128
+    ]'
+    # p = [-0.07732, -0.01423, -0.8953, -2.841, -5.982]
+    p = [-0.2, -0.5, -1, -1+im, -1-im]
+    L = place(A, B, p; verbose=true) # with verbose, should prind cond(X) ≈ 39.4 which it does 
+    @test allin(eigvals(A - B*L), p; tol=0.025) # Tolerance from paper
+    # norm(L)
+
+end
+
 @testset "LQR" begin
     Ts = 0.1
     A = [1 Ts; 0 1]