add option to design observers with direct term (#861)

* add observer_filter

* add option to design observers with direct term

This applies to several functions
- `kalman`
- `observer_controller`
- `place`
- `observer_filter` (new function in this PR)

* fix diff kalman

* rearrange tests

* only issue error for cont. systems

* expand reference

lib/ControlSystemsBase/ext/ControlSystemsBaseImplicitDifferentiationExt.jl

@@ -116,7 +116,7 @@ To make the Kalman solver work with dual numbers, make sure that the `R` matrix
 function kalman(::DiscreteType, A, C, Q, R::AbstractMatrix{<:Dual})
     X = are(Discrete, A', C', Q, R)
     CX = C*X
-    (R+CX*B)\(CX*A)
+    (R+CX*C')\(CX*A')
 end
 
 

lib/ControlSystemsBase/src/ControlSystemsBase.jl

@@ -45,6 +45,7 @@ export  LTISystem,
         time_scale,
         innovation_form,
         observer_predictor,
+        observer_filter,
         observer_controller,
         # Stability Analysis
         isstable,

lib/ControlSystemsBase/src/discrete.jl

@@ -152,7 +152,7 @@ rstc(args...)=rst(args..., ;cont=true)
 
 Polynomial synthesis in discrete time.
 
-Polynomial synthesis according to CCS ch 10 to
+Polynomial synthesis according to "Computer-Controlled Systems" ch 10 to
 design a controller ``R(q) u(k) = T(q) r(k) - S(q) y(k)``
 
 Inputs:

lib/ControlSystemsBase/src/matrix_comps.jl

@@ -747,8 +747,8 @@ function innovation_form(sys::ST, K) where ST <: AbstractStateSpace
 end
 
 """
-    observer_predictor(sys::AbstractStateSpace, K; h::Int = 1)
-    observer_predictor(sys::AbstractStateSpace, R1, R2[, R12])
+    observer_predictor(sys::AbstractStateSpace, K; h::Int = 1, output_state = false)
+    observer_predictor(sys::AbstractStateSpace, R1, R2[, R12]; output_state = false)
 
 If `sys` is continuous, return the observer predictor system
 ```math
@@ -763,22 +763,25 @@ If `sys` is discrete, the prediction horizon `h` may be specified, in which case
 
 If covariance matrices `R1, R2` are given, the kalman gain `K` is calculated using [`kalman`](@ref).
 
-See also [`innovation_form`](@ref) and [`observer_controller`](@ref).
+If `output_state` is true, the output is the state estimate `x̂` instead of the output estimate `ŷ`.
+
+See also [`innovation_form`](@ref), [`observer_controller`](@ref) and [`observer_filter`](@ref).
 """
 function observer_predictor(sys::AbstractStateSpace, R1, R2::Union{AbstractArray, UniformScaling}, args...; kwargs...)
     K = kalman(sys, R1, R2, args...)
     observer_predictor(sys, K; kwargs...)
 end
 
-function observer_predictor(sys::AbstractStateSpace, K::AbstractMatrix; h::Integer = 1)
+function observer_predictor(sys::AbstractStateSpace, K::AbstractMatrix; h::Integer = 1, output_state = false)
     h >= 1 || throw(ArgumentError("h must be positive."))
     ny = noutputs(sys)
     size(K, 1) == sys.nx && size(K,2) == ny || throw(ArgumentError("K has the wrong size, expected $((sys.nx, ny))"))
     A,B,C,D = ssdata(sys)
     if h == 1
-        ss(A-K*C, [B-K*D K], C, [D zeros(ny, ny)], sys.timeevol)
+        ss(A-K*C, [B-K*D K], output_state ? I : C, output_state ? 0 : [D zeros(ny, ny)], sys.timeevol)
     else
         isdiscrete(sys) || throw(ArgumentError("A prediction horizon is only supported for discrete systems. "))
+        output_state && throw(ArgumentError("output_state is not supported for h > 1."))
         # The impulse response of the innovation form calculates the influence of a measurement at time t on the prediction at time t+h
         # Below, we form a system del (delay) that convolves the input (y) with the impulse response
         # We then add the output again to account for the fact that we propagated error and not measurement
@@ -795,12 +798,28 @@ function observer_predictor(sys::AbstractStateSpace, K::AbstractMatrix; h::Integ
 end
 
 """
-    cont = observer_controller(sys, L::AbstractMatrix, K::AbstractMatrix)
+    cont = observer_controller(sys, L::AbstractMatrix, K::AbstractMatrix; direct=false)
+
 
+# If `direct = false`
 Return the observer_controller `cont` that is given by
 `ss(A - B*L - K*C + K*D*L, K, L, 0)`
+such that `feedback(sys, cont)` produces a closed-loop system with eigenvalues given by `A-KC` and `A-BL`.
+
+This controller does not have a direct term, and corresponds to state feedback operating on state estimated by [`observer_predictor`](@ref). Use this form if the computed control signal is applied at the next sampling instant, or with an otherwise large delay in relation to the measurement fed into the controller.
+
+Ref: "Computer-Controlled Systems" Eq 4.37
+
+# If `direct = true`
+Return the observer_controller `cont` that is given by
+`ss((I-KC)(A-BL), (I-KC)(A-BL)K, L, LK)`
+such that `feedback(sys, cont)` produces a closed-loop system with eigenvalues given by `A-BL` and `A-BL-KC`.
+This controller has a direct term, and corresponds to state feedback operating on state estimated by [`observer_filter`](@ref). Use this form if the computed control signal is applied immediately after receiveing a measurement. This version typically has better performance than the one without a direct term.
+
+!!! note
+    To use this formulation, the observer gain `K` should have been designed for the pair `(A, CA)` rather than `(A, C)`. To do this, pass `direct = true` when calling [`place`](@ref) or [`kalman`](@ref).
 
-Such that `feedback(sys, cont)` produces a closed-loop system with eigenvalues given by `A-KC` and `A-BL`.
+Ref: Ref: "Computer-Controlled Systems" pp 140 and "Computer-Controlled Systems" pp 162 prob 4.7
 
 # Arguments:
 - `sys`: Model of system
@@ -809,7 +828,48 @@ Such that `feedback(sys, cont)` produces a closed-loop system with eigenvalues g
 
 See also [`observer_predictor`](@ref) and [`innovation_form`](@ref).
 """
-function observer_controller(sys, L::AbstractMatrix, K::AbstractMatrix)
+function observer_controller(sys, L::AbstractMatrix, K::AbstractMatrix; direct=false)
     A,B,C,D = ssdata(sys)
-    ss(A - B*L - K*C + K*D*L, K, L, 0, sys.timeevol)
+    if direct && isdiscrete(sys)
+        iszero(D) || throw(ArgumentError("D must be zero when using direct formulation of `observer_controller`"))
+        IKC = (I - K*C)
+        ABL = (A - B*L)
+        ss(IKC*ABL,       IKC*ABL*K, L, L*K, sys.timeevol)
+    else
+        ss(A - B*L - K*C + K*D*L, K, L, 0,   sys.timeevol)
+    end
 end
+
+
+"""
+    observer_filter(sys, K; output_state = false)
+
+Return the observer filter 
+```math
+\\begin{aligned}
+x̂(k|k) &= (I - KC)Ax̂(k-1|k-1) + (I - KC)Bu(k-1) + Ky(k) \\\\
+\\end{aligned}
+```
+with the input equation `[(I - KC)B K] * [u(k-1); y(k)]`.
+
+Note the time indices in the equations, the filter assumes that the user passes the *current* ``y(k)``, but the *past* ``u(k-1)``, that is, this filter is used to estimate the state *before* the current control input has been applied. This causes a state-feedback controller acting on the estimate produced by this observer to have a direct term.
+
+This is similar to [`observer_predictor`](@ref), but in contrast to the predictor, the filter output depends on the current measurement, whereas the predictor output only depend on past measurements.
+
+The observer filter is equivalent to the [`observer_predictor`](@ref) for continuous-time systems.
+
+!!! note
+    To use this formulation, the observer gain `K` should have been designed for the pair `(A, CA)` rather than `(A, C)`. To do this, pass `direct = true` when calling [`place`](@ref) or [`kalman`](@ref).
+
+Ref: "Computer-Controlled Systems" Eq 4.32
+"""
+function observer_filter(sys::AbstractStateSpace{<:Discrete}, K::AbstractMatrix; output_state = false)
+    A,B,C,D = ssdata(sys)
+    iszero(D) || throw(ArgumentError("D must be zero in `observer_filter`, consider using `observer_predictor` if you have a non-zero `D`."))
+    IKC = (I-K*C)
+    ss(IKC*A, [IKC*B K], output_state ? I : C, 0, sys.timeevol)
+end
+
+function observer_filter(sys::AbstractStateSpace{Continuous}, K::AbstractMatrix; kwargs...)
+    observer_predictor(sys, K; kwargs...)
+end

lib/ControlSystemsBase/src/synthesis.jl

@@ -73,29 +73,37 @@ end
 
 """
     kalman(Continuous, A, C, R1, R2)
-    kalman(Discrete, A, C, R1, R2)
-    kalman(sys, R1, R2)
+    kalman(Discrete, A, C, R1, R2; direct = false)
+    kalman(sys, R1, R2; direct = false)
 
-Calculate the optimal Kalman gain
+Calculate the optimal Kalman gain.
+
+If `direct = true`, the observer gain is computed for the pair `(A, CA)` instead of `(A,C)`. This option is intended to be used together with the option `direct = true` to [`observer_controller`](@ref). Ref: "Computer-Controlled Systems" pp 140.
 
 The `args...; kwargs...` are sent to the Riccati solver, allowing specification of cross-covariance etc. See `?MatrixEquations.arec/ared` for more help.
 """
-kalman(te, A, C, R1,R2, args...; kwargs...) = Matrix(lqr(te, A',C',R1,R2, args...; kwargs...)')
+function kalman(te, A, C, R1,R2, args...; direct = false, kwargs...)
+    if direct
+        te isa ContinuousType && error("direct = true only applies to discrete-time systems")
+        C = C*A
+    end
+    Matrix(lqr(te, A',C',R1,R2, args...; kwargs...)')
+end
 
 function lqr(sys::AbstractStateSpace, Q, R, args...; kwargs...)
     return lqr(sys.timeevol, sys.A, sys.B, Q, R, args...; kwargs...)
 end
 
 function kalman(sys::AbstractStateSpace, R1, R2, args...; kwargs...)
-    return Matrix(lqr(sys.timeevol, sys.A', sys.C', R1,R2, args...; kwargs...)')
+    return kalman(sys.timeevol, sys.A, sys.C, R1,R2, args...; kwargs...)
 end
 
 @deprecate kalman(A::AbstractMatrix, args...; kwargs...)  kalman(Continuous, A, args...; kwargs...)
 @deprecate dkalman(args...; kwargs...)  kalman(Discrete, args...; kwargs...)
 
 """
-    place(A, B, p, opt=:c)
-    place(sys::StateSpace, p, opt=:c)
+    place(A, B, p, opt=:c; direct = false)
+    place(sys::StateSpace, p, opt=:c; direct = false)
 
 Calculate the gain matrix `K` such that `A - BK` has eigenvalues `p`.
 
@@ -104,14 +112,21 @@ Calculate the gain matrix `K` such that `A - BK` has eigenvalues `p`.
 
 Calculate the observer gain matrix `L` such that `A - LC` has eigenvalues `p`.
 
+If `direct = true` and `opt = :o`, the the observer gain `K` is calculated such that `A - KCA` has eigenvalues `p`, this option is to be used together with `direct = true` in [`observer_controller`](@ref). 
+
+Note: only apply `direct = true` to discrete-time systems.
+
+Ref: "Computer-Controlled Systems" pp 140.
+
 Uses Ackermann's formula for SISO systems and [`place_knvd`](@ref) for MIMO systems. 
 
 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; kwargs...)
+function place(A, B, p, opt=:c; direct = false, kwargs...)
     n = length(p)
     n != size(A,1) && error("Must specify as many poles as states")
     if opt === :c
+        direct && error("direct = true only applies to observer design")
         n != size(B,1) && error("A and B must have same number of rows")
         if size(B,2) == 1
             acker(A, B, p)
@@ -120,6 +135,9 @@ function place(A, B, p, opt=:c; kwargs...)
         end
     elseif opt === :o
         C = B # B is really the "C matrix"
+        if direct
+            C = C*A
+        end
         n != size(C,2) && error("A and C must have same number of columns")
         if size(C,1) == 1
             acker(A', C', p)'
@@ -130,11 +148,12 @@ function place(A, B, p, opt=:c; kwargs...)
         error("fourth argument must be :c or :o")
     end
 end
-function place(sys::AbstractStateSpace, p, opt=:c)
+function place(sys::AbstractStateSpace, p, opt=:c; direct = false, kwargs...)
     if opt === :c
-        return place(sys.A, sys.B, p, opt)
+        return place(sys.A, sys.B, p, opt; kwargs...)
     elseif opt === :o
-        return place(sys.A, sys.C, p, opt)
+        iscontinuous(sys) && direct && error("direct = true only applies to discrete-time systems")
+        return place(sys.A, sys.C, p, opt; direct, kwargs...)
     else
         error("third argument must be :c or :o")
     end

lib/ControlSystemsBase/test/test_matrix_comps.jl

@@ -118,6 +118,8 @@ K = kalman(sys, I(2), I(1))
 @test sysp.A == sys.A-K*sys.C
 @test sysp.B == [sys.B-K*sys.D K]
 
+@test sysp == observer_filter(sys, K) # Equivalent for continuous-time systems
+
 # test longer prediction horizons
 
 # With K=0, y should have no influence
@@ -191,6 +193,7 @@ R2 = I(2)
 L = lqr(sys, Q1, Q2)
 K = kalman(sys, R1, R2)
 cont = observer_controller(sys, L, K)
+@test iszero(cont.D)
 syscl = feedback(sys, cont)
 
 pcl = poles(syscl)
@@ -215,6 +218,71 @@ end
 
 
 
+# Test observer_controller discrete with LQG
+Ts = 0.01
+sys = ssrand(2,3,4; Ts, proper=true)
+Q1 = I(4)
+Q2 = I(3)
+R1 = I(4)
+R2 = I(2)
+@test are(sys, Q1, Q2) == are(Discrete, sys.A, sys.B, Q1, Q2)
+@test lyap(sys, Q1) == lyap(Discrete, sys.A, Q1)
+L = lqr(sys, Q1, Q2)
+K = kalman(sys, R1, R2; direct = true)
+cont = observer_controller(sys, L, K, direct=true)
+@test !iszero(cont.D)
+syscl = feedback(sys, cont)
+@test isstable(syscl)
+
+# Test observer_controller discrete with pole placement
+Ts = 0.01
+sys = ssrand(2,3,4; Ts, proper=true)
+p = exp.(Ts .* [-10, -20, -30, -40])
+p2 = exp.(2*Ts .* [-10, -20, -30, -40])
+L = place(sys, p, :c)
+K = place(sys, p2, :o)
+cont = observer_controller(sys, L, K)
+@test iszero(cont.D)
+syscl = feedback(sys, cont)
+
+pcl = poles(syscl)
+A,B,C,D = ssdata(sys)
+allpoles = [
+    eigvals(A-B*L)
+    eigvals(A-K*C)
+]
+@test sort(pcl, by=real) ≈ (sort(allpoles, by=real)) rtol=1e-3
+@test cont.B == K
+
+
+Kd = place(sys, p2, :o; direct = true) 
+@test Kd == place(sys.A', (sys.C*sys.A)', p2)'
+cont_direct = observer_controller(sys, L, Kd, direct=true)
+@test !iszero(cont_direct.D)
+
+syscld = feedback(sys, cont_direct)
+@test isstable(syscld)
+
+pcl = poles(syscld)
+A,B,C,D = ssdata(sys)
+allpoles = [
+    p; p2
+]
+@test sort(pcl, by=real) ≈ sort(allpoles, by=real) rtol=1e-3
+
+## Test time scaling
+for balanced in [true, false]
+    sys = ssrand(1,1,5);
+    t = 0:0.1:50
+    a = 10
+
+    Gs = tf(1, [1e-6, 1]) # micro-second time scale modeled in seconds
+    Gms = time_scale(Gs, 1e-6; balanced) # Change to micro-second time scale
+    @test Gms == tf(1, [1, 1])
+end
+
+
+
 ## Test balancing with Duals
 using ForwardDiff: Dual
 A = Dual.([-0.21 0.2; 0.2 -0.21])
@@ -227,4 +295,5 @@ sysb,T = ControlSystemsBase.balance_statespace(sys)
 @test T != I
 @test similarity_transform(sysb, T) ≈ sys
 
-end 
+end 
+