diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index d869a2416..8b04a57bd 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -4,13 +4,6 @@ Pages = ["state_estim.md"] ``` -## Augmented Model - -```@docs -ModelPredictiveControl.f̂! -ModelPredictiveControl.ĥ! -``` - ## Estimator Construction ```@docs @@ -28,6 +21,13 @@ ModelPredictiveControl.init_matconstraint_mhe ModelPredictiveControl.get_optim_functions(::MovingHorizonEstimator, ::ModelPredictiveControl.GenericModel) ``` +## Augmented Model + +```@docs +ModelPredictiveControl.f̂! +ModelPredictiveControl.ĥ! +``` + ## Update Quadratic Optimization ```@docs diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 66e0847ce..1097cb297 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -236,8 +236,8 @@ end Augment [`LinModel`](@ref) state-space matrices with stochastic ones `As`, `Cs_u`, `Cs_y`. -If ``\mathbf{x_0}`` are `model.x0` states, and ``\mathbf{x_s}``, the states defined at -[`init_estimstoch`](@ref), we define an augmented state vector ``\mathbf{x̂} = +If ``\mathbf{x_0}`` is `model.x0` state, and ``\mathbf{x_s}``, the states defined at +[`init_estimstoch`](@ref), we define an augmented state vector ``\mathbf{x̂_0} = [ \begin{smallmatrix} \mathbf{x_0} \\ \mathbf{x_s} \end{smallmatrix} ]``. The method returns the augmented matrices `Â`, `B̂u`, `Ĉ`, `B̂d` and `D̂d`: ```math @@ -253,9 +253,9 @@ See Extended Help for a detailed definition of the augmented matrices and vector # Extended Help !!! details "Extended Help" - Using the `As`, `Cs_u` and `Cs_y` matrices of the stochastic model provided in argument - and the `model.A`, `model.Bu`, `model.Bd`, `model.C`, `model.Dd` matrices, the - state-space matrices of the augmented model are defined as follows: + Using the `As`, `Cs_u` and `Cs_y` matrices of the stochastic model constructed in + [`init_estimstoch`](@ref)), and `model.A`, `model.Bu`, `model.Bd`, `model.C`, `model.Dd` + matrices, the state-space matrices of the augmented model are defined as follows: ```math \begin{aligned} \mathbf{Â} &= \begin{bmatrix} diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 04a71663e..790e4350a 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -18,8 +18,8 @@ end Mutating state function ``\mathbf{f̂}`` of the augmented model. -By introducing an augmented state vector ``\mathbf{x̂_0}`` like in [`augment_model`](@ref), the -function returns the next state of the augmented model, defined as: +By introducing an augmented state vector ``\mathbf{x̂_0}`` like in [`augment_model`](@ref), +the function returns the next state of the augmented model, defined as: ```math \begin{aligned} \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) \\ @@ -27,10 +27,35 @@ function returns the next state of the augmented model, defined as: \end{aligned} ``` where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂0next` argument. The method mutates `x̂0next`, -`û0` and `k0` in place. The argument `û0` is the input vector of the augmented model, -computed by ``\mathbf{û_0 = u_0 + ŷ_{s_u}}``. The argument `k0` is used to store the -intermediate stage values of `model.solver` (when applicable). The model parameter vector -`model.p` is not included in the function signature for conciseness. +`û0` and `k0` in place. The argument `û0` stores the disturbed input of the augmented model +``\mathbf{û_0}``, and `k0`, the intermediate stage values of `model.solver`, when applicable. +The model parameter `model.p` is not included in the function signature for conciseness. See +Extended Help for details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations. + +# Extended Help +!!! details "Extended Help" + Knowing that the augmented state vector is defined as + ``\mathbf{x̂_0} = [ \begin{smallmatrix} \mathbf{x_0} \\ \mathbf{x_s} \end{smallmatrix} ]``, + the augmented model functions are: + ```math + \begin{aligned} + \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) &= \begin{bmatrix} + \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d_0}(k), \mathbf{p}\Big) \\ + \mathbf{A_s} \mathbf{x_s}(k) \end{bmatrix} \\ + \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k)\Big) &= + \mathbf{h}\Big(\mathbf{x_0}(k), \mathbf{d_0}(k), \mathbf{p}\Big) + \mathbf{y_{s_y}}(k) + \end{aligned} + ``` + in which: + ```math + \begin{aligned} + \mathbf{û_0}(k) &= \mathbf{u_0}(k) + \mathbf{y_{s_u}}(k) \\ + \mathbf{y_{s_u}}(k) &= \mathbf{C_{s_u} x_s}(k) \\ + \mathbf{y_{s_y}}(k) &= \mathbf{C_{s_y} x_s}(k) + \end{aligned} + ``` + The ``\mathbf{f}`` and ``\mathbf{h}`` functions above are in fact the [`f!`](@ref) and + [`h!`](@ref) methods, respectively. """ function f̂!(x̂0next, û0, k0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) return f̂!(x̂0next, û0, k0, model, estim.As, estim.Cs_u, x̂0, u0, d0) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 55c31d8bd..da01b769e 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -1125,7 +1125,7 @@ augmented process model: \end{aligned} ``` The matrix ``\mathbf{Ĥ^m}`` is the rows of ``\mathbf{Ĥ}`` that are measured outputs. The -Jacobians are computed with [`ForwardDiff`](@extref ForwardDiff) bu default. The correction +Jacobians are computed with [`ForwardDiff`](@extref ForwardDiff) by default. The correction and prediction step equations are provided below. The correction step is skipped if `estim.direct == true` since it's already done by the user.