added : Luenberger observer with pole placement

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.5.2"
+version = "0.5.3"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

README.md

@@ -108,7 +108,7 @@ more detailed examples.
 - ⬜ supported state estimators/observers:
   - ✅ steady-state Kalman filter
   - ✅ Kalman filter
-  - ⬜ Luenberger observer
+  - ✅ Luenberger observer
   - ✅ internal model structure
   - ⬜ extended Kalman filter
   - ✅ unscented Kalman filter

docs/src/index.md

@@ -64,7 +64,7 @@ Pages = [
 - ⬜ supported state estimators/observers:
   - ✅ steady-state Kalman filter
   - ✅ Kalman filter
-  - ⬜ Luenberger observer
+  - ✅ Luenberger observer
   - ✅ internal model structure
   - ⬜ extended Kalman filter
   - ✅ unscented Kalman filter

docs/src/public/state_estim.md

@@ -50,6 +50,12 @@ SteadyKalmanFilter
 KalmanFilter
 ```
 
+## Luenberger
+
+```@docs
+Luenberger
+```
+
 ## UnscentedKalmanFilter
 
 ```@docs

example/juMPC.jl

@@ -51,6 +51,12 @@ internalModel3 = InternalModel(linModel1,i_ym=[2])
 
 initstate!(internalModel1,[0,0],[1,1])
 
+luenberger = Luenberger(linModel1)
+
+updatestate!(luenberger, [0,0], [0,0])
+
+mpcluen = LinMPC(luenberger)
+
 kalmanFilter1 = KalmanFilter(linModel1)
 kalmanFilter2 = KalmanFilter(linModel1,nint_ym=0)
 

src/ModelPredictiveControl.jl

@@ -12,7 +12,7 @@ import OSQP, Ipopt
 
 export SimModel, LinModel, NonLinModel, setop!, setstate!, updatestate!, evaloutput
 export StateEstimator, InternalModel
-export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter
+export SteadyKalmanFilter, KalmanFilter, Luenberger, UnscentedKalmanFilter
 export initstate!
 export PredictiveController, LinMPC, NonLinMPC, setconstraint!, moveinput!, getinfo, sim!
 

src/estimator/luenberger.jl

@@ -18,16 +18,19 @@ struct Luenberger <: StateEstimator
     Ĉm  ::Matrix{Float64}
     D̂dm ::Matrix{Float64}
     K::Matrix{Float64}
-    function Luenberger(model, i_ym, nint_ym, Asm, Csm, L)
+    function Luenberger(model, i_ym, nint_ym, Asm, Csm, p)
         nu, nx, ny = model.nu, model.nx, model.ny
         nym, nyu = length(i_ym), ny - length(i_ym)
         nxs = size(Asm,1)
         nx̂ = nx + nxs
-        validate_kfcov(nym, nx̂, Q̂, R̂)
         As, _ , Cs, _  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        f̂, ĥ, Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
+        Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
         Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
-        K = L
+        K = try
+            place(Â, Ĉ, p, :o)
+        catch
+            error("Cannot compute the Luenberger gain L with specified poles p.")        
+        end
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
         x̂ = [copy(model.x); zeros(nxs)]
@@ -38,8 +41,69 @@ struct Luenberger <: StateEstimator
             As, Cs, nint_ym,
             Â, B̂u, B̂d, Ĉ, D̂d, 
             Ĉm, D̂dm,
-            Q̂, R̂,
             K
         )
     end
