new default settings for Kalman filters

Author: franckgaga

Project.toml

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

README.md

@@ -72,49 +72,49 @@ for more detailed examples.
 
 ### Legend
 
-✅ implemented feature  
-⬜ planned feature
+- [x] implemented feature  
+- [ ] planned feature
 
 ### Model Predictive Control Features
 
-- ✅ linear and nonlinear plant models exploiting multiple dispatch
-- ✅ supported objective function terms:
-  - ✅ output setpoint tracking
-  - ✅ move suppression
-  - ✅ input setpoint tracking
-  - ✅ economic costs (economic model predictive control)
-  - ⬜ terminal cost to ensure nominal stability
-- ✅ soft and hard constraints on:
-  - ✅ output predictions
-  - ✅ manipulated inputs
-  - ✅ manipulated inputs increments
-- ⬜ custom manipulated input constraints that are a function of the predictions
-- ✅ supported feedback strategy:
-  - ✅ state estimator (see State Estimation features)
-  - ✅ internal model structure with a custom stochastic model
-- ✅ offset-free tracking with a single or multiple integrators on measured outputs
-- ✅ support for unmeasured model outputs
-- ✅ feedforward action with measured disturbances that supports direct transmission
-- ✅ custom predictions for:
-  - ✅ output setpoints
-  - ✅ measured disturbances
-- ✅ easy integration with `Plots.jl`
-- ✅ optimization based on `JuMP.jl`:
-  - ✅ quickly compare multiple optimizers
-  - ✅ nonlinear solvers relying on automatic differentiation (exact derivative)
-- ✅ additional information about the optimum to ease troubleshooting
+- [x] linear and nonlinear plant models exploiting multiple dispatch
+- [x] supported objective function terms:
+  - [x] output setpoint tracking
+  - [x] move suppression
+  - [x] input setpoint tracking
+  - [x] economic costs (economic model predictive control)
+  - [ ] terminal cost to ensure nominal stability
+- [x] soft and hard constraints on:
+  - [x] output predictions
+  - [x] manipulated inputs
+  - [x] manipulated inputs increments
+- [ ] custom manipulated input constraints that are a function of the predictions
+- [x] supported feedback strategy:
+  - [x] state estimator (see State Estimation features)
+  - [x] internal model structure with a custom stochastic model
+- [x] offset-free tracking with a single or multiple integrators on measured outputs
+- [x] support for unmeasured model outputs
+- [x] feedforward action with measured disturbances that supports direct transmission
+- [x] custom predictions for:
+  - [x] output setpoints
+  - [x] measured disturbances
+- [x] easy integration with `Plots.jl`
+- [x] optimization based on `JuMP.jl`:
+  - [x] quickly compare multiple optimizers
+  - [x] nonlinear solvers relying on automatic differentiation (exact derivative)
+- [x] additional information about the optimum to ease troubleshooting
 
 ### State Estimation Features
 
-- ⬜ supported state estimators/observers:
-  - ✅ steady-state Kalman filter
-  - ✅ Kalman filter
-  - ✅ Luenberger observer
-  - ✅ internal model structure
-  - ⬜ extended Kalman filter
-  - ✅ unscented Kalman filter
-  - ⬜ moving horizon estimator
-- ✅ observers in predictor form to ease  control applications
-- ⬜ moving horizon estimator that supports:
-  - ⬜ inequality state constraints
-  - ⬜ zero process noise equality constraint to reduce the problem size
+- [ ] supported state estimators/observers:
+  - [x] steady-state Kalman filter
+  - [x] Kalman filter
+  - [x] Luenberger observer
+  - [x] internal model structure
+  - [ ] extended Kalman filter
+  - [x] unscented Kalman filter
+  - [ ] moving horizon estimator
+- [x] observers in predictor form to ease  control applications
+- [ ] moving horizon estimator that supports:
+  - [ ] inequality state constraints
+  - [ ] zero process noise equality constraint to reduce the problem size

docs/src/manual/nonlinmpc.md

@@ -59,7 +59,7 @@ plot(sim!(model, 60, u), plotu=false)
 An [`UnscentedKalmanFilter`](@ref) estimates the plant state :
 
 ```@example 1
-estim = UnscentedKalmanFilter(model, σQ=[0.5, 2.5], σQ_int=[5.0], σR=[0.5])
+estim = UnscentedKalmanFilter(model, σQ=[0.1, 0.5], σQ_int=[5.0], σR=[0.5])
 ```
 
 The standard deviation of the angular velocity ``ω`` is higher here (`σQ` second value)
