debug: adding .cov where needed

Author: franckgaga

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/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
@@ -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/kalman.jl

@@ -443,7 +443,7 @@ end
 @doc raw"""
     correct_estimate!(estim::KalmanFilter, y0m, d0)
 
-Correct `estim.x̂0` and `estim.P̂` using the time-varying [`KalmanFilter`](@ref).
+Correct `estim.x̂0` and `estim.cov.P̂` using the time-varying [`KalmanFilter`](@ref).
 
 It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)`` estimation covariance 
 ``\mathbf{P̂}_{k}(k)``.
@@ -456,7 +456,7 @@ end
 @doc raw"""
     update_estimate!(estim::KalmanFilter, y0m, d0, u0)
 
-Update [`KalmanFilter`](@ref) state `estim.x̂0` and estimation error covariance `estim.P̂`.
+Update [`KalmanFilter`](@ref) state `estim.x̂0` and estimation error covariance `estim.cov.P̂`.
 
 It implements the classical time-varying Kalman Filter based on the process model described
 in [`SteadyKalmanFilter`](@ref). If `estim.direct == false`, it first corrects the estimate
@@ -790,7 +790,7 @@ end
 @doc raw"""
     update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0)
     
-Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂0` and covariance estimate `estim.P̂`.
+Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂0` and covariance estimate `estim.cov.P̂`.
 
 It implements the unscented Kalman Filter based on the generalized unscented transform[^3].
 See [`init_ukf`](@ref) for the definition of the constants ``\mathbf{m̂, Ŝ}`` and ``γ``. The
@@ -1107,7 +1107,7 @@ end
 @doc raw"""
     update_estimate!(estim::ExtendedKalmanFilter, y0m, d0, u0)
 
-Update [`ExtendedKalmanFilter`](@ref) state `estim.x̂0` and covariance `estim.P̂`.
+Update [`ExtendedKalmanFilter`](@ref) state `estim.x̂0` and covariance `estim.cov.P̂`.
 
 The equations are similar to [`update_estimate!(::KalmanFilter)`](@ref) but with the 
 substitutions ``\mathbf{Ĉ^m = Ĥ^m}(k)`` and ``\mathbf{Â = F̂}(k)``, the Jacobians of the
@@ -1158,7 +1158,7 @@ function update_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0, u0) where NT
     return predict_estimate_kf!(estim, u0, d0, estim.F̂)
 end
 
-"Set `estim.P̂` to `estim.P̂_0` for the time-varying Kalman Filters."
+"Set `estim.cov.P̂` to `estim.cov.P̂_0` for the time-varying Kalman Filters."
 function init_estimate_cov!(
     estim::Union{KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter}, _ , _ , _
 ) 

src/estimator/mhe/execute.jl

@@ -16,7 +16,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0)
         estim.D0[1:estim.model.nd] .= d0
     end
     estim.lastu0 .= u0
-    # estim.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0)
+    # estim.cov.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0)
     invert_cov!(estim, estim.cov.P̂_0)
     estim.P̂arr_old  .= estim.cov.P̂_0
     estim.x̂0arr_old .= 0
@@ -793,16 +793,16 @@ function setmodel_estimator!(
     estim.x̂0arr_old     .-= x̂op
     # --- covariance matrices ---
     if !isnothing(Q̂)
-        estim.Q̂ .= to_hermitian(Q̂)
+        estim.cov.Q̂ .= to_hermitian(Q̂)
         invQ̂  = Hermitian(estim.buffer.Q̂, :L)
-        invQ̂ .= estim.Q̂
+        invQ̂ .= estim.cov.Q̂
         inv!(invQ̂)
         estim.invQ̂_He .= Hermitian(repeatdiag(invQ̂, He), :L)
     end
     if !isnothing(R̂) 
-        estim.R̂ .= to_hermitian(R̂)
+        estim.cov.R̂ .= to_hermitian(R̂)
         invR̂  = Hermitian(estim.buffer.R̂, :L)
-        invR̂ .= estim.R̂
+        invR̂ .= estim.cov.R̂
         inv!(invR̂)
         estim.invR̂_He .= Hermitian(repeatdiag(invR̂, He), :L)
     end

test/3_test_predictive_control.jl

@@ -215,8 +215,8 @@ end
     @test initstate!(mpc1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]]
     setstate!(mpc1, [1,2,3,4], diagm(.1:.1:.4))
     @test mpc1.estim.x̂0 ≈ [1,2,3,4]
-    @test mpc1.estim.P̂  ≈ diagm(.1:.1:.4)
-    setstate!(mpc1, [0,0,0,0], mpc1.estim.P̂_0)
+    @test mpc1.estim.cov.P̂  ≈ diagm(.1:.1:.4)
+    setstate!(mpc1, [0,0,0,0], mpc1.estim.cov.P̂_0)
     preparestate!(mpc1, [50, 30])
     updatestate!(mpc1, mpc1.estim.model.uop, [50, 30])
     @test mpc1.estim.x̂0 ≈ [0,0,0,0]