Merge pull request #216 from JuliaControl/cov_sparsity

added: new `KalmanCovariance` struct to handle covariance sparsity efficiently

Author: franckgaga

src/controller/construct.jl

@@ -91,7 +91,29 @@ function ControllerWeights(
         model::SimModel{NT}, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0
     ) where {NT<:Real}
     validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
-    return ControllerWeights{NT}(NT.(M_Hp), NT.(N_Hc), NT.(L_Hp), Cwt, Ewt)
+    M_Hp, N_Hc, L_Hp = NT.(M_Hp), NT.(N_Hc), NT.(L_Hp)
+    return ControllerWeights{NT}(M_Hp, N_Hc, L_Hp, Cwt, Ewt)
+end
+
+"Validate predictive controller weight and horizon specified values."
+function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C=Inf, E=nothing)
+    nu, ny = model.nu, model.ny
+    nM, nN, nL = ny*Hp, nu*Hc, nu*Hp
+    Hp < 1  && throw(ArgumentError("Prediction horizon Hp should be ≥ 1"))
+    Hc < 1  && throw(ArgumentError("Control horizon Hc should be ≥ 1"))
+    Hc > Hp && throw(ArgumentError("Control horizon Hc should be ≤ prediction horizon Hp"))
+    size(M_Hp) ≠ (nM,nM) && throw(ArgumentError("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)"))
+    size(N_Hc) ≠ (nN,nN) && throw(ArgumentError("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)"))
+    size(L_Hp) ≠ (nL,nL) && throw(ArgumentError("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)"))
+    (isdiag(M_Hp) && any(diag(M_Hp) .< 0)) && throw(ArgumentError("Mwt values should be nonnegative"))
+    (isdiag(N_Hc) && any(diag(N_Hc) .< 0)) && throw(ArgumentError("Nwt values should be nonnegative"))
+    (isdiag(L_Hp) && any(diag(L_Hp) .< 0)) && throw(ArgumentError("Lwt values should be nonnegative"))
+    !ishermitian(M_Hp) && throw(ArgumentError("M_Hp should be hermitian"))
+    !ishermitian(N_Hc) && throw(ArgumentError("N_Hc should be hermitian"))
+    !ishermitian(L_Hp) && throw(ArgumentError("L_Hp should be hermitian"))
+    size(C) ≠ ()    && throw(ArgumentError("Cwt should be a real scalar"))
+    C < 0           && throw(ArgumentError("Cwt weight should be ≥ 0"))
+    !isnothing(E) && size(E) ≠ () && throw(ArgumentError("Ewt should be a real scalar"))
 end
 
 "Include all the data for the constraints of [`PredictiveController`](@ref)"
@@ -867,25 +889,4 @@ end
 "Return empty matrices if `estim` is not a [`InternalModel`](@ref)."
 function init_stochpred(estim::StateEstimator{NT}, _ ) where NT<:Real
     return zeros(NT, 0, estim.nxs), zeros(NT, 0, estim.model.ny)
-end
-
-"Validate predictive controller weight and horizon specified values."
-function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C=Inf, E=nothing)
-    nu, ny = model.nu, model.ny
-    nM, nN, nL = ny*Hp, nu*Hc, nu*Hp
-    Hp < 1  && throw(ArgumentError("Prediction horizon Hp should be ≥ 1"))
-    Hc < 1  && throw(ArgumentError("Control horizon Hc should be ≥ 1"))
-    Hc > Hp && throw(ArgumentError("Control horizon Hc should be ≤ prediction horizon Hp"))
-    size(M_Hp) ≠ (nM,nM) && throw(ArgumentError("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)"))
-    size(N_Hc) ≠ (nN,nN) && throw(ArgumentError("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)"))
-    size(L_Hp) ≠ (nL,nL) && throw(ArgumentError("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)"))
-    (isdiag(M_Hp) && any(diag(M_Hp) .< 0)) && throw(ArgumentError("Mwt values should be nonnegative"))
-    (isdiag(N_Hc) && any(diag(N_Hc) .< 0)) && throw(ArgumentError("Nwt values should be nonnegative"))
-    (isdiag(L_Hp) && any(diag(L_Hp) .< 0)) && throw(ArgumentError("Lwt values should be nonnegative"))
-    !ishermitian(M_Hp) && throw(ArgumentError("M_Hp should be hermitian"))
-    !ishermitian(N_Hc) && throw(ArgumentError("N_Hc should be hermitian"))
-    !ishermitian(L_Hp) && throw(ArgumentError("L_Hp should be hermitian"))
-    size(C) ≠ ()    && throw(ArgumentError("Cwt should be a real scalar"))
-    C < 0           && throw(ArgumentError("Cwt weight should be ≥ 0"))
-    !isnothing(E) && size(E) ≠ () && throw(ArgumentError("Ewt should be a real scalar"))
 end