@@ -80,15 +80,15 @@ sufficient for control. As the motor torque is limited to -1.5 to 1.5 N m, we in
 the input constraints in a [`NonLinMPC`](@ref):
 
 ```@example 1
-mpc = NonLinMPC(estim, Hp=20, Hc=4, Mwt=[0.1], Nwt=[1.0], Cwt=Inf)
+mpc = NonLinMPC(estim, Hp=20, Hc=4, Mwt=[0.05], Nwt=[2.5], Cwt=Inf)
 mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5])
 ```
 
 We test `mpc` performance on `plant` by imposing an angular setpoint of 180° (inverted
 position):
 
 ```@example 1
-res = sim!(mpc, 65, [180.0], plant=plant, x0=zeros(plant.nx), x̂0=zeros(mpc.estim.nx̂))
+res = sim!(mpc, 60, [180.0], plant=plant, x0=zeros(plant.nx), x̂0=zeros(mpc.estim.nx̂))
 plot(res)
 ```
 
@@ -97,6 +97,6 @@ inverted position, the closed-loop response to a step disturbances of 10° is al
 satisfactory:
 
 ```@example 1
-res = sim!(mpc, 65, [180.0], plant=plant, x0=[π, 0], x̂0=[π, 0, 0], y_step=[10])
+res = sim!(mpc, 60, [180.0], plant=plant, x0=[π, 0], x̂0=[π, 0, 0], y_step=[10])
 plot(res)
 ```

src/estimator/kalman.jl

@@ -85,13 +85,13 @@ ones, for ``\mathbf{Ĉ^u, D̂_d^u}``).
 - `model::LinModel` : (deterministic) model for the estimations.
 - `i_ym=1:model.ny` : `model` output indices that are measured ``\mathbf{y^m}``, the rest 
     are unmeasured ``\mathbf{y^u}``.
-- `σQ=fill(0.1,model.nx)` : main diagonal of the process noise covariance ``\mathbf{Q}`` of
-    `model`, specified as a standard deviation vector.
-- `σR=fill(0.1,length(i_ym))` : main diagonal of the sensor noise covariance ``\mathbf{R}``
+- `σQ=fill(1/model.nx,model.nx)` : main diagonal of the process noise covariance
+    ``\mathbf{Q}`` of `model`, specified as a standard deviation vector.
+- `σR=fill(1,length(i_ym))` : main diagonal of the sensor noise covariance ``\mathbf{R}``
     of `model` measured outputs, specified as a standard deviation vector.
 - `nint_ym=fill(1,length(i_ym))` : integrator quantity per measured outputs (vector) for the 
     stochastic model, use `nint_ym=0` for no integrator at all.
-- `σQ_int=fill(0.1,sum(nint_ym))` : same than `σQ` but for the stochastic model covariance
+- `σQ_int=fill(1,sum(nint_ym))` : same than `σQ` but for the stochastic model covariance
     ``\mathbf{Q_{int}}`` (composed of output integrators).
 
 # Examples
@@ -124,10 +124,10 @@ you can use 0 integrator on `model` integrating outputs, or the alternative time
 function SteadyKalmanFilter(
     model::LinModel;
     i_ym::IntRangeOrVector = 1:model.ny,
-    σQ::Vector = fill(0.1, model.nx),
-    σR::Vector = fill(0.1, length(i_ym)),
+    σQ::Vector = fill(1/model.nx, model.nx),
+    σR::Vector = fill(1, length(i_ym)),
     nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
-    σQ_int::Vector = fill(0.1, max(sum(nint_ym), 0))
+    σQ_int::Vector = fill(1, max(sum(nint_ym), 0))
 )
     # estimated covariances matrices (variance = σ²) :
     Q̂  = Diagonal{Float64}([σQ   ; σQ_int    ].^2);
