removed validate_obsv function (move content in augment_model)

franckgaga · franckgaga · commit 0eafd3dc59cd · 2023-09-01T11:50:05.000-04:00
diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl
@@ -28,7 +28,6 @@ struct SteadyKalmanFilter <: StateEstimator
         nx̂ = nx + nxs
         validate_kfcov(nym, nx̂, Q̂, R̂)
         As, _ , Cs  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        validate_obsv(model, As, Cs)
         Â , B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
         K = try
             Q̂_kalman = Matrix(Q̂) # Matrix() required for Julia 1.6
@@ -207,7 +206,6 @@ struct KalmanFilter <: StateEstimator
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
         As, _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
         Â , B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
-        validate_obsv(model, As, Cs)
         Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
@@ -344,7 +342,7 @@ struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
         nx̂ = nx + nxs
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
         As, _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        validate_obsv(model, As, Cs)
+        augment_model(model, As, Cs) # verify observability for LinModel
         nσ, γ, m̂, Ŝ = init_ukf(nx̂, α, β, κ)
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
@@ -564,7 +562,7 @@ struct ExtendedKalmanFilter{M<:SimModel} <: StateEstimator
         nx̂ = nx + nxs
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
         As, _ , Cs, _  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        validate_obsv(model, As, Cs)
+        augment_model(model, As, Cs) # verify observability for LinModel
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
         x̂ = [zeros(model.nx); zeros(nxs)]
diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl
@@ -26,7 +26,6 @@ struct Luenberger <: StateEstimator
         nx̂ = nx + nxs
         As, _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
         Â , B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
-        validate_obsv(model, As, Cs)
         K = try
             place(Â, Ĉ, p̂, :o)[:, i_ym]
         catch
diff --git a/src/state_estim.jl b/src/state_estim.jl
@@ -143,16 +143,23 @@ returns the augmented matrices `Â`, `B̂u`, `Ĉ`, `B̂d` and `D̂d`:
 \end{aligned}
 ```
 """
-function augment_model(model::LinModel, As, Cs)
+function augment_model(model::LinModel, As, Cs; verify_obsv=true)
     nu, nx, nd = model.nu, model.nx, model.nd
     nxs = size(As, 1)
     Â   = [model.A zeros(nx,nxs); zeros(nxs,nx) As]
     B̂u  = [model.Bu; zeros(nxs,nu)]
     Ĉ   = [model.C Cs]
     B̂d  = [model.Bd; zeros(nxs,nd)]
     D̂d  = model.Dd
+    # observability on Ĉ instead of Ĉm, since it would always return false when nym ≠ ny:
+    if verify_obsv && !isobservable(Â, Ĉ)
+        error("The augmented model is unobservable. You may try to use 0 "*
+              "integrator on model integrating outputs with nint_ym parameter.")
+    end
     return Â, B̂u, Ĉ, B̂d, D̂d
 end
+"No need to augment the model if `model` is not a [`LinModel`](@ref)."
+augment_model(::SimModel, _ , _ ) = nothing
 
 @doc raw"""
     default_nint(model::LinModel, i_ym)
@@ -169,7 +176,7 @@ function default_nint(model::LinModel, i_ym)
         nint_ym[i]  = 1
         Asm, Csm    = init_estimstoch(i_ym, nint_ym)
         As , _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        Â  , _ , Ĉ  = augment_model(model, As, Cs)
+        Â  , _ , Ĉ  = augment_model(model, As, Cs, verify_obsv=false)
         # observability on Ĉ instead of Ĉm, since it would always return false when nym ≠ ny
         isobservable(Â, Ĉ) || (nint_ym[i] = 0)
     end
@@ -178,18 +185,6 @@ end
 "One integrator per measured outputs by default if `model` is not a  [`LinModel`](@ref)."
 default_nint(::SimModel, i_ym) = fill(1, length(i_ym))
 
-"Validate if the augmented model is observable before augmenting it."
-function validate_obsv(model::LinModel, As, Cs)
-    Â , _ , Ĉ  = augment_model(model, As, Cs)
-    # observability on Ĉ instead of Ĉm, since it would always return false when nym ≠ ny:
-    if !isobservable(Â, Ĉ)
-        error("The augmented model is unobservable. You may try to use 0 "*
-              "integrator on model integrating outputs with nint_ym parameter.")
-    end
-end
-"If model is not a [`LinModel`](@ref), the observability is not verified."
-validate_obsv(::SimModel, _ , _ ) = nothing
-
 """
     isobservable(A, C)
 
diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl
@@ -9,17 +9,20 @@ sys = [ tf(1.90,[18.0,1])   tf(1.90,[18.0,1])   tf(1.90,[18.0,1]);
     @test skalmanfilter1.nyu == 0
     @test skalmanfilter1.nxs == 2
     @test skalmanfilter1.nx̂ == 4
+    @test skalmanfilter1.nint_ym == [1, 1]
 
     linmodel2 = LinModel(sys,Ts,i_d=[3])
     skalmanfilter2 = SteadyKalmanFilter(linmodel2, i_ym=[2])
     @test skalmanfilter2.nym == 1
     @test skalmanfilter2.nyu == 1
     @test skalmanfilter2.nxs == 1
     @test skalmanfilter2.nx̂ == 5
+    @test skalmanfilter2.nint_ym == [1]
 
     skalmanfilter3 = SteadyKalmanFilter(linmodel1, nint_ym=0)
     @test skalmanfilter3.nxs == 0
     @test skalmanfilter3.nx̂ == 2
+    @test skalmanfilter3.nint_ym == [0, 0]
 
     skalmanfilter4 = SteadyKalmanFilter(linmodel1, nint_ym=[2,2])
     @test skalmanfilter4.nxs == 4
@@ -29,14 +32,21 @@ sys = [ tf(1.90,[18.0,1])   tf(1.90,[18.0,1])   tf(1.90,[18.0,1]);
     @test skalmanfilter5.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36]))
     @test skalmanfilter5.R̂ ≈ Hermitian(diagm(Float64[49, 64]))
 
+    linmodel3 = LinModel(append(tf(1,[1, 0]),tf(1,[10, 1]),tf(1,[-1, 1])), 0.1)
+    skalmanfilter6 = SteadyKalmanFilter(linmodel3)
+    @test skalmanfilter6.nxs == 2
+    @test skalmanfilter6.nx̂ == 5
+    @test skalmanfilter6.nint_ym == [0, 1, 1]
+
     @test_throws ErrorException SteadyKalmanFilter(linmodel1, nint_ym=[1,1,1])
     @test_throws ErrorException SteadyKalmanFilter(linmodel1, nint_ym=[-1,0])
     @test_throws ErrorException SteadyKalmanFilter(linmodel1, nint_ym=0, σQ=[1])
     @test_throws ErrorException SteadyKalmanFilter(linmodel1, nint_ym=0, σR=[1,1,1])
+    @test_throws ErrorException SteadyKalmanFilter(linmodel3, nint_ym=[1, 0, 0])
     model_unobs = LinModel([1 0;0 1.5], [1;0][:,:], [1 0], zeros(2,0), zeros(1,0),1,1,2,1,0)
     @test_throws ErrorException SteadyKalmanFilter(model_unobs, nint_ym=[1])
 end
-    
+
 @testset "SteadyKalmanFilter estimator methods" begin
     linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
     skalmanfilter1 = SteadyKalmanFilter(linmodel1)
