generalize also extended controller

Author: baggepinnen

src/lqg.jl

@@ -271,6 +271,7 @@ function extended_controller(K::AbstractStateSpace)
 end
 
 """
+    extended_controller(P::StateSpace, L, K; z = nothing)
     extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l); z = nothing)
 
 Returns a statespace system representing the controller that is obtained when state-feedback `u = L(xᵣ-x̂)` is combined with a Kalman filter with gain `K` that produces state estimates x̂. The controller is an instance of `ExtendedStateSpace` where `C2 = -L, D21 = L` and `B2 = K`.
@@ -291,8 +292,7 @@ system_mapping(Ce) == -C
 
 Please note, without the reference pre-filter, the DC gain from references to controlled outputs may not be identity. If a vector of output indices is provided through the keyword argument `z`, the closed-loop system from state reference `xᵣ` to outputs `z` is returned as a second return argument. The inverse of the DC-gain of this closed-loop system may be useful to compensate for the DC-gain of the controller.
 """
-function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::AbstractMatrix = kalman(l); z::Union{Nothing, AbstractVector} = nothing)
-    P = system_mapping(l, identity)
+function extended_controller(P::AbstractStateSpace, L::AbstractMatrix, K::AbstractMatrix; z::Union{Nothing, AbstractVector} = nothing)
     A,B,C,D = ssdata(P)
     Ac = A - B*L - K*C + K*D*L # 8.26b
     (; nx, nu, ny) = P
@@ -302,7 +302,7 @@ function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::Abstr
     D21 = L #   L*xᵣ # should be D21?
     C2 = -L # - L*x̂
     C1 = zeros(0, nx)
-    Ce0 = ss(Ac, B1, B2, C1, C2; D21, Ts = l.timeevol)
+    Ce0 = ss(Ac, B1, B2, C1, C2; D21, Ts = P.timeevol)
     if z === nothing
         return Ce0
     end
@@ -313,6 +313,11 @@ function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::Abstr
 end
 
 
+function extended_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::AbstractMatrix = kalman(l); kwargs...)
+    P = system_mapping(l, identity)
+    extended_controller(P, L, K; kwargs...)
+end
+
 """
     observer_controller(l::LQGProblem, L = lqr(l), K = kalman(l))
 
@@ -377,7 +382,8 @@ function ff_controller(sys::AbstractStateSpace, L, K, Lr = nothing; comp_dc = tr
     if comp_dc
         if Lr === nothing
             Cfb = observer_controller(sys, L, K)
-            Lr = pinv(dcgain(feedback(sys*Cfb)))
+            Cff0 = ss(I(sys.nu), sys.timeevol) - ss(Ac, Be, Cc, Dc, sys.timeevol)
+            Lr = pinv(dcgain(feedback(sys, Cfb)*Cff0))
         end
         Bc = Be*Lr
         return Lr - ss(Ac, Bc, Cc, Dc, sys.timeevol)