improve coverage for StateEstimator types

Author: franckgaga

src/estimator/kalman.jl

@@ -161,6 +161,7 @@ Construct the estimator from the augmented covariance matrices `Q̂` and `R̂`.
 This syntax allows nonzero off-diagonal elements in ``\mathbf{Q̂, R̂}``.
 """
 function SteadyKalmanFilter(model::SM, i_ym, nint_u, nint_ym, Q̂, R̂) where {NT<:Real, SM<:LinModel{NT}}
+    Q̂, R̂ = to_mat(Q̂), to_mat(R̂)
     return SteadyKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, Q̂ , R̂)
 end
 
@@ -302,6 +303,7 @@ Construct the estimator from the augmented covariance matrices `P̂0`, `Q̂` and
 This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``.
 """
 function KalmanFilter(model::SM, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂) where {NT<:Real, SM<:LinModel{NT}}
+    P̂0, Q̂, R̂ = to_mat(P̂0), to_mat(Q̂), to_mat(R̂)
     return KalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂)
 end
 
@@ -473,6 +475,7 @@ This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mat
 function UnscentedKalmanFilter(
     model::SM, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, α, β, κ
 ) where {NT<:Real, SM<:SimModel{NT}}
+    P̂0, Q̂, R̂ = to_mat(P̂0), to_mat(Q̂), to_mat(R̂)
     return UnscentedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂, α, β, κ)
 end
 
@@ -694,6 +697,7 @@ Construct the estimator from the augmented covariance matrices `P̂0`, `Q̂` and
 This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``.
 """
 function ExtendedKalmanFilter(model::SM, i_ym, nint_u, nint_ym,P̂0, Q̂, R̂) where {NT<:Real, SM<:SimModel{NT}}
+    P̂0, Q̂, R̂ = to_mat(P̂0), to_mat(Q̂), to_mat(R̂)
     return ExtendedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂)
 end
 

src/estimator/mhe.jl

@@ -109,11 +109,11 @@ nonlinear objective function at each discrete time ``k``:
                                          + \mathbf{Ŵ}' \mathbf{Q̂}_{N_k}^{-1} \mathbf{Ŵ}  
                                          + \mathbf{V̂}' \mathbf{R̂}_{N_k}^{-1} \mathbf{V̂}
 ```
-in which the arrival costs are defined by:
+in which the arrival costs are evaluated from the states estimated at time ``k-N_k``:
 ```math
 \begin{aligned}
     \mathbf{x̄} &= \mathbf{x̂}_k(k-N_k+1) - \mathbf{x̂}_{k-N_k}(k-N_k+1) \\
-    \mathbf{P̄} &= \mathbf{P̂}_k(k-N_k+1)
+    \mathbf{P̄} &= \mathbf{P̂}_{k-N_k}(k-N_k+1)
 \end{aligned}
 ```
 and the covariances are repeated ``N_k`` times:
@@ -133,7 +133,12 @@ N_k =                     \begin{cases}
 ```
 See [`SteadyKalmanFilter`](@ref) for details on ``\mathbf{R̂}, \mathbf{Q̂}`` covariances and
 model augmentation. The process model is identical to the one in [`UnscentedKalmanFilter`](@ref)
-documentation.
+documentation. Note that the estimation error covariance ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is
+approximated with an [`ExtendedKalmanFilter`](@ref).
+
+!!! warning
+    See the Extended Help of [`NonLinMPC`](@ref) function if you get an error like:    
+    `MethodError: no method matching (::var"##")(::Vector{ForwardDiff.Dual})`.
 
 # Arguments
 - `model::SimModel` : (deterministic) model for the estimations.
@@ -193,6 +198,7 @@ This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mat
 function MovingHorizonEstimator(
     model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim::JM
 ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel}
+    P̂0, Q̂, R̂ = to_mat(P̂0), to_mat(Q̂), to_mat(R̂)
     return MovingHorizonEstimator{NT, SM, JM}(
         model, He, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂, optim
     )

src/model/linmodel.jl

@@ -14,6 +14,7 @@ struct LinModel{NT<:Real} <: SimModel{NT}
     yop::Vector{NT}
     dop::Vector{NT}
     function LinModel{NT}(A, Bu, C, Bd, Dd, Ts) where {NT<:Real}
+        A, Bu, C, Bd, Dd = to_mat(A), to_mat(Bu), to_mat(C), to_mat(Bd), to_mat(Dd)
         nu, nx, ny, nd = size(Bu,2), size(A,2), size(C,1), size(Bd,2)
         size(A)  == (nx,nx) || error("A size must be $((nx,nx))")
         size(Bu) == (nx,nu) || error("Bu size must be $((nx,nu))")
@@ -192,24 +193,24 @@ The optional parameter `NT` explicitly specifies the number type of the matrices
 LinModel{NT}(A, Bu, C, Bd, Dd, Ts) where NT<:Real
 
 function LinModel(
-    A::Matrix{NT}, Bu::Matrix{NT}, C::Matrix{NT}, Bd::Matrix{NT}, Dd::Matrix{NT}, Ts::NT
+    A::Array{NT}, Bu::Array{NT}, C::Array{NT}, Bd::Array{NT}, Dd::Array{NT}, Ts::Real
 ) where {NT<:Real} 
     return LinModel{NT}(A, Bu, C, Bd, Dd, Ts)
 end
 
 function LinModel(
-    A::Matrix{<:Real}, 
-    Bu::Matrix{<:Real}, 
-    C::Matrix{<:Real}, 
-    Bd::Matrix{<:Real}, 
-    Dd::Matrix{<:Real},
+    A::Array{<:Real}, 
+    Bu::Array{<:Real}, 
+    C::Array{<:Real}, 
+    Bd::Array{<:Real}, 
+    Dd::Array{<:Real},
     Ts::Real
 )
-    A, Bu, C, Bd, Dd, Ts_arr = promote(A, Bu, C, Bd, Dd, Ts*ones(1,1))
-    return LinModel(A, Bu, C, Bd, Dd, Ts_arr[])
+    A, Bu, C, Bd, Dd = to_mat(A), to_mat(Bu), to_mat(C), to_mat(Bd), to_mat(Dd)
+    A, Bu, C, Bd, Dd = promote(A, Bu, C, Bd, Dd)
+    return LinModel(A, Bu, C, Bd, Dd, Ts)
 end
 
-
 @doc raw"""
     steadystate!(model::LinModel, u, d)
 

