debug: remove .data in KalmanFilter matrix products

Project.toml

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

src/estimator/execute.jl

@@ -125,7 +125,7 @@ julia> estim = SteadyKalmanFilter(LinModel(tf(3, [10, 1]), 0.5), nint_ym=[2], di
 
 julia> u = [1]; y = [3 - 0.1]; x̂ = round.(initstate!(estim, u, y), digits=3)
 3-element Vector{Float64}:
-  5.0
+ 10.0
   0.0
  -0.1
 

src/estimator/kalman.jl

@@ -1210,7 +1210,7 @@ function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter},
     x̂0, P̂ = estim.x̂0, estim.P̂
     # in-place operations to reduce allocations:
     P̂_Ĉmᵀ = K̂
-    mul!(P̂_Ĉmᵀ, P̂.data, Ĉm') # the ".data" weirdly removes a type instability in mul!
+    mul!(P̂_Ĉmᵀ, P̂, Ĉm')
     M̂ = estim.buffer.R̂
     mul!(M̂, Ĉm, P̂_Ĉmᵀ)
     M̂ .+= R̂
@@ -1252,7 +1252,7 @@ function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter},
     # in-place operations to reduce allocations:
     f̂!(x̂0next, û0, k0, estim, estim.model, x̂0corr, u0, d0)
     P̂corr_Âᵀ = estim.buffer.P̂
-    mul!(P̂corr_Âᵀ, P̂corr.data, Â') # the ".data" weirdly removes a type instability in mul!
+    mul!(P̂corr_Âᵀ, P̂corr, Â')
     Â_P̂corr_Âᵀ = estim.buffer.Q̂
     mul!(Â_P̂corr_Âᵀ, Â, P̂corr_Âᵀ)
     P̂next  = estim.buffer.P̂

src/model/linmodel.jl

@@ -82,9 +82,9 @@ with the state ``\mathbf{x}`` and output ``\mathbf{y}`` vectors. The ``\mathbf{z
 comprises the manipulated inputs ``\mathbf{u}`` and measured disturbances ``\mathbf{d}``, 
 in any order. `i_u` provides the indices of ``\mathbf{z}`` that are manipulated, and `i_d`, 
 the measured disturbances. The constructor automatically discretizes continuous systems,
-resamples discrete ones if `Ts ≠ sys.Ts`, computes a new realization if not minimal, and 
-separates the ``\mathbf{z}`` terms in two parts (details in Extended Help). The rest of the
-documentation assumes discrete dynamics since all systems end up in this form.
+resamples discrete ones if `Ts ≠ sys.Ts`, computes a new balancing and minimal state-space
+realization, and separates the ``\mathbf{z}`` terms in two parts (details in Extended Help). 
+The rest of the documentation assumes discrete models since all systems end up in this form.
 
 See also [`ss`](@extref ControlSystemsBase.ss)
 
@@ -119,10 +119,11 @@ LinModel with a sample time Ts = 0.1 s and:
     `sys` is discrete and the provided argument `Ts ≠ sys.Ts`, the system is resampled by
     using the aforementioned discretization methods.
 
-    Note that the constructor transforms the system to its minimal realization using [`minreal`](@extref ControlSystemsBase.minreal)
-    for controllability/observability. As a consequence, the final state-space
-    representation may be different from the one provided in `sys`. It is also converted 
-    into a more practical form (``\mathbf{D_u=0}`` because of the zero-order hold):
+    Note that the constructor transforms the system to its minimal and balancing realization
+    using [`minreal`](@extref ControlSystemsBase.minreal) for controllability/observability.
+    As a consequence, the final state-space representation will be presumably different from
+    the one provided in `sys`. It is also converted into a more practical form
+    (``\mathbf{D_u=0}`` because of the zero-order hold):
     ```math
     \begin{aligned}
         \mathbf{x}(k+1) &=  \mathbf{A x}(k) + \mathbf{B_u u}(k) + \mathbf{B_d d}(k) \\
@@ -152,8 +153,8 @@ function LinModel(
     sysu = sminreal(sys[:,i_u]) # remove states associated to measured disturbances d
     sysd = sminreal(sys[:,i_d]) # remove states associated to manipulates inputs u
     if !iszero(sysu.D)
-        error("LinModel only supports strictly proper systems (state matrix D must be 0 "*
-              "for columns associated to manipulated inputs u)")
+        error("LinModel only supports strictly proper systems (state-space matrix D must "*
+              "be 0 for columns associated to manipulated inputs u)")
     end
     if iscontinuous(sys)
         isnothing(Ts) && error("Sample time Ts must be specified if sys is continuous")
@@ -175,7 +176,7 @@ function LinModel(
         end     
     end
     # minreal to merge common poles if possible and ensure controllability/observability:
-    sys_dis = minreal([sysu_dis sysd_dis]) # same realization if already minimal
+    sys_dis = minreal([sysu_dis sysd_dis])
     nu  = length(i_u)
     A   = sys_dis.A
     Bu  = sys_dis.B[:,1:nu]
@@ -207,7 +208,7 @@ LinModel with a sample time Ts = 0.5 s and:
 ```
 """
 function LinModel(sys::TransferFunction, Ts::Union{Real,Nothing} = nothing; kwargs...) 
-    sys_min = minreal(ss(sys)) # remove useless states with pole-zero cancellation
+    sys_min = ss(sys) # minreal is automatically called during conversion
     return LinModel(sys_min, Ts; kwargs...)
 end
 
@@ -220,7 +221,7 @@ Discretize with zero-order hold when `sys` is a continuous system with delays.
 The delays must be multiples of the sample time `Ts`.
 """
 function LinModel(sys::DelayLtiSystem, Ts::Real; kwargs...)
-    sys_dis = minreal(c2d(sys, Ts, :zoh)) # c2d only supports :zoh for DelayLtiSystem
+    sys_dis = c2d(sys, Ts, :zoh) # c2d only supports :zoh for DelayLtiSystem
     return LinModel(sys_dis, Ts; kwargs...)
 end
 

test/0_test_module.jl

@@ -3,8 +3,9 @@
     Ts = 400.0
     sys = [ tf(1.90,[1800.0,1])   tf(1.90,[1800.0,1])   tf(1.90,[1800.0,1]);
             tf(-0.74,[800.0,1])   tf(0.74,[800.0,1])    tf(-0.74,[800.0,1])   ] 
-    sys_ss = minreal(ss(sys))
-    Gss = c2d(sys_ss[:,1:2], Ts, :zoh)
-    Gss2 = c2d(sys_ss[:,1:2], 0.5Ts, :zoh)
+    sys_ss = ss(sys)
+    sys_ss_u = sminreal(sys_ss[:,1:2])
+    Gss  = minreal(c2d(sys_ss_u, Ts, :zoh))
+    Gss2 = minreal(c2d(sys_ss_u, 0.5Ts, :zoh))
     export Ts, sys, sys_ss, Gss, Gss2
 end

test/1_test_sim_model.jl

@@ -37,9 +37,9 @@
     @test linmodel5.nu == 2
     @test linmodel5.nd == 1
     @test linmodel5.ny == 2
-    sysu_ss = sminreal(c2d(minreal(ss(sys))[:,1:2], Ts, :zoh))
-    sysd_ss = sminreal(c2d(minreal(ss(sys))[:,3],   Ts, :tustin))
-    sys_ss = [sysu_ss sysd_ss]
+    sysu_ss = c2d(sminreal(ss(sys)[:,1:2]), Ts, :zoh)
+    sysd_ss = c2d(sminreal(ss(sys)[:,3]),   Ts, :tustin)
+    sys_ss = minreal([sysu_ss sysd_ss])
     @test linmodel5.A   ≈ sys_ss.A
     @test linmodel5.Bu  ≈ sys_ss.B[:,1:2]
     @test linmodel5.Bd  ≈ sys_ss.B[:,3]
@@ -51,14 +51,19 @@
 
     linmodel6 = LinModel([delay(Ts) delay(Ts)]*sys,Ts,i_d=[3])
     @test linmodel6.nx == 3
-    @test sum(eigvals(linmodel6.A) .≈ 0) == 1
-
-    linmodel7 = LinModel(
-        ss(diagm( .1: .1: .3), I(3), diagm( .4: .1: .6), 0, 1.0), 
-        i_u=[1, 2],
-        i_d=[3])
-    @test linmodel7.A ≈ diagm( .1: .1: .3)
-    @test linmodel7.C ≈ diagm( .4: .1: .6)
+    @test sum(isapprox.(eigvals(linmodel6.A), 0, atol=1e-15)) == 1
+
+    A = diagm( .1: .1: .3)
+    Bu = [I(2); zeros(1,2)]
+    C = diagm( .4: .1: .6)
+    Bd = [zeros(2,1); I(1)]
+    Dd = 0;
+    linmodel7 = LinModel(A, Bu, C, Bd, Dd, 1.0)
+    @test linmodel7.A ≈ A
+    @test linmodel7.Bu ≈ Bu
+    @test linmodel7.Bd ≈ Bd
+    @test linmodel7.C ≈ C
+    @test linmodel7.Dd ≈ zeros(3,1)
 
     linmodel8 = LinModel(Gss.A, Gss.B, Gss.C, zeros(Float32, 2, 0), zeros(Float32, 2, 0), Ts)
     @test isa(linmodel8, LinModel{Float64})

test/2_test_state_estim.jl

@@ -1334,8 +1334,8 @@ end
 @testitem "MovingHorizonEstimator v.s. Kalman filters" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = setop!(LinModel(sys,Ts,i_d=[3]), uop=[10,50], yop=[50,30], dop=[20])
-    mhe = MovingHorizonEstimator(linmodel1, He=3, nint_ym=0, direct=false)
     kf  = KalmanFilter(linmodel1, nint_ym=0, direct=false)
+    mhe = MovingHorizonEstimator(linmodel1, He=3, nint_ym=0, direct=false)
     X̂_mhe = zeros(4, 6)
     X̂_kf  = zeros(4, 6)
     for i in 1:6
@@ -1347,28 +1347,33 @@ end
         updatestate!(mhe, [11, 50], y, [25])
         updatestate!(kf,  [11, 50], y, [25])
     end
-    @test X̂_mhe ≈ X̂_kf atol=1e-3 rtol=1e-3
-    mhe = MovingHorizonEstimator(linmodel1, He=3, nint_ym=0, direct=true)
+    @test X̂_mhe ≈ X̂_kf atol=1e-6 rtol=1e-6
     kf  = KalmanFilter(linmodel1, nint_ym=0, direct=true)
+    # recuperate P̂(-1|-1) exact value using the Kalman filter:
+    preparestate!(kf, [50, 30], [20])
+    σP̂ = sqrt.(diag(kf.P̂))
+    mhe = MovingHorizonEstimator(linmodel1, He=3, nint_ym=0, direct=true, σP_0=σP̂)
+    updatestate!(kf, [10, 50], [50, 30], [20])
     X̂_mhe = zeros(4, 6)
     X̂_kf  = zeros(4, 6)
     for i in 1:6
-        y = [50,31] + randn(2)
+        y = [50,31] #+ randn(2)
         x̂_mhe = preparestate!(mhe, y, [25])
         x̂_kf  = preparestate!(kf,  y, [25])
         X̂_mhe[:,i] = x̂_mhe
         X̂_kf[:,i]  = x̂_kf
         updatestate!(mhe, [11, 50], y, [25])
         updatestate!(kf,  [11, 50], y, [25])
     end
-    @test X̂_mhe ≈ X̂_kf atol=1e-3 rtol=1e-3
+    @test X̂_mhe ≈ X̂_kf atol=1e-6 rtol=1e-6
+
     f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d
     h = (x,d,model)   -> model.C*x + model.Dd*d
     nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel1, solver=nothing)
     nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[20])
-    mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=false)
     ukf = UnscentedKalmanFilter(nonlinmodel, nint_ym=0, direct=false)
     ekf = ExtendedKalmanFilter(nonlinmodel, nint_ym=0, direct=false)
+    mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=false)
     X̂_mhe = zeros(4, 6)
     X̂_ukf = zeros(4, 6)
     X̂_ekf = zeros(4, 6)
