Update prescale function

src/matrix_comps.jl

@@ -568,7 +568,7 @@ end
     syst = similarity_transform(sys, T)
 Perform a similarity transform `T : Tx̃ = x` on `sys` such that
 ```
-Ã = T⁻¹AT
+Ã = T⁻¹AT
 B̃ = T⁻¹ B
 C̃ = CT
 D̃ = D
@@ -585,24 +585,31 @@ end
 
 """
     syst, S = prescale(sys)
-Perform a eigendecomposition on system state-transition matrix `sys.A`.
+
+Balances the metasystem matrix
 ```
-Ã = S⁻¹AS
-B̃ = S⁻¹ B
-C̃ = CS
-D̃ = D
+    M = [A B;
+         C 0]
 ```
-Such that `Ã` is diagonal.
-Returns a new scaled state-space object and the associated transformation
-matrix.
+such that their column and row norms are closer.
+This results in a system with overall better numerical conditioning.
+
+Returns a new scaled state-space object.
 """
-function prescale(sys::StateSpace)
-    d, S = eigen(sys.A)
-    A = Diagonal(d)
-    B = S\sys.B
-    C = sys.C*S
-    normalized_sys = iscontinuous(sys) ? ss(A, B, C, sys.D) : ss(A, B, C, sys.D, sys.Ts)
-    return normalized_sys, S
+function prescale(sys::ST) where ST <: AbstractStateSpace
+    n, m = size(sys.B)
+    p = size(sys.C, 1)
+
+    # create an augmented system to balance the whole system at once
+    metasys = [sys.A sys.B; sys.C zeros(p, m)]
+    S, P, B = balance(metasys)
+
+    # reconstruct A, B and C by taking slices of the balanced metasystem
+    bal_metasys = P\B*P  # undo permutation
+    A = bal_metasys[1:n, 1:n]
+    B = bal_metasys[1:n, n+1:end]
+    C = bal_metasys[n+1:end, 1:n]
+    return ST(A, B, C, sys.D, sys.timeevol)
 end
 
 """

test/test_matrix_comps.jl

@@ -70,11 +70,13 @@ syst = similarity_transform(sys, Tr)
 @test sys.B ≈ Tr*syst.B
 @test sys.C*Tr ≈ syst.C
 
-nsys, T = prescale(sys)
-@test isdiag(nsys.A)
-@test T*nsys.A ≈ sys.A*T
-@test T*nsys.B ≈ sys.B
-@test nsys.C ≈ sys.C*T
+nsys = prescale(sys)
+@test pole(nsys) ≈ pole(sys)
+n, m = size(sys.B)
+p = size(sys.C, 1)
+M = [nsys.A nsys.B; nsys.C zeros(p, m)]
+M2 = [sys.A sys.B; sys.C zeros(p, m)]
+@test cond(M) <= cond(M2)
 
 sys = ss([1 0.1; 0 1], ones(2), [1. 0], 0)
 sysi = ControlSystems.innovation_form(sys, I, I)