place command for pole placement

neveritt · neveritt · commit 76082d761304 · 2017-04-24T16:06:33.000+02:00
diff --git a/src/design/place.jl b/src/design/place.jl
@@ -0,0 +1,148 @@
+"""
+    place(A, B, p[; rtol = 1e-3, α::Real = 0, iter::Integer=200]) -> K
+
+Places the closed-loop poles of the single- or multi-input system
+
+x = Ax + Bu
+
+at the poles of the vector `p` of desired self-conjugate closed-loop pole
+locations. Computes a gain matrix `K` such that the state feedback `u = –Kx`
+places the closed-loop poles at the locations `p`. In other words, the
+eigenvalues of `A – BK` match the entries of `p` (up to the ordering).
+Place works for multi-input systems and is based on the algorithm from [1].
+A closed loop pole `pₖ` in `p` that is close enough to an eigenvalue of `A`,
+`λₖ` will not be moved by the feedback. The tolerance is set `|λₖ-pₖ|< rtol`.
+
+The non-uniqueness of solutions is used to minimize the sensitivity of
+closed-loop eigenvalues and the norm of the corresponding state feedback matrix
+`K`. The parameter `α ∈ [0,1]` determines the relative weight of these
+criteria. `α=0` only minimizes the norm of `K` and `α=1` only minimizes the
+sensitivity of the closed loop eigenvalues measured by the condition number of
+the eigenvectors of the closed loop eigenvalues.
+
+You can also use place for estimator gain selection by transposing the `A` matrix
+and substituting `C.'` for `B`.
+
+l = place(A.',C.',p).'
+
+# Examples
+```julia
+julia> A = [0 0  0   0   ;
+            1 10 100 1000;
+            0 1  10  100 ;
+            0 0  1   10  ];
+
+julia> B = [1 0;
+            0 1;
+            0 0;
+            0 0];
+
+julia> p = [-1, -2, -1+1im, -1-1im];
+
+julia> K = place(A, B, p);
+
+julia> λ, V = eig(A+B*K);
+
+julia> λ
+Complex{Float64}[4]
+  -2.00 + 0.00im
+  -1.00… + 1.00…im
+  -1.00… - 1.00…im
+  -1.00… + 0.00im
+```
+
+# References
+
+-  [1]: Varga, A. "Robust pole assignment via Sylvester equation based state
+        feedback parametrization." IEEE International Symposium on
+        Computer-Aided Control System Design, 2000.
+"""
+function place{M1<:AbstractMatrix,M2<:AbstractMatrix,M3<:AbstractVector}(
+    A::M1, B::M2, p::M3; α::Real = 0.99, rtol::Real=1e-3, iter::Integer=200)
+
+  # /TODO check (A,B) controllable
+  if length(p) != size(A,1)
+    warn("The number of poles in `p` does not match match the size of `A`")
+    throw(DomainError())
+  end
+
+  # schur factorization of A
+  Rₐ, Qₐ, λ = schur(A)
+
+  # find eigenvalues not moved
+  select = zeros(Bool, length(p))
+  for i in eachindex(p)
+    j = findfirst(y->isapprox(p[i],y; rtol=rtol), λ)
+    if (j > 0)
+      splice!(λ, j)
+      select[i] = true
+    end
+  end
+
+  m = sum(select) # number of eigenvalues not moved
+  p = float(p[!select]) # convert to float (A and B are handled by schur)
+  Ã = realjordanform(p)
+
+  # move eigenvalues to be moved to the lower right corner of the real schur
+  # form, i.e. in A₃
+  # QₐᵀAQₐ = Rₐ =  [A₁ A₂;      QₐᵀB = QB = [B₁;
+  #                 0  A₃]                   B₂]
+  ordschur!(Rₐ, Qₐ, select)
+  QB = Qₐ.'*B
+  A₃ = Rₐ[m+1:end,m+1:end]
+  B₂ = QB[m+1:end,:]
+
+  # find good G that parametrizes the freedom of the state feedback
+  G = randn(eltype(A₃), size(B₂,2), size(A₃,2))
+  # /TODO check (Ã,G) is observable
+
+  # minimize Frobenius norm of state feedback K, ‖K‖ and condition number
+  # of X, κ(X) according to cost function J = ακ(X) + (1-α)‖K‖²
+  last_g = zeros(length(G))
+  X      = zeros(A₃)
+  Xi     = zeros(A₃)
+  K      = zeros(G*Xi)
+
+  df = OnceDifferentiable(x -> place_cost(x, last_g, X, Xi, K, A₃, Ã, B₂, α),
+                         (x, stor) -> place_gradient!(stor, x, last_g, X, Xi, K, A₃, Ã, B₂, α))
+  res = optimize(df,
+                vec(G),
+                Optim.BFGS(),
+                Optim.Options(iterations = iter))
+
+  # transform controller according to the factorization of A
+  K = hcat(zeros(size(K,1), m), K)*Qₐ.'
+  K
+end
+
+function place_cost(x, last_x, X, Xi, K, A, Ã, B, α)
+  common_first_sylvester!(x, last_x, X, Xi, K, A, Ã, B)
+  α*(sumabs2(X) + sumabs2(Xi))/2 + (1-α)*sumabs2(K)/2
+end
+
+function place_gradient!(storage, x, last_x, X, Xi, K, A, Ã, B, α)
+  common_first_sylvester!(x, last_x, X, Xi, K, A, Ã, B)
+
+  # solve ÃU - UA = -S
+  H = Xi*K'
+  S = α*(-X.' + Xi*Xi.'*Xi) + (1-α)*H*K  # Xi*Xi.'*Xi (X.'*X)\Xi
+  U,scale = LAPACK.trsyl!('N', 'N', Ã, A, -S, -1)
+  scale!(U, inv(scale)) # this scale is always 1?
+
+  # calculate gradient
+  storage[1:length(x)] = vec((1-α)*H' + B.'*U.')
+end
+
+# helper function to save work done in both function call and gradient call
+function common_first_sylvester!(x, last_x, X, Xi, K, A, Ã, B)
+  if x != last_x
+    copy!(last_x, x)
+    G = reshape(x, size(B,2), size(A,2))
+
+    # solve AX - XÃ = -B*G
+    X[:], scale = LAPACK.trsyl!('N', 'N', A, Ã, -B*G, -1)
+    scale!(X, inv(scale)) # this scale is always 1?
+    Xi[:] = inv(X)
+    K[:]  = G*Xi
+  end
+end
diff --git a/src/matrix_comps/realjordanform.jl b/src/matrix_comps/realjordanform.jl
@@ -0,0 +1,59 @@
+"""
+    realjordanform(D) -> J
+
+Returns the real Jordan canonical form of the eigenvalues specified in
+the vector D.
+
+# Examples
+```julia
+julia> D = [-1, -1+2im, -1-2im, -1+2im, -1-2im];
+julia> realjordanform(D)
+5x5 Array{Int64,2}:
+ -1   0   0   0   0
+  0  -1  -2   1   0
+  0   2  -1   0   1
+  0   0   0  -1  -2
+  0   0   0   2  -1
+```
+"""
+
+function realjordanform{T<:Number}(D::AbstractVector{T})
+  sort!(D, by=imag)
+  sort!(D, by=x->abs(imag(x))) # make sure conjugated pairs are next to eachother
+  sort!(D, by=real)
+
+  J = zeros(real(T), length(D), length(D))
+  i = 1
+  while i <= length(D)
+    d = D[i]
+    if isreal(d)
+      n = findlast(D, d) - i + 1 # multiplicity of real eigenvalue
+      @assert all(D[i+j] == d for j = 1:n-1) "There should be $n eigenvalues equal to $d"
+
+      # fill jordan block for real eigenvalue
+      J[i,i] = d
+      for k = 1:n-1
+        J[i+k-1,i+k] = 1
+        J[i+k,i+k]   = d
+      end
+      i += n
+    else
+      n = findlast(D, d) - i + 1 # multiplicity of complex eigenvalue
+      @assert all(D[i+j] == d for j = 1:n-1) "There should be $n eigenvalues equal to $d"
+      @assert all(D[i+n+j] == conj(d) for j = 1:n-1) "There should be $n eigenvalues equal to $(conj(d))"
+      # construct real block C
+      a = real(d)
+      b = imag(d)
+      C = [a b; -b a]
+
+      # fill jordan block for complex eigenvalue
+      J[i:i+1,i:i+1] = C
+      for k = 1:n-1
+        J[i+2(k-1)+(0:1),i+2k+(0:1)] = eye(C)
+        J[i+2k+(0:1),i+2k+(0:1)]     = C
+      end
+      i += 2n
+    end
+  end
+  J
+end
diff --git a/test/design/place.jl b/test/design/place.jl
@@ -0,0 +1,143 @@
+# These examples are mostly taken from [1] and the references therein.
+# References
+#
+#  [1]: Chu, E.K. "Pole assignment via the Schur form." Systems & control
+#       letters 56.4 (2007): 303-314.
+# example 1
+A₁ = [1.38       -2.077e-1  6.715  -5.67   ;
+     -5.814e-1   -4.29      0       6.77e-1;
+      1.067       4.273    -6.654   5.893  ;
+      4.8e-2      4.273     1.343  -2.104  ]
+B₁ = [0      0    ;
+      5.679  0    ;
+      1.136 -3.146;
+      1.136  0    ]
+p₁ = [-0.2, -0.5, -5.0566, -8.6659]
+κ₁ = 5
+c₁ = 5
+
+# example 2
+A₂ = [-1.094e-1   6.28e-2    0         0         0       ;
+       1.306     -2.132      9.87e-1   0         0       ;
+       0          1.595     -3.149     1.547     0       ;
+       0          3.55e-2    2.632    -4.257     1.855   ;
+       0          2.3e-3     0         1.636e-1 -1.625e-1]
+B₂ = [0          0       ;
+      6.38e-2    0       ;
+      8.38e-2   -1.396e-1;
+      1.004e-1  -2.06e-1 ;
+      6.3e-3    -1.28e-2 ]
+p₂ = [-0.2, -0.5, -1, -1+im, -1-im]
+κ₂ = 1_000
+c₂ = 200
+
+# example 3
+A₃ = [-65   65  -19.5  19.4;
+       0.1 -0.1  0     0   ;
+       1    0   -0.5  -1   ;
+       0    0    0.4   0   ]
+B₃ = [65  0  ;
+      0   0  ;
+      0   0.4;
+      0   0  ]
+p₃ = [-1, -2, -3, -4.0]
+κ₃ = 50
+c₃ = 100
+
+# example 4
+A₄ = [ 0   1   0;
+       0   0   1;
+      -6  -11 -6]
+B₄ = [1 1;
+      0 1;
+      1 1]
+p₄ = [-1, -2, -3]
+κ₄ = 1_000
+c₄ = 1
+
+# example 5
+D  = diagm([1,10,0.1,0.1,10])
+D₁ = diagm([1,0.1,10,10,0.1])
+D₂ = diagm([1e5,1e2])
+A₅ = D*[-1.29e-1  0  3.96e-2   2.5e-2    1.91e-2;
+         3.29e-3  0 -7.79e-5   1.22e-4  -6.21e-1;
+         7.18e-2  0 -1e-1      8.87e-4  -3.85   ;
+         4.11e-2  0  0        -8.22e-2   0      ;
+         3.51e-4  0  3.5e-5    4.26e-5  -7.43e-2]*D₁
+B₅ = D*[0        1.39e-3;
+        0        3.59e-5;
+        0       -9.89e-3;
+        2.49e-5  0      ;
+        0       -5.34e-6]*D₂
+p₅ = [-0.01, -0.02, -0.03, -0.04, -0.05]
+κ₅ = 1_000
+c₅ = 5
+
+# example 6
+A₆ = [5.8765 9.3456 4.5634 9.3520;
+      6.6526 0.5867 3.5829 0.6534;
+      0      9.6738 7.4876 4.7654;
+      0      0      6.6784 2.5678]
+B₆ = [3.9878 0.5432;
+      0      2.7650;
+      0      0     ;
+      0      0     ]
+p₆ = [-29.4986, -10.0922, 2.5201+6.891im, 2.5201-6.891im]
+κ₆ = 5
+c₆ = 50
+
+# example 7 (pole of multiplicity 4 > rank(B) = 2)
+A₇ = [1.38       -2.077e-1  6.715  -5.67   ;
+     -5.814e-1   -4.29      0       6.77e-1;
+      1.067       4.273    -6.654   5.893  ;
+      4.8e-2      4.273     1.343  -2.104  ]
+B₇ = [0      0    ;
+      5.679  0    ;
+      1.136 -3.146;
+      1.136  0    ]
+p₇ = [-0.5+1im, -0.5-1im, -0.5+1im, -0.5-1im]
+κ₇ = 1e12
+c₇ = 5
+
+# example 8
+A₈ = [ 0        1        0       0      ;
+       5.32e-7 -4.18e-1 -0.12    2.32e-2;
+      -4.62e-9  1       -0.752  -2.39e-2;
+      -5.61e-1  0       -0.3    -1.74e-2]
+B₈ = [ 0         0      ;
+      -1.72e-1   7.45e-6;
+      -2.38e-2  -7.78e-5;
+       0         3.69e-3]
+p₈ = [-1,-2,-3,-4]
+κ₈ = 1_000
+c₈ = 1_000
+
+# example 9 (ill-conditioned example)
+A₉ = [0 0  0   0   ;
+      1 10 100 1000;
+      0 1  10  100 ;
+      0 0  1   10  ]
+B₉ = [1 0;
+      0 1;
+      0 0;
+      0 0]
+p₉ = [-1,-2,-3,-4]
+κ₉ = 1e6
+c₉ = 10_000
+
+for (A,B,p,κ,c) in ((A₁,B₁,p₁,κ₁,c₁), (A₂,B₂,p₂,κ₂,c₂), (A₃,B₃,p₃,κ₃,c₃),
+                    (A₄,B₄,p₄,κ₄,c₄), (A₅,B₅,p₅,κ₅,c₅), (A₆,B₆,p₆,κ₆,c₆),
+                    (A₇,B₇,p₇,κ₇,c₇), (A₈,B₈,p₈,κ₈,c₈), (A₉,B₉,p₉,κ₉,c₉))
+  F = place(A, B, p)
+  λ, V = eig(A+B*F)
+
+  @test cond(V)     < κ
+  @test vecnorm(F)  < c
+  for i in 1:length(p)
+    j = findfirst(y->isapprox(y, p[i], rtol=1e-3), λ)
+    if j > 0
+      splice!(λ,j)
+    end
+  end
+  @test isempty(λ)
+end
diff --git a/test/matrix_comps/realjordanform.jl b/test/matrix_comps/realjordanform.jl
@@ -0,0 +1,11 @@
+# test real jordan form
+D = [-1, -1+2im, -1+1im, -1-1im,-1-2im, -1+2im, -1-2im]
+C1 = [-1 -1; 1 -1]
+C2 = [-1 -2; 2 -1]
+JD = zeros(Int, 7,7)
+JD[1,1] = -1
+JD[2:3,2:3] = C1
+JD[4:5,4:5] = JD[6:7,6:7] = C2
+JD[4:5,6:7] = eye(Int, 2)
+
+@test realjordanform(D) == JD
diff --git a/test/runtests.jl b/test/runtests.jl
@@ -3,3 +3,6 @@ using Base.Test
 
 # write your own tests here
 @test 1 == 1
+
+#include("matrix_comps/realjordanform.jl")
+#include("design/place.jl")