@@ -1384,11 +1389,18 @@ end
         updatestate!(ukf, [11, 50], y, [25])
         updatestate!(ekf, [11, 50], y, [25])
     end
-    @test X̂_mhe ≈ X̂_ukf atol=1e-3 rtol=1e-3
-    @test X̂_mhe ≈ X̂_ekf atol=1e-3 rtol=1e-3
-    mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=true)
+    @test X̂_mhe ≈ X̂_ukf atol=1e-6 rtol=1e-6
+    @test X̂_mhe ≈ X̂_ekf atol=1e-6 rtol=1e-6
+
     ukf = UnscentedKalmanFilter(nonlinmodel, nint_ym=0, direct=true)
     ekf = ExtendedKalmanFilter(nonlinmodel, nint_ym=0, direct=true)
+    # recuperate P̂(-1|-1) exact value using the Unscented Kalman filter:
+    preparestate!(ukf, [50, 30], [20])
+    preparestate!(ekf, [50, 30], [20])
+    σP̂ = sqrt.(diag(ukf.P̂))
+    mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=true, σP_0=σP̂)
+    updatestate!(ukf, [10, 50], [50, 30], [20])
+    updatestate!(ekf, [10, 50], [50, 30], [20])
     X̂_mhe = zeros(4, 6)
     X̂_ukf = zeros(4, 6)
     X̂_ekf = zeros(4, 6)