src/controller/execute.jl

@@ -535,12 +535,12 @@ prediction horizon ``H_p``.
 ```jldoctest
 julia> mpc = LinMPC(KalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4.0)), σR=[√25]), Hp=1, Hc=1);
 
-julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1]
+julia> mpc.estim.model.A[1], mpc.estim.cov.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1]
 (0.1, 25.0, 1.0, 0.1)
 
 julia> setmodel!(mpc, LinModel(ss(0.42, 0.5, 1, 0, 4.0)); R̂=[9], M_Hp=[10], Nwt=[0.666]);
 
-julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1]
+julia> mpc.estim.model.A[1], mpc.estim.cov.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1]
 (0.42, 9.0, 10.0, 0.666)
 ```
 """

src/estimator/construct.jl

@@ -38,6 +38,97 @@ function StateEstimatorBuffer{NT}(
     return StateEstimatorBuffer{NT}(u, û, k, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty)
 end
 
+"Include all the covariance matrices for the Kalman filters and moving horizon estimator."
+struct KalmanCovariances{
+    NT<:Real,
+    # parameters to support both dense and Diagonal matrices (with specialization):
+    Q̂C<:AbstractMatrix{NT},
+    R̂C<:AbstractMatrix{NT},
+}
+    P̂_0::Hermitian{NT, Matrix{NT}}
+    P̂::Hermitian{NT, Matrix{NT}}
+    Q̂::Hermitian{NT, Q̂C}
+    R̂::Hermitian{NT, R̂C}
+    invP̄::Hermitian{NT, Matrix{NT}}
+    invQ̂_He::Hermitian{NT, Q̂C}
+    invR̂_He::Hermitian{NT, R̂C}
+    function KalmanCovariances{NT}(
+        Q̂::Q̂C, R̂::R̂C, P̂_0, He
+    ) where {NT<:Real, Q̂C<:AbstractMatrix{NT}, R̂C<:AbstractMatrix{NT}}
+        if isnothing(P̂_0)
+            P̂_0 = zeros(NT, 0, 0)
+        end
+        P̂_0 = Hermitian(P̂_0, :L)
+        P̂   = copy(P̂_0)
+        Q̂   = Hermitian(Q̂, :L)
+        R̂   = Hermitian(R̂, :L)
+        # the following variables are only for the moving horizon estimator:
+        invP̄, invQ̂, invR̂ = copy(P̂_0), copy(Q̂), copy(R̂)
+        try
+            inv!(invP̄)
+        catch err
+            if err isa PosDefException
+                error("P̂_0 is not positive definite")
+            else
+                rethrow()
+            end
+        end
+        try
+            inv!(invQ̂)
+        catch err
+            if err isa PosDefException
+                error("Q̂ is not positive definite")
+            else
+                rethrow()
+            end
+        end
+        try
+            inv!(invR̂)
+        catch err
+            if err isa PosDefException
+                error("R̂ is not positive definite")
+            else
+                rethrow()
+            end
+        end
+        invQ̂_He = repeatdiag(invQ̂, He)
+        invR̂_He = repeatdiag(invR̂, He)
+        invQ̂_He = Hermitian(invQ̂_He, :L)
+        invR̂_He = Hermitian(invR̂_He, :L)
+        return new{NT, Q̂C, R̂C}(P̂_0, P̂, Q̂, R̂, invP̄, invQ̂_He, invR̂_He)
+    end
+end
+
+"Outer constructor to validate and convert covariance matrices if necessary."
+function KalmanCovariances(
+        model::SimModel{NT}, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing, He=1
+    ) where {NT<:Real}
+    validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0)
+    Q̂, R̂ = NT.(Q̂), NT.(R̂)
+    !isnothing(P̂_0) && (P̂_0 = NT.(P̂_0))
+    return KalmanCovariances{NT}(Q̂, R̂, P̂_0, He)
+end
+
+"""
+    validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing)
+
+Validate sizes and Hermitianity of process `Q̂`` and sensor `R̂` noises covariance matrices.
+
+Also validate initial estimate covariance `P̂_0`, if provided.
+"""
+function validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing)
+    nym = length(i_ym)
+    nx̂  = model.nx + sum(nint_u) + sum(nint_ym)
+    size(Q̂)  ≠ (nx̂, nx̂)     && error("Q̂ size $(size(Q̂)) ≠ nx̂, nx̂ $((nx̂, nx̂))")
+    !ishermitian(Q̂)         && error("Q̂ is not Hermitian")
+    size(R̂)  ≠ (nym, nym)   && error("R̂ size $(size(R̂)) ≠ nym, nym $((nym, nym))")
+    !ishermitian(R̂)         && error("R̂ is not Hermitian")
+    if ~isnothing(P̂_0)
+        size(P̂_0) ≠ (nx̂, nx̂) && error("P̂_0 size $(size(P̂_0)) ≠ nx̂, nx̂ $((nx̂, nx̂))")
+        !ishermitian(P̂_0)    && error("P̂_0 is not Hermitian")
+    end
+end
+
 @doc raw"""
     init_estimstoch(model, i_ym, nint_u, nint_ym) -> As, Cs_u, Cs_y, nxs, nint_u, nint_ym
 

src/estimator/execute.jl

@@ -112,7 +112,7 @@ removes the operating points with [`remove_op!`](@ref) and call [`init_estimate!
   bumpless manual to automatic transfer. See [`init_estimate!`](@ref) for details.
 - Else, `estim.x̂0` is left unchanged. Use [`setstate!`](@ref) to manually modify it.
 
