Skip to content

Commit 855fb22

Browse files
committed
revert: bump compat of ControlSystemsBase
I now use `pkgversion`
1 parent a9ea963 commit 855fb22

File tree

2 files changed

+40
-21
lines changed

2 files changed

+40
-21
lines changed

Project.toml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ SparseConnectivityTracer = "9f842d2f-2579-4b1d-911e-f412cf18a3f5"
2121
SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35"
2222

2323
[compat]
24-
ControlSystemsBase = "1.18.2"
24+
ControlSystemsBase = "1.9"
2525
DAQP = "0.6, 0.7.1"
2626
DifferentiationInterface = "0.6.45, 0.7"
2727
Documenter = "1"

src/estimator/kalman.jl

Lines changed: 39 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -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.
46-
if ny == nym
47-
R̂_y =
48-
else
49-
R̂_y = zeros(NT, ny, ny)
50-
R̂_y[i_ym, i_ym] =
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-
= (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̂ .=
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)
214196
end
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] =
208+
R̂_y = Hermitian(R̂_y, :L)
209+
= 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+
= ControlSystemsBase.kalman(Discrete, Â, Ĉ, Q̂, R̂; direct)
216+
= 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̂[:, i_ym]
230+
end
231+
= 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."
217236
function setmodel_estimator!(estim::SteadyKalmanFilter, model, _ , _ , _ , Q̂, R̂)
218237
if estim.model !== model || !isnothing(Q̂) || !isnothing(R̂)

0 commit comments

Comments
 (0)