EMPC additions

- added: moveinput! ry arg default value for pure EMPC
- added economic cost JE simple unit test

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.6.0"
+version = "0.6.1"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

src/estimator/kalman.jl

@@ -298,7 +298,7 @@ control period ``k-1``. See [^2] for details.
      Linear Dynamical Systems*, https://web.stanford.edu/class/ee363/lectures/kf.pdf.
 """
 function update_estimate!(estim::KalmanFilter, u, ym, d)
-    return update_state_kf!(estim, estim.Â, estim.Ĉm, u, ym, d)
+    return update_estimate_kf!(estim, estim.Â, estim.Ĉm, u, ym, d)
 end
 
 struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
@@ -665,7 +665,7 @@ function update_estimate!(estim::ExtendedKalmanFilter, u, ym, d=Float64[])
     F̂  = ForwardDiff.jacobian(x̂ -> f̂(estim, x̂, u, d), estim.x̂)
     Ĥ  = ForwardDiff.jacobian(x̂ -> ĥ(estim, x̂, d), estim.x̂)
     Ĥm = Ĥ[estim.i_ym, :] 
-    return update_state_kf!(estim, F̂, Ĥm, u, ym, d)
+    return update_estimate_kf!(estim, F̂, Ĥm, u, ym, d)
 end
 
 "Initialize the covariance estimate `P̂` for the time-varying Kalman Filters" 
@@ -692,15 +692,15 @@ function validate_kfcov(nym, nx̂, Q̂, R̂, P̂0=nothing)
 end
 
 """
-    update_state_kf!(estim, Â, Ĉm, u, ym, d)
+    update_estimate_kf!(estim, Â, Ĉm, u, ym, d)
 
 Update time-varying/extended Kalman Filter estimates with augmented `Â` and `Ĉm` matrices.
 
 Allows code reuse for the time-varying and extended Kalman filters. They update the state
 `x̂` and covariance `P̂` with the same equations. The extended filter substitutes the 
 augmented model matrices with its Jacobians (`Â = F̂` and `Ĉm = Ĥm`).
 """
-function update_state_kf!(estim, Â, Ĉm, u, ym, d)
+function update_estimate_kf!(estim, Â, Ĉm, u, ym, d)
     x̂, P̂, Q̂, R̂ = estim.x̂, estim.P̂, estim.Q̂, estim.R̂
     M  = (P̂ * Ĉm') / (Ĉm * P̂ * Ĉm' + R̂)
     K  = Â * M

src/predictive_control.jl

@@ -199,7 +199,7 @@ setnonlincon!(::PredictiveController, ::SimModel) = nothing
 @doc raw"""
     moveinput!(
         mpc::PredictiveController, 
-        ry, 
+        ry = mpc.estim.model.yop, 
         d  = Float64[];
         R̂y = repeat(ry, mpc.Hp), 
         D̂  = repeat(d,  mpc.Hp), 
@@ -246,7 +246,7 @@ julia> u = moveinput!(mpc, [5]); round.(u, digits=3)
 """
 function moveinput!(
     mpc::PredictiveController, 
-    ry::Vector, 
+    ry::Vector = mpc.estim.model.yop, 
     d ::Vector = Float64[];
     R̂y::Vector = repeat(ry, mpc.Hp),
     D̂ ::Vector = repeat(d,  mpc.Hp),
@@ -1025,7 +1025,7 @@ end
 
 "Functor allowing callable `PredictiveController` object as an alias for `moveinput!`."
 function (mpc::PredictiveController)(
-    ry::Vector, 
+    ry::Vector = mpc.estim.model.yop, 
     d ::Vector = Float64[];
     kwargs...
 )

test/test_predictive_control.jl

@@ -164,6 +164,12 @@ end
     nmpc3 = NonLinMPC(nonlinmodel, Mwt=[0], Nwt=[0], Lwt=[1], ru=[12])
     u = moveinput!(nmpc3, [0])
     @test u ≈ [12] atol=1e-2
+    Hp = 1000
+    R̂y = fill(5, Hp)
+    JE = (_ , ŶE, _ ) -> sum((ŶE[2:end] - R̂y).^2)
+    nmpc4 = NonLinMPC(nonlinmodel, Mwt=[0], Nwt=[0], Cwt=Inf, Ewt=1, JE=JE, Hp=Hp, Hc=1)
+    u = moveinput!(nmpc4)
+    @test u ≈ [1] atol=1e-2
 end
 
 @testset "NonLinMPC other methods" begin