+end
+
+@doc raw"""
+    Luenberger(
+        model::LinModel; 
+        i_ym = 1:model.ny, 
+        nint_ym = fill(1, length(i_ym)),
+        p̂ = 1e-3*(0:(model.nx + sum(nint_ym)-1)) .+ 0.5)
+    )
+
+Construct a Luenberger observer with the [`LinModel`](@ref) `model`.
+
+`i_ym` provides the `model` output indices that are measured ``\mathbf{y^m}``, the rest are 
+unmeasured ``\mathbf{y^u}``. `model` matrices are augmented with the stochastic model, which
+is specified by the numbers of output integrator `nint_ym` (see [`SteadyKalmanFilter`](@ref)
+Extended Help). The argument `p̂` is a vector of `model.nx + sum(nint_ym)` elements 
+specifying the observer poles/eigenvalues (near ``z=0.5`` by default). The method computes 
+the observer gain ``\mathbf{K}`` with [`place`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.place).
+
+# Examples
+```jldoctest
+julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5);
+
+julia> lo = Luenberger(model, nint_ym=[1, 1], p̂=[0.61, 0.62, 0.63, 0.64])
+Luenberger estimator with a sample time Ts = 0.5 s, LinModel and:
+ 1 manipulated inputs u
+ 4 states x̂
+ 2 measured outputs ym
+ 0 unmeasured outputs yu
+ 0 measured disturbances d
+```
+"""
+function Luenberger(
+    model::LinModel;
+    i_ym::IntRangeOrVector  = 1:model.ny,
+    nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
+    p̂ = 1e-3*(0:(model.nx + sum(nint_ym)-1)) .+ 0.5
+)
+    if nint_ym == 0 # alias for no output integrator at all :
+        nint_ym = fill(0, length(i_ym));
+    end
+    Asm, Csm = init_estimstoch(i_ym, nint_ym)
+    nx = model.nx
+    if length(p̂) ≠ model.nx + sum(nint_ym)
+        error("p̂ length ($(length(p̂))) ≠ nx ($nx) + integrator quantity ($(sum(nint_ym)))")
+    end
+    any(abs.(p̂) .≥ 1) && error("Observer poles p̂ should be inside the unit circles.")
+    return Luenberger(model, i_ym, nint_ym, Asm, Csm, p̂)
+end
+
+
+"""
+    updatestate!(estim::Luenberger, u, ym, d=Float64[])
+
+Same `SteadyKalmanFilter` but using the Luenberger observer.
+"""
+function updatestate!(estim::Luenberger, u, ym, d=Float64[])
+    u, d, ym = remove_op!(estim, u, d, ym)
+    Â, B̂u, B̂d, Ĉm, D̂dm = estim.Â, estim.B̂u, estim.B̂d, estim.Ĉm, estim.D̂dm
+    x̂, K = estim.x̂, estim.K
+    x̂[:] = Â*x̂ + B̂u*u + B̂d*d + K*(ym - Ĉm*x̂ - D̂dm*d)
+    return x̂    
 end

src/state_estim.jl

@@ -265,6 +265,7 @@ end
 (estim::StateEstimator)(d=Float64[]) = evaloutput(estim, d)
 
 include("estimator/kalman.jl")
+include("estimator/luenberger.jl")
 include("estimator/internal_model.jl")
 
 "Get [`InternalModel`](@ref) output `ŷ` from current measured outputs `ym` and dist. `d`."

test/test_state_estim.jl

@@ -97,6 +97,48 @@ end
     @test kalmanfilter1.x̂ ≈ [1,2,3,4]
 end   
 
+@testset "Luenberger construction" begin
+    linmodel1 = LinModel(sys,Ts,i_u=[1,2])
+    lo1 = Luenberger(linmodel1)
+    @test lo1.nym == 2
+    @test lo1.nyu == 0
+    @test lo1.nxs == 2
+    @test lo1.nx̂ == 4
+
+    linmodel2 = LinModel(sys,Ts,i_d=[3])
+    lo2 = Luenberger(linmodel2, i_ym=[2])
+    @test lo2.nym == 1
+    @test lo2.nyu == 1
+    @test lo2.nxs == 1
+    @test lo2.nx̂ == 5
+
+    lo3 = Luenberger(linmodel1, nint_ym=0)
+    @test lo3.nxs == 0
+    @test lo3.nx̂ == 2
+
+    lo4 = Luenberger(linmodel1, nint_ym=[2,2])
+    @test lo4.nxs == 4
+    @test lo4.nx̂ == 6
+
+    @test_throws ErrorException Luenberger(linmodel1, nint_ym=[1,1,1])
+    @test_throws ErrorException Luenberger(linmodel1, nint_ym=[-1,0])
+    @test_throws ErrorException Luenberger(linmodel1, p̂=[0.5])
+    @test_throws ErrorException Luenberger(linmodel1, p̂=fill(1.5, lo1.nx̂))
+end    
+    
+@testset "Luenberger estimator methods" begin
+    linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
+    lo1 = Luenberger(linmodel1)
+    @test updatestate!(lo1, [10, 50], [50, 30]) ≈ zeros(4)
+    @test updatestate!(lo1, [10, 50], [50, 30], Float64[]) ≈ zeros(4)
+    @test lo1.x̂ ≈ zeros(4)
+    @test evaloutput(lo1) ≈ lo1() ≈ [50, 30]
+    @test evaloutput(lo1, Float64[]) ≈ lo1(Float64[]) ≈ [50, 30]
+    @test initstate!(lo1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]]
+    setstate!(lo1, [1,2,3,4])
+    @test lo1.x̂ ≈ [1,2,3,4]
+end
+
 @testset "InternalModel construction" begin
     linmodel1 = LinModel(sys,Ts,i_u=[1,2])
     internalmodel1 = InternalModel(linmodel1)