@@ -43,26 +43,8 @@ struct SteadyKalmanFilter{
4343 Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model (model, As, Cs_u, Cs_y)
4444 Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
4545 R̂, Q̂ = cov. R̂, cov. Q̂
46- if ny == nym
47- R̂_y = R̂
48- else
49- R̂_y = zeros (NT, ny, ny)
50- R̂_y[i_ym, i_ym] = R̂
51- R̂_y = Hermitian (R̂_y, :L )
52- end
53- K̂_y, P̂ = try
54- ControlSystemsBase. kalman (Discrete, Â, Ĉ, Q̂, R̂_y; direct, extra= Val (true ))
55- catch my_error
56- if isa (my_error, ErrorException)
57- error (" Cannot compute the optimal Kalman gain K̂ for the " *
58- " SteadyKalmanFilter. You may try to remove integrators with " *
59- " nint_u/nint_ym parameter or use the time-varying KalmanFilter." )
60- else
61- rethrow ()
62- end
63- end
64- K̂ = (ny == nym) ? K̂_y : K̂_y[:, i_ym]
65- cov. P̂ .= Hermitian (P̂, :L )
46+ K̂, P̂ = init_skf (model, i_ym, Â, Ĉ, Q̂, R̂; direct)
47+ cov. P̂ .= P̂
6648 x̂0 = [zeros (NT, model. nx); zeros (NT, nxs)]
6749 corrected = [false ]
6850 buffer = StateEstimatorBuffer {NT} (nu, nx̂, nym, ny, nd, nk)
@@ -213,6 +195,43 @@ function SteadyKalmanFilter(
213195 return SteadyKalmanFilter {NT} (model, i_ym, nint_u, nint_ym, cov; direct)
214196end
215197
198+ """
199+ init_skf(model, i_ym, Â, Ĉ, Q̂, R̂; direct=true) -> K̂, P̂
200+
201+ Initialize the steady-state Kalman gain `K̂` and estimation error covariance `P̂`.
202+ """
203+ function init_skf (model, i_ym, Â, Ĉ, Q̂, R̂; direct= true )
204+ ny, nym = model. ny, length (i_ym)
205+ if ny != nym
206+ R̂_y = zeros (NT, ny, ny)
207+ R̂_y[i_ym, i_ym] = R̂
208+ R̂_y = Hermitian (R̂_y, :L )
209+ R̂ = R̂_y
210+ end
211+ K̂, P̂ = try
212+ if pkgversion (ControlSystemsBase) ≥ v " 1.18.2"
213+ ControlSystemsBase. kalman (Discrete, Â, Ĉ, Q̂, R̂; direct, extra= Val (true ))
214+ else
215+ K̂ = ControlSystemsBase. kalman (Discrete, Â, Ĉ, Q̂, R̂; direct)
216+ P̂ = ControlSystemsBase. are (Discrete, Â' , Ĉ' , Q̂, R̂)
217+ K̂, P̂
218+ end
219+ catch my_error
220+ if isa (my_error, ErrorException)
221+ error (" Cannot compute the optimal Kalman gain K̂ for the " *
222+ " SteadyKalmanFilter. You may try to remove integrators with " *
223+ " nint_u/nint_ym parameter or use the time-varying KalmanFilter." )
224+ else
225+ rethrow ()
226+ end
227+ end
228+ if ny != nym
229+ K̂ = K̂[:, i_ym]
230+ end
231+ P̂ = Hermitian (P̂, :L )
232+ return K̂, P̂
233+ end
234+
216235" Throw an error if `setmodel!` is called on a SteadyKalmanFilter w/o the default values."
217236function setmodel_estimator! (estim:: SteadyKalmanFilter , model, _ , _ , _ , Q̂, R̂)
218237 if estim. model != = model || ! isnothing (Q̂) || ! isnothing (R̂)
0 commit comments