@@ -877,7 +877,8 @@ struct ExtendedKalmanFilter{
877877 SM<: SimModel ,
878878 KC<: KalmanCovariances ,
879879 JB<: AbstractADType ,
880- LF<: Function
880+ FF<: Function ,
881+ HF<: Function
881882} <: StateEstimator{NT}
882883 model:: SM
883884 cov :: KC
@@ -907,13 +908,23 @@ struct ExtendedKalmanFilter{
907908 Ĥ :: Matrix{NT}
908909 Ĥm :: Matrix{NT}
909910 jacobian:: JB
910- linfunc!:: LF
911+ linfuncF̂!:: FF
912+ linfuncĤ!:: HF
911913 direct:: Bool
912914 corrected:: Vector{Bool}
913915 buffer:: StateEstimatorBuffer{NT}
914916 function ExtendedKalmanFilter {NT} (
915- model:: SM , i_ym, nint_u, nint_ym, cov:: KC ; jacobian:: JB , linfunc!:: LF , direct= true
916- ) where {NT<: Real , SM<: SimModel , KC<: KalmanCovariances , JB<: AbstractADType , LF<: Function }
917+ model:: SM ,
918+ i_ym, nint_u, nint_ym, cov:: KC ;
919+ jacobian:: JB , linfuncF̂!:: FF , linfuncĤ!:: HF , direct= true
920+ ) where {
921+ NT<: Real ,
922+ SM<: SimModel ,
923+ KC<: KalmanCovariances ,
924+ JB<: AbstractADType ,
925+ FF<: Function ,
926+ HF<: Function
927+ }
917928 nu, ny, nd, nk = model. nu, model. ny, model. nd, model. nk
918929 nym, nyu = validate_ym (model, i_ym)
919930 As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch (model, i_ym, nint_u, nint_ym)
@@ -927,7 +938,7 @@ struct ExtendedKalmanFilter{
927938 Ĥ, Ĥm = zeros (NT, ny, nx̂), zeros (NT, nym, nx̂)
928939 corrected = [false ]
929940 buffer = StateEstimatorBuffer {NT} (nu, nx̂, nym, ny, nd, nk)
930- return new {NT, SM, KC, JB, LF } (
941+ return new {NT, SM, KC, JB, FF, HF } (
931942 model,
932943 cov,
933944 x̂op, f̂op, x̂0,
@@ -936,7 +947,7 @@ struct ExtendedKalmanFilter{
936947 Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
937948 K̂,
938949 F̂_û, F̂, Ĥ, Ĥm,
939- jacobian, linfunc !,
950+ jacobian, linfuncF̂!, linfuncĤ !,
940951 direct, corrected,
941952 buffer
942953 )
@@ -1044,60 +1055,54 @@ function ExtendedKalmanFilter(
10441055) where {NT<: Real , SM<: SimModel{NT} }
10451056 P̂_0, Q̂, R̂ = to_mat (P̂_0), to_mat (Q̂), to_mat (R̂)
10461057 cov = KalmanCovariances (model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0)
1047- linfunc! = get_ekf_linfunc (NT, model, i_ym, nint_u, nint_ym, jacobian)
1058+ linfuncF̂!, linfuncĤ! = get_ekf_linfuncs (NT, model, i_ym, nint_u, nint_ym, jacobian)
10481059 return ExtendedKalmanFilter {NT} (
1049- model, i_ym, nint_u, nint_ym, cov; jacobian, direct, linfunc!
1060+ model, i_ym, nint_u, nint_ym, cov; jacobian, linfuncF̂!, linfuncĤ!, direct
10501061 )
10511062end
10521063
10531064"""
1054- get_ekf_linfunc (NT, model, i_ym, nint_u, nint_ym, jacobian) -> linfunc!
1065+ get_ekf_linfuncs (NT, model, i_ym, nint_u, nint_ym, jacobian) -> linfuncF̂!, linfuncĤ!
10551066
1056- Return the `linfunc!` function that computes the Jacobians of the augmented model.
1067+ Return the functions that computes the `F̂` and `Ĥ` Jacobians of the augmented model.
10571068
1058- The function has the two following methods :
1069+ The functions has the following signatures :
10591070```
1060- linfunc!(x̂0next , ::Nothing, F̂ , ::Nothing , backend, x̂0, cst_u0, cst_d0) -> nothing
1061- linfunc!(::Nothing , ŷ0 , ::Nothing, Ĥ , backend, x̂0, _ , cst_d0) -> nothing
1071+ linfuncF̂!(F̂, x̂0next , backend, x̂0, cst_u0, cst_d0) -> nothing
1072+ linfuncĤ!(Ĥ , ŷ0 , backend, x̂0, cst_d0) -> nothing
10621073```
1063- To respectively compute only `F̂` or `Ĥ` Jacobian. The methods mutate all the arguments
1064- before `backend` argument. The `backend` argument is an `AbstractADType` object from
1074+ They mutates all the arguments before `backend`, which is an `AbstractADType` object from
10651075`DifferentiationInterface`. The `cst_u0` and `cst_d0` are `DifferentiationInterface.Constant`
10661076objects with the linearization points.
10671077"""
1068- function get_ekf_linfunc (NT, model, i_ym, nint_u, nint_ym, jacobian)
1078+ function get_ekf_linfuncs (NT, model, i_ym, nint_u, nint_ym, jacobian)
10691079 As, Cs_u, Cs_y = init_estimstoch (model, i_ym, nint_u, nint_ym)
1070- nxs = size (As, 1 )
1071- x̂op, f̂op = [model. xop; zeros (nxs)], [model. fop; zeros (nxs)]
1072- f̂_ekf! (x̂0next, x̂0, û0, k0, u0, d0) = f̂! (
1073- x̂0next, û0, k0, model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0
1074- )
1075- ĥ_ekf! (ŷ0, x̂0, d0) = ĥ! (
1076- ŷ0, model, Cs_y, x̂0, d0
1077- )
1078- strict = Val (true )
10791080 nu, ny, nd, nk = model. nu, model. ny, model. nd, model. nk
1080- nx̂ = model. nx + nxs
1081+ nx̂ = model. nx + size (As, 1 )
1082+ x̂op = f̂op = zeros (nx̂) # not important for Jacobian computations
1083+ function f̂_ekf! (x̂0next, x̂0, û0, k0, u0, d0)
1084+ return f̂! (x̂0next, û0, k0, model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0)
1085+ end
1086+ ĥ_ekf! (ŷ0, x̂0, d0) = ĥ! (ŷ0, model, Cs_y, x̂0, d0)
1087+ strict = Val (true )
10811088 x̂0next = zeros (NT, nx̂)
10821089 ŷ0 = zeros (NT, ny)
10831090 x̂0 = zeros (NT, nx̂)
1084- tmp_û0 = Cache (zeros (NT, nu))
1085- tmp_x0i = Cache (zeros (NT, nk))
1091+ û0 = Cache (zeros (NT, nu))
1092+ k0 = Cache (zeros (NT, nk))
10861093 cst_u0 = Constant (zeros (NT, nu))
10871094 cst_d0 = Constant (zeros (NT, nd))
1088- F̂_prep = prepare_jacobian (
1089- f̂_ekf!, x̂0next, jacobian, x̂0, tmp_û0, tmp_x0i , cst_u0, cst_d0; strict
1095+ F̂prep = prepare_jacobian (
1096+ f̂_ekf!, x̂0next, jacobian, x̂0, û0, k0 , cst_u0, cst_d0; strict
10901097 )
1091- Ĥ_prep = prepare_jacobian (ĥ_ekf!, ŷ0, jacobian, x̂0, cst_d0; strict)
1092- function linfunc! (x̂0next, ŷ0:: Nothing , F̂, Ĥ:: Nothing , backend, x̂0, cst_u0, cst_d0)
1093- return jacobian! (
1094- f̂_ekf!, x̂0next, F̂, F̂_prep, backend, x̂0, tmp_û0, tmp_x0i, cst_u0, cst_d0
1095- )
1098+ Ĥprep = prepare_jacobian (ĥ_ekf!, ŷ0, jacobian, x̂0, cst_d0; strict)
1099+ function linfuncF̂! (F̂, x̂0next, backend, x̂0, cst_u0, cst_d0)
1100+ return jacobian! (f̂_ekf!, x̂0next, F̂, F̂prep, backend, x̂0, û0, k0, cst_u0, cst_d0)
10961101 end
1097- function linfunc! (x̂0next :: Nothing , ŷ0, F̂ :: Nothing , Ĥ, backend, x̂0, _ , cst_d0)
1098- return jacobian! (ĥ_ekf!, ŷ0, Ĥ, Ĥ_prep , backend, x̂0, cst_d0)
1102+ function linfuncĤ! (Ĥ , ŷ0, backend, x̂0, cst_d0)
1103+ return jacobian! (ĥ_ekf!, ŷ0, Ĥ, Ĥprep , backend, x̂0, cst_d0)
10991104 end
1100- return linfunc!
1105+ return linfuncF̂!, linfuncĤ!
11011106end
11021107
11031108"""
@@ -1106,12 +1111,12 @@ end
11061111Do the same but for the [`ExtendedKalmanFilter`](@ref).
11071112"""
11081113function correct_estimate! (estim:: ExtendedKalmanFilter , y0m, d0)
1109- model, x̂0 = estim . model, estim. x̂0
1114+ x̂0 = estim. x̂0
11101115 cst_d0 = Constant (d0)
1111- ŷ0, Ĥ = estim. buffer. ŷ, estim. Ĥ
1112- estim. linfunc! ( nothing , ŷ0, nothing , Ĥ, estim. jacobian, x̂0, nothing , cst_d0)
1113- estim . Ĥm .= @views estim . Ĥ[estim. i_ym, :]
1114- return correct_estimate_kf! (estim, y0m, d0, estim . Ĥm)
1116+ ŷ0, Ĥ, Ĥm = estim. buffer. ŷ, estim. Ĥ, estim . Ĥm
1117+ estim. linfuncĤ! (Ĥ , ŷ0, estim. jacobian, x̂0, cst_d0)
1118+ Ĥm .= @views Ĥ[estim. i_ym, :]
1119+ return correct_estimate_kf! (estim, y0m, d0, Ĥm)
11151120end
11161121
11171122
@@ -1154,19 +1159,14 @@ and prediction step equations are provided below. The correction step is skipped
11541159```
11551160"""
11561161function update_estimate! (estim:: ExtendedKalmanFilter{NT} , y0m, d0, u0) where NT<: Real
1157- model, x̂0 = estim. model, estim. x̂0
1158- nx̂, nu = estim. nx̂, model. nu
1159- cst_u0, cst_d0 = Constant (u0), Constant (d0)
11601162 if ! estim. direct
1161- ŷ0, Ĥ = estim. buffer. ŷ, estim. Ĥ
1162- estim. linfunc! (nothing , ŷ0, nothing , Ĥ, estim. jacobian, x̂0, nothing , cst_d0)
1163- estim. Ĥm .= @views estim. Ĥ[estim. i_ym, :]
1164- correct_estimate_kf! (estim, y0m, d0, estim. Ĥm)
1163+ correct_estimate! (estim, y0m, d0)
11651164 end
1165+ cst_u0, cst_d0 = Constant (u0), Constant (d0)
11661166 x̂0corr = estim. x̂0
11671167 x̂0next, F̂ = estim. buffer. x̂, estim. F̂
1168- estim. linfunc! (x̂0next, nothing , F̂, nothing , estim. jacobian, x̂0corr, cst_u0, cst_d0)
1169- return predict_estimate_kf! (estim, u0, d0, estim . F̂)
1168+ estim. linfuncF̂! ( F̂, x̂0next , estim. jacobian, x̂0corr, cst_u0, cst_d0)
1169+ return predict_estimate_kf! (estim, u0, d0, F̂)
11701170end
11711171
11721172" Set `estim.cov.P̂` to `estim.cov.P̂_0` for the time-varying Kalman Filters."
0 commit comments