debug MHE

Author: franckgaga

src/estimator/internal_model.jl

@@ -261,6 +261,22 @@ function init_estimate!(estim::InternalModel, model::LinModel{NT}, u, ym, d) whe
     return nothing
 end
 
+@doc raw"""
+    evalŷ(estim::InternalModel, ym, d) -> ŷ
+
+Get [`InternalModel`](@ref) output `ŷ` from current measured outputs `ym` and dist. `d`.
+
+[`InternalModel`](@ref) estimator needs current measured outputs ``\mathbf{y^m}(k)`` to 
+estimate its outputs ``\mathbf{ŷ}(k)``, since the strategy imposes that 
+``\mathbf{ŷ^m}(k) = \mathbf{y^m}(k)`` is always true.
+"""
+function evalŷ(estim::InternalModel{NT}, ym, d) where NT<:Real
+    ŷ = Vector{NT}(undef, estim.model.ny)
+    ŷ = h!(ŷ, estim.model, estim.x̂d, d - estim.model.dop) .+ estim.model.yop
+    ŷ[estim.i_ym] = ym
+    return ŷ
+end
+
 "Print InternalModel information without i/o integrators."
 function print_estim_dim(io::IO, estim::InternalModel, n)
     nu, nd = estim.model.nu, estim.model.nd

src/estimator/mhe/execute.jl

@@ -137,8 +137,9 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     Ŷ = Vector{NT}(undef, ny*Nk)
     for i=1:Nk
         d0 = @views D[(1 + nd*(i-1)):(nd*i)] # operating point already removed in estim.D
-        x̂ = @views X̂[(1 + nx̂*(i-1)):(nx̂*i)]
-        Ŷ[(1 + ny*(i-1)):(ny*i)] = ĥ(estim, model, x̂, d0) + model.yop
+        x̂  = @views X̂[(1 + nx̂*(i-1)):(nx̂*i)]
+        ĥ!(Ŷ[(1 + ny*(i-1)):(ny*i)], estim, model, x̂, d0) 
+        Ŷ[(1 + ny*(i-1)):(ny*i)] .+= model.yop
     end
     Ŷm = Ym - V̂
     info[:Ŵ] = estim.Ŵ[1:Nk*nŵ]
@@ -326,10 +327,11 @@ function update_cov!(P̂, estim::MovingHorizonEstimator, ::LinModel, u, ym, d)
     return update_estimate_kf!(estim, u, ym, d, estim.Â, estim.Ĉ[estim.i_ym, :], P̂)
 end
 "Update it with the `ExtendedKalmanFilter` if model is not a `LinModel`."
-function update_cov!(P̂, estim::MovingHorizonEstimator, ::SimModel, u, ym, d) 
+function update_cov!(P̂, estim::MovingHorizonEstimator, model::SimModel, u, ym, d) 
     # TODO: also support UnscentedKalmanFilter
-    F̂ = ForwardDiff.jacobian(x̂ -> f̂(estim, estim.model, x̂, u, d), estim.x̂)
-    Ĥ = ForwardDiff.jacobian(x̂ -> ĥ(estim, estim.model, x̂, d), estim.x̂)
+    x̂next, ŷ = similar(estim.x̂), similar(model.yop)
+    F̂ = ForwardDiff.jacobian((x̂next, x̂) -> f̂!(x̂next, estim, estim.model, x̂, u, d), x̂next, estim.x̂)
+    Ĥ = ForwardDiff.jacobian((ŷ, x̂)     -> ĥ!(ŷ, estim, estim.model, x̂, d), ŷ, estim.x̂)
     return update_estimate_kf!(estim, u, ym, d, F̂, Ĥ[estim.i_ym, :],  P̂)
 end
 
@@ -394,16 +396,22 @@ function predict!(
     V̂, X̂, estim::MovingHorizonEstimator, model::SimModel, Z̃::Vector{T}
 ) where {T<:Real}
     Nk = estim.Nk[]
-    nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym
+    nu, nd, ny, nx̂, nŵ, nym = model.nu, model.nd, model.ny, estim.nx̂, estim.nx̂, estim.nym
     nx̃ = !isinf(estim.C) + nx̂
+    # TODO: avoid these two allocations if possible:
+    ŷ, x̂next = Vector{T}(undef, ny), Vector{T}(undef, nx̂)
     x̂ = @views Z̃[nx̃-nx̂+1:nx̃]
     for j=1:Nk
         u  = @views estim.U[ (1 + nu  * (j-1)):(nu*j)]
         ym = @views estim.Ym[(1 + nym * (j-1)):(nym*j)]
         d  = @views estim.D[ (1 + nd  * (j-1)):(nd*j)]
         ŵ  = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)]
-        V̂[(1 + nym*(j-1)):(nym*j)] = ym - ĥ(estim, model, x̂, d)[estim.i_ym]
-        X̂[(1 + nx̂ *(j-1)):(nx̂ *j)] = f̂(estim, model, x̂, u, d) + ŵ
+        ĥ!(ŷ, estim, model, x̂, d)
+        ŷm = @views ŷ[estim.i_ym]
+        V̂[(1 + nym*(j-1)):(nym*j)] .= ym .- ŷm
+        f̂!(x̂next, estim, model, x̂, u, d)
+        x̂next .+= ŵ
+        X̂[(1 + nx̂ *(j-1)):(nx̂ *j)] = x̂next
         x̂ = @views X̂[(1 + nx̂*(j-1)):(nx̂*j)]
     end
     return V̂, X̂

src/state_estim.jl

@@ -66,19 +66,4 @@ Evaluate [`StateEstimator`](@ref) output `ŷ` from measured disturbance `d` and
 Second argument is ignored, except for [`InternalModel`](@ref).
 """
 evalŷ(estim::StateEstimator, _ , d) = evaloutput(estim, d)
-
-@doc raw"""
-    evalŷ(estim::InternalModel, ym, d) -> ŷ
-
-Get [`InternalModel`](@ref) output `ŷ` from current measured outputs `ym` and dist. `d`.
-
-[`InternalModel`](@ref) estimator needs current measured outputs ``\mathbf{y^m}(k)`` to 
-estimate its outputs ``\mathbf{ŷ}(k)``, since the strategy imposes that 
-``\mathbf{ŷ^m}(k) = \mathbf{y^m}(k)`` is always true.
-"""
-function evalŷ(estim::InternalModel, ym, d)
-    ŷ = h(estim.model, estim.x̂d, d - estim.model.dop) + estim.model.yop
-    ŷ[estim.i_ym] = ym
-    return ŷ
-end