@@ -22,9 +22,11 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
2222 Q̂:: Hermitian{NT, Matrix{NT}}
2323 R̂:: Hermitian{NT, Matrix{NT}}
2424 K̂:: Matrix{NT}
25+ buffer:: StateEstimatorBuffer{NT}
2526 function SteadyKalmanFilter {NT, SM} (
2627 model:: SM , i_ym, nint_u, nint_ym, Q̂, R̂
2728 ) where {NT<: Real , SM<: LinModel }
29+ nu, ny, nd = model. nu, model. ny, model. nd
2830 nym, nyu = validate_ym (model, i_ym)
2931 As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch (model, i_ym, nint_u, nint_ym)
3032 nxs = size (As, 1 )
@@ -33,7 +35,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
3335 validate_kfcov (nym, nx̂, Q̂, R̂)
3436 K̂ = try
3537 Q̂_kalman = Matrix (Q̂) # Matrix() required for Julia 1.6
36- R̂_kalman = zeros (NT, model . ny, model . ny)
38+ R̂_kalman = zeros (NT, ny, ny)
3739 R̂_kalman[i_ym, i_ym] = R̂
3840 ControlSystemsBase. kalman (Discrete, Â, Ĉ, Q̂_kalman, R̂_kalman)[:, i_ym]
3941 catch my_error
@@ -45,17 +47,19 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
4547 rethrow ()
4648 end
4749 end
48- lastu0 = zeros (NT, model . nu)
50+ lastu0 = zeros (NT, nu)
4951 x̂0 = [zeros (NT, model. nx); zeros (NT, nxs)]
5052 Q̂, R̂ = Hermitian (Q̂, :L ), Hermitian (R̂, :L )
53+ buffer = StateEstimatorBuffer {NT} (nu, nx̂, nym, ny, nd)
5154 return new {NT, SM} (
5255 model,
5356 lastu0, x̂op, f̂op, x̂0,
5457 i_ym, nx̂, nym, nyu, nxs,
5558 As, Cs_u, Cs_y, nint_u, nint_ym,
5659 Â, B̂u, Ĉ, B̂d, D̂d,
5760 Q̂, R̂,
58- K̂
61+ K̂,
62+ buffer
5963 )
6064 end
6165end
@@ -234,29 +238,33 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
234238 R̂:: Hermitian{NT, Matrix{NT}}
235239 K̂:: Matrix{NT}
236240 M̂:: Matrix{NT}
241+ buffer:: StateEstimatorBuffer{NT}
237242 function KalmanFilter {NT, SM} (
238243 model:: SM , i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂
239244 ) where {NT<: Real , SM<: LinModel }
245+ nu, ny, nd = model. nu, model. ny, model. nd
240246 nym, nyu = validate_ym (model, i_ym)
241247 As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch (model, i_ym, nint_u, nint_ym)
242248 nxs = size (As, 1 )
243249 nx̂ = model. nx + nxs
244250 Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model (model, As, Cs_u, Cs_y)
245251 validate_kfcov (nym, nx̂, Q̂, R̂, P̂_0)
246- lastu0 = zeros (NT, model . nu)
252+ lastu0 = zeros (NT, nu)
247253 x̂0 = [zeros (NT, model. nx); zeros (NT, nxs)]
248254 Q̂, R̂ = Hermitian (Q̂, :L ), Hermitian (R̂, :L )
249255 P̂_0 = Hermitian (P̂_0, :L )
250256 P̂ = copy (P̂_0)
251257 K̂, M̂ = zeros (NT, nx̂, nym), zeros (NT, nx̂, nym)
258+ buffer = StateEstimatorBuffer {NT} (nu, nx̂, nym, ny, nd)
252259 return new {NT, SM} (
253260 model,
254261 lastu0, x̂op, f̂op, x̂0, P̂,
255262 i_ym, nx̂, nym, nyu, nxs,
256263 As, Cs_u, Cs_y, nint_u, nint_ym,
257264 Â, B̂u, Ĉ, B̂d, D̂d,
258265 P̂_0, Q̂, R̂,
259- K̂, M̂
266+ K̂, M̂,
267+ buffer
260268 )
261269 end
262270end
@@ -402,17 +410,19 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
402410 γ:: NT
403411 m̂:: Vector{NT}
404412 Ŝ:: Diagonal{NT, Vector{NT}}
413+ buffer:: StateEstimatorBuffer{NT}
405414 function UnscentedKalmanFilter {NT, SM} (
406415 model:: SM , i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ
407416 ) where {NT<: Real , SM<: SimModel{NT} }
417+ nu, ny, nd = model. nu, model. ny, model. nd
408418 nym, nyu = validate_ym (model, i_ym)
409419 As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch (model, i_ym, nint_u, nint_ym)
410420 nxs = size (As, 1 )
411421 nx̂ = model. nx + nxs
412422 Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model (model, As, Cs_u, Cs_y)
413423 validate_kfcov (nym, nx̂, Q̂, R̂, P̂_0)
414424 nσ, γ, m̂, Ŝ = init_ukf (model, nx̂, α, β, κ)
415- lastu0 = zeros (NT, model . nu)
425+ lastu0 = zeros (NT, nu)
416426 x̂0 = [zeros (NT, model. nx); zeros (NT, nxs)]
417427 Q̂, R̂ = Hermitian (Q̂, :L ), Hermitian (R̂, :L )
418428 P̂_0 = Hermitian (P̂_0, :L )
@@ -421,6 +431,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
421431 M̂ = Hermitian (zeros (NT, nym, nym), :L )
422432 X̂0, Ŷ0m = zeros (NT, nx̂, nσ), zeros (NT, nym, nσ)
423433 sqrtP̂ = LowerTriangular (zeros (NT, nx̂, nx̂))
434+ buffer = StateEstimatorBuffer {NT} (nu, nx̂, nym, ny, nd)
424435 return new {NT, SM} (
425436 model,
426437 lastu0, x̂op, f̂op, x̂0, P̂,
@@ -429,7 +440,8 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
429440 Â, B̂u, Ĉ, B̂d, D̂d,
430441 P̂_0, Q̂, R̂,
431442 K̂, M̂, X̂0, Ŷ0m, sqrtP̂,
432- nσ, γ, m̂, Ŝ
443+ nσ, γ, m̂, Ŝ,
444+ buffer
433445 )
434446 end
435447end
@@ -698,23 +710,26 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
698710 M̂:: Matrix{NT}
699711 F̂_û:: Matrix{NT}
700712 Ĥ :: Matrix{NT}
713+ buffer:: StateEstimatorBuffer{NT}
701714 function ExtendedKalmanFilter {NT, SM} (
702715 model:: SM , i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂
703716 ) where {NT<: Real , SM<: SimModel }
717+ nu, ny, nd = model. nu, model. ny, model. nd
704718 nym, nyu = validate_ym (model, i_ym)
705719 As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch (model, i_ym, nint_u, nint_ym)
706720 nxs = size (As, 1 )
707721 nx̂ = model. nx + nxs
708722 Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model (model, As, Cs_u, Cs_y)
709723 validate_kfcov (nym, nx̂, Q̂, R̂, P̂_0)
710- lastu0 = zeros (NT, model . nu)
724+ lastu0 = zeros (NT, nu)
711725 x̂0 = [zeros (NT, model. nx); zeros (NT, nxs)]
712726 P̂_0 = Hermitian (P̂_0, :L )
713727 Q̂ = Hermitian (Q̂, :L )
714728 R̂ = Hermitian (R̂, :L )
715729 P̂ = copy (P̂_0)
716730 K̂, M̂ = zeros (NT, nx̂, nym), zeros (NT, nx̂, nym)
717- F̂_û, Ĥ = zeros (NT, nx̂+ model. nu, nx̂), zeros (NT, model. ny, nx̂)
731+ F̂_û, Ĥ = zeros (NT, nx̂+ nu, nx̂), zeros (NT, ny, nx̂)
732+ buffer = StateEstimatorBuffer {NT} (nu, nx̂, nym, ny, nd)
718733 return new {NT, SM} (
719734 model,
720735 lastu0, x̂op, f̂op, x̂0, P̂,
@@ -723,7 +738,8 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
723738 Â, B̂u, Ĉ, B̂d, D̂d,
724739 P̂_0, Q̂, R̂,
725740 K̂, M̂,
726- F̂_û, Ĥ
741+ F̂_û, Ĥ,
742+ buffer
727743 )
728744 end
729745end
0 commit comments