added tests for ExtendedKalmanFIlter

Author: franckgaga

src/controller/nonlinmpc.jl

@@ -203,13 +203,13 @@ end
 Invoke [`getinfo(::PredictiveController)`](@ref) and add `:JE` the economic optimum ``J_E``.
 """
 function getinfo(mpc::NonLinMPC)
-    info, sol_summary = invoke(getinfo, Tuple{PredictiveController}, mpc)
+    sol_summary, info = invoke(getinfo, Tuple{PredictiveController}, mpc)
     U, Ŷ, D̂ = info[:U], info[:Ŷ], info[:D̂]
     UE = [U; U[(end - mpc.estim.model.nu + 1):end]]
     ŶE = [mpc.ŷ; Ŷ]
     D̂E = [mpc.d; D̂]
     info[:JE] = mpc.JE(UE, ŶE, D̂E)
-    return info, sol_summary
+    return sol_summary, info
 end
 
 """

src/predictive_control.jl

@@ -268,9 +268,8 @@ end
 
 Get additional information about `mpc` controller optimum to ease troubleshooting.
 
-Return the dictionary `info` with the additional information, and `sol_summary`, the
-optimizer solution summary that can be printed. The dictionary `info` has the following 
-fields:
+Return the optimizer solution summary that can be printed, `sol_summary`, and the dictionary 
+`info` with the following fields:
 
 - `:ΔU` : optimal manipulated input increments over `Hc` ``(\mathbf{ΔU})``
 - `:ϵ`  : optimal slack variable ``(ϵ)``
@@ -292,7 +291,7 @@ julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1, Hc=1);
 
 julia> u = moveinput!(mpc, [10]);
 
-julia> info, sol_summary = getinfo(mpc); round.(info[:Ŷ], digits=3)
+julia> sol_summary, info = getinfo(mpc); round.(info[:Ŷ], digits=3)
 1-element Vector{Float64}:
  10.0
 ```
@@ -313,7 +312,7 @@ function getinfo(mpc::PredictiveController)
     info[:Ŷd]  = info[:Ŷ] - info[:Ŷs]
     info[:R̂y]  = mpc.R̂y
     info[:R̂u]  = mpc.R̂u
-    return info, sol_summary
+    return sol_summary, info
 end
 
 """

test/test_predictive_control.jl

@@ -63,7 +63,7 @@ end
     @test u ≈ [1] atol=1e-2
     u = mpc1(r)
     @test u ≈ [1] atol=1e-2
-    info, _ = getinfo(mpc1)
+    _ , info = getinfo(mpc1)
     @test info[:u] ≈ u
     @test info[:Ŷ][end] ≈ 5 atol=1e-2
     mpc2 = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Cwt=Inf, Hp=1000, Hc=1)
@@ -143,7 +143,7 @@ end
     @test u ≈ [1] atol=1e-3
     u = nmpc_lin(r)
     @test u ≈ [1] atol=1e-3
-    info, _ = getinfo(nmpc_lin)
+    _ , info = getinfo(nmpc_lin)
     @test info[:u] ≈ u
     @test info[:Ŷ][end] ≈ 5 atol=1e-2
     f(x,u,_) = linmodel.A*x + linmodel.Bu*u
@@ -155,7 +155,7 @@ end
     @test u ≈ [1] atol=1e-2
     u = nmpc1(r)
     @test u ≈ [1] atol=1e-2
-    info, _ = getinfo(nmpc1)
+    _ , info = getinfo(nmpc1)
     @test info[:u] ≈ u
     @test info[:Ŷ][end] ≈ 5 atol=1e-2
     nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=Inf, Hp=1000, Hc=1)

test/test_state_estim.jl

@@ -277,4 +277,58 @@ end
     @test initstate!(ukf1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]]
     setstate!(ukf1, [1,2,3,4])
     @test ukf1.x̂ ≈ [1,2,3,4]
-end
+end
+
+@testset "ExtendedKalmanFilter construction" begin
+    linmodel1 = LinModel(sys,Ts,i_d=[3])
+    f(x,u,d) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
+    h(x,d)   = linmodel1.C*x + linmodel1.Du*d
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1)
+
+    ekf1 = ExtendedKalmanFilter(linmodel1)
+    @test ekf1.nym == 2
+    @test ekf1.nyu == 0
+    @test ekf1.nxs == 2
+    @test ekf1.nx̂ == 6
+
+    ekf2 = ExtendedKalmanFilter(nonlinmodel)
+    @test ekf2.nym == 2
+    @test ekf2.nyu == 0
+    @test ekf2.nxs == 2
+    @test ekf2.nx̂ == 6
+
+    ekf3 = ExtendedKalmanFilter(nonlinmodel, i_ym=[2])
+    @test ekf3.nym == 1
+    @test ekf3.nyu == 1
+    @test ekf3.nxs == 1
+    @test ekf3.nx̂ == 5
+
+    ekf4 = ExtendedKalmanFilter(nonlinmodel, σQ=[1,2,3,4], σQ_int=[5, 6],  σR=[7, 8])
+    @test ekf4.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36]))
+    @test ekf4.R̂ ≈ Hermitian(diagm(Float64[49, 64]))
+    
+    ekf5 = ExtendedKalmanFilter(nonlinmodel, nint_ym=[2,2])
+    @test ekf5.nxs == 4
+    @test ekf5.nx̂ == 8
+
+    ekf6 = ExtendedKalmanFilter(nonlinmodel, σP0=[1,2,3,4], σP0_int=[5,6])
+    @test ekf6.P̂0 ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36]))
+    @test ekf6.P̂  ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36]))
+    @test ekf6.P̂0 !== ekf6.P̂
+end
+
+@testset "ExtendedKalmanFilter estimator methods" begin
+    linmodel1 = LinModel(sys,Ts,i_u=[1,2])
+    f(x,u,_) = linmodel1.A*x + linmodel1.Bu*u
+    h(x,_)   = linmodel1.C*x
+    nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2), uop=[10,50], yop=[50,30])
+    ekf1 = ExtendedKalmanFilter(nonlinmodel)
+    @test updatestate!(ekf1, [10, 50], [50, 30]) ≈ zeros(4) atol=1e-9
+    @test updatestate!(ekf1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) atol=1e-9
+    @test ekf1.x̂ ≈ zeros(4) atol=1e-9
+    @test evaloutput(ekf1) ≈ ekf1() ≈ [50, 30]
+    @test evaloutput(ekf1, Float64[]) ≈ ekf1(Float64[]) ≈ [50, 30]
+    @test initstate!(ekf1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]]
+    setstate!(ekf1, [1,2,3,4])
+    @test ekf1.x̂ ≈ [1,2,3,4]
+end