@@ -1404,8 +1416,8 @@ end
         updatestate!(ukf, [11, 50], y, [25])
         updatestate!(ekf, [11, 50], y, [25])
     end
-    @test X̂_mhe ≈ X̂_ukf atol=1e-3 rtol=1e-3
-    @test X̂_mhe ≈ X̂_ekf atol=1e-3 rtol=1e-3 
+    @test X̂_mhe ≈ X̂_ukf atol=1e-6 rtol=1e-6
+    @test X̂_mhe ≈ X̂_ekf atol=1e-6 rtol=1e-6 
 end
 
 @testitem "MovingHorizonEstimator LinModel v.s. NonLinModel" setup=[SetupMPCtests] begin

test/3_test_predictive_control.jl

@@ -705,13 +705,13 @@ end
     @test_nowarn geq_end(5.0, 4.0, 3.0, 2.0)
     nmpc6  = NonLinMPC(linmodel3, Hp=10)
     preparestate!(nmpc6, [0])
-    @test moveinput!(nmpc6, [0]) ≈ [0.0]
+    @test moveinput!(nmpc6, [0]) ≈ [0.0] atol=5e-2
     nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2)
     nmpc7  = NonLinMPC(nonlinmodel2, Hp=10)
     y = similar(nonlinmodel2.yop)
     nonlinmodel2.solver_h!(y, Float32[0,0], Float32[0], nonlinmodel2.p)
     preparestate!(nmpc7, [0], [0])
-    @test moveinput!(nmpc7, [0], [0]) ≈ [0.0]
+    @test moveinput!(nmpc7, [0], [0]) ≈ [0.0] atol=5e-2
     nmpc8 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=100, Hc=1, transcription=MultipleShooting())
     preparestate!(nmpc8, [0], [0])
     u = moveinput!(nmpc8, [10], [0])