finish MIMO place

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
-            place_knvd(A, B, p)
+            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
-            place_knvd(A', C', p)'
+            place_knvd(A', C', p; kwargs...)'
         end
     else
         error("fourth argument must be :c or :o")
@@ -173,7 +173,7 @@ end
 
 
 """
-    place_knvd(A::AbstractMatrix, B, λ; verbose = false, init = :id)
+    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
@@ -185,35 +185,36 @@ This function will be called automatically when [`place`](@ref) is called with a
 # 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)
+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
-    sλ = sort(λ, by=real)
     if m == n # Easy case, B is full rank
         r = count(e->imag(e) == 0, λ)
-        ABF = diagm(real(sλ))
+        ABF = diagm(real(λ))
         j = r+1
         while j <= n-1
-            ABF[j, j+1] = imag(sλ[j])
-            ABF[j+1, j] = - imag(sλ[j])
+            ABF[j, j+1] = imag(λ[j])
+            ABF[j+1, j] = - imag(λ[j])
             j += 2
         end;
-        return B\(A - ABF)
+        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]
@@ -235,33 +236,69 @@ function place_knvd(A::AbstractMatrix, B, λ; verbose=false, init=:rand)
         X = randn(CT, n, n)
     elseif init === :s
         X = zeros(CT, n, n)
-        for j = 1:n
+        @views for j = 1:n
             X[:,j] = sum(S[j], dims=2)
-            X[:,j] = X[:,j]/norm(X[:,j])
+            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)
-    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
+    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
-        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"
+    # 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

lib/ControlSystemsBase/test/test_synthesis.jl

@@ -104,26 +104,26 @@ end
 
 @testset "MIMO place" begin
 
-    function allin(a, b)
-        all(minimum(abs, a .- b', dims=2)[:] .< 10e-2)
+    function allin(a, b; tol = 1e-2)
+        all(minimum(abs, a .- b', dims=2)[:] .< tol)
     end
 
-    for i = 1:100
+    for i = 1:10
         # B smaller than A
         sys = ssrand(2,2,3)
-        @show cond(gram(sys, :c))
+        # @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
+        # 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)
+        # cond(gram(sys, :c))
+        # @test allin(eigvals(A - B*L), p)
 
 
         # B same size as A
@@ -152,6 +152,22 @@ end
 
     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