From b51adf744d7e98b2fdb19d5f07cf67c064a6ed7a Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <baggepinnen@gmail.com>
Date: Wed, 6 Dec 2023 14:07:17 +0100
Subject: [PATCH] handle proper quotient of proper statespace systems

---
 .../src/types/StateSpace.jl                   | 50 ++++++++++++++++++-
 .../test/test_statespace.jl                   | 11 +++-
 2 files changed, 58 insertions(+), 3 deletions(-)

diff --git a/lib/ControlSystemsBase/src/types/StateSpace.jl b/lib/ControlSystemsBase/src/types/StateSpace.jl
index 9f0fec519..0a358387f 100644
--- a/lib/ControlSystemsBase/src/types/StateSpace.jl
+++ b/lib/ControlSystemsBase/src/types/StateSpace.jl
@@ -378,7 +378,55 @@ end
 
 
 ## DIVISION ##
-/(sys1::AbstractStateSpace, sys2::AbstractStateSpace) = sys1*inv(sys2)
+
+
+"""
+    /(sys1::AbstractStateSpace{TE}, sys2::AbstractStateSpace{TE}; atol::Real = 0, atol1::Real = atol, atol2::Real = atol, rtol::Real = max(size(sys1.A, 1), size(sys2.A, 1)) * eps(real(float(one(numeric_type(sys1))))) * iszero(min(atol1, atol2)))
+
+Compute `sys1 / sys2 = sys1 * inv(sys2)` in a way that tries to handle situations in which the inverse `sys2` is non-proper, but the resulting system `sys1 / sys2` is proper.
+
+See `ControlSystemsBase.MatrixPencils.isregular` for keyword arguments `atol`, `atol1`, `atol2`, and `rtol`.
+"""
+function Base.:(/)(sys1::AbstractStateSpace{TE}, sys2::AbstractStateSpace{TE}; 
+    atol::Real = 0, atol1::Real = atol, atol2::Real = atol, 
+    rtol::Real = max(size(sys1.A,1),size(sys2.A,1))*eps(real(float(one(numeric_type(sys1)))))*iszero(min(atol1,atol2))) where {TE<:ControlSystemsBase.TimeEvolution}
+    T1 = float(numeric_type(sys1))
+    T2 = float(numeric_type(sys2))
+    T = promote_type(T1,T2)
+    timeevol = common_timeevol(sys1, sys2)
+    ny2, nu2 = sys2.ny, sys2.nu
+    nu2 == ny2  || error("The system sys2 must be square")
+    ny1, nu1 = sys1.ny, sys1.nu
+    nu1 == nu2  || error("The systems sys1 and sys2 must have the same number of inputs")
+    nx1 = sys1.nx
+    nx2 = sys2.nx
+    if nx2 > 0
+        A, B, C, D = ssdata([sys2; sys1])
+        Ai = [A B; C[1:ny2,:] D[1:ny2,:]]
+        Ei = [I zeros(T,nx1+nx2,ny2); zeros(T,ny2,nx1+nx2+ny2)] |> Matrix # TODO: rm call to Matrix when type piracy in https://github.com/JuliaLinearAlgebra/LinearMaps.jl/issues/219 is fixed
+        MatrixPencils.isregular(Ai, Ei; atol1, atol2, rtol) || 
+            error("The system sys2 is not invertible")
+        Ci = [C[ny2+1:ny1+ny2,:] D[ny2+1:ny1+ny2,:]]
+        Bi = [zeros(T,nx1+nx2,nu1); -I] |> Matrix # TODO: rm call to Matrix when type piracy in https://github.com/JuliaLinearAlgebra/LinearMaps.jl/issues/219 is fixed
+        Di = zeros(T,ny1,nu1)
+        Ai, Ei, Bi, Ci, Di = MatrixPencils.lsminreal(Ai, Ei, Bi, Ci, Di; fast = true, atol1 = 0, atol2, rtol, contr = true, obs = true, noseig = true)
+        if Ei != I
+            luE = lu!(Ei, check=false)
+            issuccess(luE) || throw(ArgumentError("The system sys2 is not invertible"))
+            Ai = luE\Ai
+            Bi = luE\Bi
+        end
+    else
+        D2 = T.(sys2.D)
+        LUD = lu(D2)
+        (norm(D2,Inf) <= atol1 || rcond(LUD.U) <= 10*nu1*eps(real(float(one(T))))) && 
+                error("The system sys2 is not invertible")
+        Ai, Bi, Ci, Di = ssdata(sys1)
+        rdiv!(Bi,LUD); rdiv!(Di,LUD)
+    end
+
+    return StateSpace{TE, T}(Ai, Bi, Ci, Di, timeevol) 
+end
 
 function /(n::Number, sys::ST) where ST <: AbstractStateSpace
     # Ensure s.D is invertible
diff --git a/lib/ControlSystemsBase/test/test_statespace.jl b/lib/ControlSystemsBase/test/test_statespace.jl
index ab9693814..b31f1716a 100644
--- a/lib/ControlSystemsBase/test/test_statespace.jl
+++ b/lib/ControlSystemsBase/test/test_statespace.jl
@@ -159,11 +159,18 @@
 
         # Division
         @test 1/C_222_d == SS([-6 -3; 2 -11],[1 0; 0 2],[-1 0; -0 -1],[1 -0; 0 1])
-        @test C_221/C_222_d == SS([-5 -3 -1 0; 2 -9 -0 -2; 0 0 -6 -3;
-        0 0 2 -11],[1 0; 0 2; 1 0; 0 2],[1 0 0 0],[0 0])
+        @test hinfnorm((C_221/C_222_d) - SS([-5 -3 -1 0; 2 -9 -0 -2; 0 0 -6 -3;
+        0 0 2 -11],[1 0; 0 2; 1 0; 0 2],[1 0 0 0],[0 0]))[1] < 1e-10
         @test 1/D_222_d == SS([-0.8 -0.8; -0.8 -1.93],[1 0; 0 2],[-1 0; -0 -1],
         [1 -0; 0 1],0.005)
 
+        # Division when denominator inverse is non-proper but quotient is proper
+        G1 = tf([1], [1, 1])
+        G2 = tf([1], [1, 1, 1])
+        G1s = ss(G1)
+        G2s = ss(G2)
+        @test tf(G2s / G1s) ≈ G2 / G1
+
         fsys = ss(1,1,1,0)/3 # Int becomes FLoat after division
         @test fsys.B[]*fsys.C[] == 1/3