doc: arguments MHE

Author: franckgaga

README.md

@@ -121,6 +121,6 @@ for more detailed examples.
   - [x] measured outputs
 - [x] bumpless manual to automatic transfer for control with a proper intial estimate
 - [x] observers in predictor form to ease control applications
-- [ ] moving horizon estimator that supports:
-  - [ ] inequality state constraints
+- [x] moving horizon estimator that supports:
+  - [x] inequality state constraints
   - [ ] zero process noise equality constraint to reduce the problem size

src/estimator/mhe.jl

@@ -131,6 +131,31 @@ N_k =                     \begin{cases}
     k + 1   &  k < H_e    \\
     H_e     &  k ≥ H_e    \end{cases}
 ```
+See [`SteadyKalmanFilter`](@ref) for details on ``\mathbf{R̂}, \mathbf{Q̂}`` covariances and
+model augmentation.
+
+# Arguments
+- `model::SimModel` : (deterministic) model for the estimations.
+- `He=nothing`: estimation horizon ``H_e``, must be specified.
+- `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the moving horizon
+   estimator, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
+   (default to [`Ipopt.jl`](https://github.com/jump-dev/Ipopt.jl) optimizer).
+- `<keyword arguments>` of [`SteadyKalmanFilter`](@ref) constructor.
+- `<keyword arguments>` of [`KalmanFilter`](@ref) constructor.
+
+# Examples
+```jldoctest
+julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1);
+
+julia> estim = MovingHorizonEstimator(model, He=5, σR=[1], σP0=[0.01])
+MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, NonLinModel and:
+ 5 estimation steps He
+ 1 manipulated inputs u (0 integrating states)
+ 2 states x̂
+ 1 measured outputs ym (1 integrating states)
+ 0 unmeasured outputs yu
+ 0 measured disturbances d
+```
 """
 function MovingHorizonEstimator(
     model::SM;
@@ -157,6 +182,21 @@ function MovingHorizonEstimator(
     )
 end
 
+@doc raw"""
+    MovingHorizonEstimator(model, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim)
+
+Construct the estimator from the augmented covariance matrices `P̂0`, `Q̂` and `R̂`.
+
+This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``.
+"""
+function MovingHorizonEstimator(
+    model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim
+) where {NT<:Real, SM<:SimModel{NT}}
+    return MovingHorizonEstimator{NT, SM}(
+        model, He, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂, optim
+    )
+end
+
 
 """
     init_optimization!(estim::MovingHorizonEstimator, optim::JuMP.GenericModel)
@@ -244,8 +284,6 @@ function init_optimization!(
             register(optim, sym, nvar, gfunc[i_end_X̂min+i], autodiff=true)
         end
     end
-    # TODO: moved this call to setconstraint! when the function will handle MHE
-    setnonlincon!(estim, estim.model)
     return nothing
 end
 
@@ -464,6 +502,13 @@ function obj_nonlinprog(
     return dot(x̄0, invP̄, x̄0) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂)
 end
 
+"""
+    predict!(Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃) -> Ŷm, X̂
+
+Compute the predicted measured outputs `Ŷm` and states `X̂` for the `MovingHorizonEstimator`.
+
+The method mutates `Ŷm` and `X̂` vector arguments.
+"""
 function predict!(
     Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T}
 ) where {T<:Real}
@@ -480,6 +525,11 @@ function predict!(
     return Ŷm, X̂
 end
 
+"""
+    con_nonlinprog!(g, estim::MovingHorizonEstimator, model::SimModel, X̂)
+
+Nonlinear constrains for [`MovingHorizonEstimator`](@ref).
+"""
 function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂)
     nX̂con, nX̂ = length(estim.X̂min), estim.Nk[]*estim.nx̂
     for i in eachindex(g)

test/test_state_estim.jl

@@ -453,7 +453,7 @@ end
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     ukf3 = UnscentedKalmanFilter(linmodel3)
     x̂ = updatestate!(ukf3, [0], [0])
-    @test x̂ ≈ [0, 0]
+    @test x̂ ≈ [0, 0] atol=1e-3
     @test isa(x̂, Vector{Float32})
 end