WIP implementation of palce for MIMO systems

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

@@ -129,15 +129,15 @@ function place(A, B, p, opt=:c)
         if size(B,2) == 1
             acker(A, B, p)
         else
-            error("place only implemented for SISO systems")
+            place_knvd(A, B, p)
         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)'
         end
     else
         error("fourth argument must be :c or :o")
@@ -170,3 +170,99 @@ function acker(A,B,P)
     end
     return [zeros(1,n-1) 1]*(S\q)
 end
+
+
+"""
+    place_knvd(A::AbstractMatrix, B, λ; verbose = false, init = :id)
+
+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=:rand)
+    n, m = size(B)
+    T = float(promote_type(eltype(A), eltype(B)))
+    CT = Complex{real(T)}
+    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
+    sλ = sort(λ, by=real)
+    if m == n # Easy case, B is full rank
+        r = count(e->imag(e) == 0, λ)
+        ABF = diagm(real(sλ))
+        j = r+1
+        while j <= n-1
+            ABF[j, j+1] = imag(sλ[j])
+            ABF[j+1, j] = - imag(sλ[j])
+            j += 2
+        end;
+        return B\(A - ABF)
+    end
+
+
+    S = Matrix{CT}[]
+    for j = 1:n
+        qj = qr((U1'*(A- λ[j]*I))')
+        # @assert nr == m
+        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)
+        for j = 1:n
+            X[:,j] = sum(S[j], dims=2)
+            X[:,j] = X[:,j]/norm(X[:,j])
+        end
+    else
+        error("Unknown init method")
+    end
+
+    cond_old = float(T)(Inf)
+    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
+    M = X*Λ/X
+    F = real((Z\U0')*(M-A))
+    -F # Paper assumes positive feedback
+end

lib/ControlSystemsBase/test/test_synthesis.jl

@@ -101,6 +101,60 @@ K = ControlSystemsBase.acker(A,B,p)
 @test ControlSystemsBase.eigvalsnosort(A-B*K) ≈ p
 end
 
+
+@testset "MIMO place" begin
+
+    function allin(a, b)
+        all(minimum(abs, a .- b', dims=2)[:] .< 10e-2)
+    end
+
+    for i = 1:100
+        # 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
+
+        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
+
+
+end
+
 @testset "LQR" begin
     Ts = 0.1
     A = [1 Ts; 0 1]