@@ -158,11 +158,11 @@ The [`SteadyKalmanFilter`](@ref) updates it with the precomputed Kalman gain ``\
 
 # Examples
 ```jldoctest
-julia> kf = SteadyKalmanFilter(LinModel(ss(1, 1, 1, 0, 1.0)));
+julia> kf = SteadyKalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4.0)));
 
 julia> x̂ = updatestate!(kf, [1], [0]) # x̂[2] is the integrator state (nint_ym argument)
 2-element Vector{Float64}:
- 1.0
+ 0.5
  0.0
 ```
 """
@@ -239,9 +239,9 @@ with ``\mathbf{P̂}_{-1}(0) = \mathrm{diag}\{ \mathbf{P}(0), \mathbf{P_{int}}(0)
 
 # Arguments
 - `model::LinModel` : (deterministic) model for the estimations.
-- `σP0=fill(10,model.nx)` : main diagonal of the initial estimate covariance 
+- `σP0=fill(1/model.nx,model.nx)` : main diagonal of the initial estimate covariance
     ``\mathbf{P}(0)``, specified as a standard deviation vector.
-- `σP0_int=fill(10,sum(nint_ym))` : same than `σP0` but for the stochastic model
+- `σP0_int=fill(1,sum(nint_ym))` : same than `σP0` but for the stochastic model
     covariance ``\mathbf{P_{int}}(0)`` (composed of output integrators).
 - `<keyword arguments>` of [`SteadyKalmanFilter`](@ref) constructor.
 
@@ -261,12 +261,12 @@ KalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and:
 function KalmanFilter(
     model::LinModel;
     i_ym::IntRangeOrVector = 1:model.ny,
-    σP0::Vector = fill(10, model.nx),
-    σQ::Vector = fill(0.1, model.nx),
-    σR::Vector = fill(0.1, length(i_ym)),
+    σP0::Vector = fill(1/model.nx, model.nx),
+    σQ::Vector  = fill(1/model.nx, model.nx),
+    σR::Vector  = fill(1, length(i_ym)),
     nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
-    σP0_int::Vector = fill(10, max(sum(nint_ym), 0)),
-    σQ_int::Vector = fill(0.1, max(sum(nint_ym), 0))
+    σP0_int::Vector = fill(1, max(sum(nint_ym), 0)),
+    σQ_int::Vector  = fill(1, max(sum(nint_ym), 0))
 )
     # estimated covariances matrices (variance = σ²) :
     P̂0 = Diagonal{Float64}([σP0  ; σP0_int   ].^2);
@@ -429,12 +429,12 @@ UnscentedKalmanFilter estimator with a sample time Ts = 10.0 s, NonLinModel and:
 function UnscentedKalmanFilter(
     model::M;
     i_ym::IntRangeOrVector = 1:model.ny,
-    σP0::Vector = fill(10, model.nx),
-    σQ::Vector = fill(0.1, model.nx),
-    σR::Vector = fill(0.1, length(i_ym)),
+    σP0::Vector = fill(1/model.nx, model.nx),
+    σQ::Vector  = fill(1/model.nx, model.nx),
+    σR::Vector  = fill(1, length(i_ym)),
     nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
-    σP0_int::Vector = fill(10, max(sum(nint_ym), 0)),
-    σQ_int::Vector = fill(0.1, max(sum(nint_ym), 0)),
+    σP0_int::Vector = fill(1, max(sum(nint_ym), 0)),
+    σQ_int::Vector  = fill(1, max(sum(nint_ym), 0)),
     α::Real = 1e-3,
     β::Real = 2,
     κ::Real = 0

test/test_state_estim.jl

@@ -33,8 +33,10 @@ sys = [ tf(1.90,[18.0,1])   tf(1.90,[18.0,1])   tf(1.90,[18.0,1]);
     @test_throws ErrorException SteadyKalmanFilter(linmodel1, nint_ym=[-1,0])
     @test_throws ErrorException SteadyKalmanFilter(linmodel1, nint_ym=0, σQ=[1])
     @test_throws ErrorException SteadyKalmanFilter(linmodel1, nint_ym=0, σR=[1,1,1])
-    @test_throws ErrorException SteadyKalmanFilter(LinModel(tf(1,[1,0]),1),nint_ym=[10])
-end    
+    @test_throws ErrorException SteadyKalmanFilter( # test error compute Kalman gain K
+        LinModel(tf(1,[10,1]),1), nint_ym=[2], σQ_int=[0,0]
+    )
+end
 
 @testset "SteadyKalmanFilter estimator methods" begin
     linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])