added : ExtendedKalmanFilter based on ForwardDiff.jl

Author: franckgaga

Project.toml

@@ -5,6 +5,7 @@ version = "0.5.10"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
+ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9"
 JuMP = "4076af6c-e467-56ae-b986-b466b2749572"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"

docs/src/internals/predictive_control.md

@@ -5,16 +5,38 @@ The prediction methodology of this module is mainly based on Maciejowski textboo
 [^1]: Maciejowski, J. 2000, "Predictive control : with constraints", 1st ed., Prentice Hall,
      ISBN 978-0201398236.
 
+## Controller Initialization
+
 ```@docs
 ModelPredictiveControl.init_deterpred
 ModelPredictiveControl.init_ΔUtoU
-ModelPredictiveControl.relaxU
-ModelPredictiveControl.relaxΔU
-ModelPredictiveControl.relaxŶ
 ModelPredictiveControl.init_quadprog
 ModelPredictiveControl.init_stochpred
 ModelPredictiveControl.init_linconstraint
+```
+
+## Constraint Relaxation
+
+```@docs
+ModelPredictiveControl.relaxU
+ModelPredictiveControl.relaxΔU
+ModelPredictiveControl.relaxŶ
+```
+
+## Get Estimates
+
+```@docs
 ModelPredictiveControl.getestimates!
+```
+
+## Predictions
+
+```@docs
 ModelPredictiveControl.initpred!
+```
+
+## Constraints
+
+```@docs
 ModelPredictiveControl.linconstraint!
 ```

docs/src/internals/state_estim.md

@@ -1,27 +1,34 @@
 # Functions: StateEstimator Internals
 
+## Estimator Initialization
+
 ```@docs
 ModelPredictiveControl.init_estimstoch
 ModelPredictiveControl.augment_model
-ModelPredictiveControl.f̂
-ModelPredictiveControl.ĥ
+ModelPredictiveControl.init_ukf
+ModelPredictiveControl.init_internalmodel
 ```
 
-## KalmanFilter
+## Augmented Model
 
 ```@docs
-ModelPredictiveControl.updatestate_kf!
+ModelPredictiveControl.f̂
+ModelPredictiveControl.ĥ
 ```
 
-## UnscentedKalmanFilter
+## Remove Operating Points
 
 ```@docs
-ModelPredictiveControl.init_ukf
-ModelPredictiveControl.updatestate_ukf!
+ModelPredictiveControl.remove_op!
 ```
 
-## InternalModel
+## Update Estimate
+
+!!! info
+    All these methods assume that the operating points are already removed in `u`, `ym`
+    and `d` arguments. Strickly speaking, the arguments should be called `u0`, `ym0` and
+    `d0`, following [`setop!`](@ref) notation. The `0` is dropped to simplify the notation.
 
 ```@docs
-ModelPredictiveControl.init_internalmodel
+ModelPredictiveControl.update_estimate!
 ```

docs/src/public/state_estim.md

@@ -62,6 +62,12 @@ Luenberger
 UnscentedKalmanFilter
 ```
 
+## ExtendedKalmanFilter
+
+```@docs
+ExtendedKalmanFilter
+```
+
 ## InternalModel
 
 ```@docs

example/juMPC.jl

@@ -72,6 +72,13 @@ updatestate!(ssKalmanFilter2,[1, 1],[1,1])
 
 initstate!(ssKalmanFilter1,[0,0],[2,1])
 
+extKalmanFilter = ExtendedKalmanFilter(nonLinModel2)
+
+initstate!(extKalmanFilter, [10,10], [50,30], [5]) 
+updatestate!(extKalmanFilter, [10,11], [50,30], [5])
+
+
+
 uscKalmanFilter1 = UnscentedKalmanFilter(linModel1)
 
 updatestate!(uscKalmanFilter1,[0,0],[2,1])

src/ModelPredictiveControl.jl

@@ -5,14 +5,15 @@ using PrecompileTools
 using LinearAlgebra, Random
 using RecipesBase
 using ControlSystemsBase
+using ForwardDiff
 using JuMP
 using PreallocationTools
 import OSQP, Ipopt
 
 
 export SimModel, LinModel, NonLinModel, setop!, setstate!, updatestate!, evaloutput
-export StateEstimator, InternalModel
-export SteadyKalmanFilter, KalmanFilter, Luenberger, UnscentedKalmanFilter
+export StateEstimator, InternalModel, Luenberger
+export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter
 export initstate!
 export PredictiveController, LinMPC, NonLinMPC, setconstraint!, moveinput!, getinfo, sim!
 

src/estimator/internal_model.jl

@@ -133,9 +133,9 @@ optimally update the stochastic estimate ``\mathbf{x̂_s}`` are:
 ```
 with current stochastic outputs estimation ``\mathbf{ŷ_s}(k)``, composed of the measured 
 ``\mathbf{ŷ_s^m}(k) = \mathbf{y^m}(k) - \mathbf{ŷ_d^m}(k)`` and unmeasured 
-``\mathbf{ŷ_s^u = 0}`` outputs. See [^3].
+``\mathbf{ŷ_s^u = 0}`` outputs. See [^1].
 
-[^3]: Desbiens, A., D. Hodouin & É. Plamondon. 2000, "Global predictive control : a unified
+[^1]: Desbiens, A., D. Hodouin & É. Plamondon. 2000, "Global predictive control : a unified
     control structure for decoupling setpoint tracking, feedforward compensation and 
     disturbance rejection dynamics", *IEE Proceedings - Control Theory and Applications*, 
     vol. 147, no 4, https://doi.org/10.1049/ip-cta:20000443, p. 465–475, ISSN 1350-2379.
@@ -147,14 +147,22 @@ function init_internalmodel(As, Bs, Cs, Ds)
 end
 
 @doc raw"""
-    updatestate!(estim::InternalModel, u, ym, d=Float64[])
+    update_estimate!(estim::InternalModel, u, ym, d=Float64[])
 
 Update `estim.x̂` \ `x̂d` \ `x̂s` with current inputs `u`, measured outputs `ym` and dist. `d`.
+
+The [`InternalModel`](@ref) updates the deterministic `x̂d` and stochastic `x̂s` estimates with:
+```math
+\begin{aligned}
+    \mathbf{x̂_d}(k+1) &= \mathbf{f}\Big( \mathbf{x̂_d}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) \\
+    \mathbf{x̂_s}(k+1) &= \mathbf{Â_s x̂_s}(k) + \mathbf{B̂_s ŷ_s}(k)
+\end{aligned}
+```
+This estimator does not augment the state vector, thus ``\mathbf{x̂ = x̂_d}``. See 
+[`init_internalmodel`](@ref) for details. 
 """
-function updatestate!(estim::InternalModel, u, ym, d=Float64[])
+function update_estimate!(estim::InternalModel, u, ym, d=Float64[])
     model = estim.model
-    # ---- remove operating points ----
-    u, d, ym = remove_op!(estim, u, d, ym)
     x̂d, x̂s = estim.x̂d, estim.x̂s
     # -------------- deterministic model ---------------------
     ŷd = h(model, x̂d, d)