-If applicable, it also sets the error covariance `estim.P̂` to `estim.P̂_0`.
+If applicable, it also sets the error covariance `estim.cov.P̂` to `estim.cov.P̂_0`.
 
 # Examples
 ```jldoctest
@@ -327,7 +327,7 @@ end
 """
     setstate!(estim::StateEstimator, x̂[, P̂]) -> estim
 
-Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`, and `estim.P̂` to `P̂` if applicable. 
+Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`, and `estim.cov.P̂` to `P̂` if applicable. 
 
 The covariance error estimate `P̂` can be set only if `estim` is a [`StateEstimator`](@ref)
 that computes it.
@@ -339,11 +339,11 @@ function setstate!(estim::StateEstimator, x̂, P̂=nothing)
     return estim
 end
 
-"Set the covariance error estimate `estim.P̂` to `P̂`."
+"Set the covariance error estimate `estim.cov.P̂` to `P̂`."
 function setstate_cov!(estim::StateEstimator, P̂)
     if !isnothing(P̂)
         size(P̂) == (estim.nx̂, estim.nx̂) || error("P̂ size must be $((estim.nx̂, estim.nx̂))")
-        estim.P̂ .= to_hermitian(P̂)
+        estim.cov.P̂ .= to_hermitian(P̂)
     end
     return nothing
 end
@@ -375,12 +375,12 @@ augmented model is not verified (see Extended Help for more info).
 ```jldoctest
 julia> kf = KalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4.0)), σQ=[√4.0], σQint_ym=[√0.25]);
 
-julia> kf.model.A[], kf.Q̂[1, 1], kf.Q̂[2, 2] 
+julia> kf.model.A[], kf.cov.Q̂[1, 1], kf.cov.Q̂[2, 2] 
 (0.1, 4.0, 0.25)
 
 julia> setmodel!(kf, LinModel(ss(0.42, 0.5, 1, 0, 4.0)), Q̂=[1 0;0 0.5]);
 
-julia> kf.model.A[], kf.Q̂[1, 1], kf.Q̂[2, 2] 
+julia> kf.model.A[], kf.cov.Q̂[1, 1], kf.cov.Q̂[2, 2] 
 (0.42, 1.0, 0.5)
 ```
 
@@ -449,7 +449,7 @@ function setmodel_estimator!(estim::StateEstimator, model, _ , _ , _ , Q̂, R̂)
     estim.f̂op .= f̂op
     estim.x̂0 .-= estim.x̂op # convert x̂ to x̂0 with the new operating point
     # --- update covariance matrices ---
-    !isnothing(Q̂) && (estim.Q̂ .= to_hermitian(Q̂))
-    !isnothing(R̂) && (estim.R̂ .= to_hermitian(R̂))
+    !isnothing(Q̂) && (estim.cov.Q̂ .= to_hermitian(Q̂))
+    !isnothing(R̂) && (estim.cov.R̂ .= to_hermitian(R̂))
     return nothing
 end

src/estimator/internal_model.jl

@@ -225,8 +225,8 @@ function init_internalmodel(As, Bs, Cs, Ds)
 end
 
 "Throw an error if P̂ != nothing."
-function setstate_cov!(estim::InternalModel, P̂)
-    P̂ == nothing || error("InternalModel does not compute an estimation covariance matrix P̂.")
+function setstate_cov!(::InternalModel, P̂)
+    isnothing(P̂) || error("InternalModel does not compute an estimation covariance matrix P̂.")
     return nothing
 end