src/model/nonlinmodel.jl

@@ -47,7 +47,7 @@ manually call a differential equation solver in `f` (see [Manual](@ref man_nonli
 
 !!! warning
     `f` and `h` must be pure Julia functions to use the model in [`NonLinMPC`](@ref),
-    [`ExtendedKalmanFilter`](@ref) and [`linearize`](@ref).
+    [`ExtendedKalmanFilter`](@ref), [`MovingHorizonEstimator`](@ref) and [`linearize`](@ref).
 
 See also [`LinModel`](@ref).
 

src/sim_model.jl

@@ -177,6 +177,10 @@ function validate_args(model::SimModel, u, d)
     size(d) ≠ (nd,) && throw(DimensionMismatch("d size $(size(d)) ≠ meas. dist. size ($nd,)"))
 end
 
+"Convert vectors to single column matrices when necessary."
+to_mat(A::AbstractVector) = reshape(A, length(A), 1)
+to_mat(A::AbstractMatrix) = A
+
 "Functor allowing callable `SimModel` object as an alias for `evaloutput`."
 (model::SimModel)(d=empty(model.x)) = evaloutput(model::SimModel, d)
 

test/test_sim_model.jl

@@ -66,8 +66,11 @@ Gss2 = c2d(sys_ss[:,1:2], 0.5Ts, :zoh)
     @test linmodel7.A ≈ diagm( .1: .1: .3)
     @test linmodel7.C ≈ diagm( .4: .1: .6)
 
-    linmodel8 = LinModel{Float32}(Gss.A, Gss.B, Gss.C, zeros(2, 0), zeros(2, 0), Ts)
-    @test isa(linmodel8, LinModel{Float32})
+    linmodel8 = LinModel(Gss.A, Gss.B, Gss.C, zeros(Float32, 2, 0), zeros(Float32, 2, 0), Ts)
+    @test isa(linmodel8, LinModel{Float64})
+
+    linmodel9 = LinModel{Float32}(Gss.A, Gss.B, Gss.C, zeros(2, 0), zeros(2, 0), Ts)
+    @test isa(linmodel9, LinModel{Float32})
 
     @test_throws ErrorException LinModel(sys)
     @test_throws ErrorException LinModel(sys,-Ts)

test/test_state_estim.jl

@@ -57,8 +57,9 @@ sys = [ tf(1.90,[18.0,1])   tf(1.90,[18.0,1])   tf(1.90,[18.0,1]);
     @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.0)
+    model_unobs = LinModel([1 0;0 1.5], [1; 0], [1 0], zeros(2,0), zeros(1,0), 1.0)
     @test_throws ErrorException SteadyKalmanFilter(model_unobs, nint_ym=[1])
+    @test_throws ErrorException SteadyKalmanFilter(LinModel(tf(1,[10, 1]), 1.0), 1:1, 0, 0, [-1], [1])
     @test_throws ErrorException SteadyKalmanFilter(LinModel(tf(1, [1,0]), 1), nint_ym=[1])
     @test_throws ErrorException SteadyKalmanFilter(linmodel1, nint_u=[1,1], nint_ym=[1,1])
 end
@@ -229,6 +230,7 @@ end
     @test_throws ErrorException Luenberger(linmodel1, p̂=[0.5])
     @test_throws ErrorException Luenberger(linmodel1, p̂=fill(1.5, lo1.nx̂))
     @test_throws ErrorException Luenberger(LinModel(tf(1,[1, 0]),0.1), p̂=[0.5,0.6])
+    @test_throws ErrorException Luenberger(linmodel1, p̂=[0,0,0,0])
 end
 
 @testset "Luenberger estimator methods" begin