@@ -38,6 +38,97 @@ function StateEstimatorBuffer{NT}(
3838 return StateEstimatorBuffer {NT} (u, û, k, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty)
3939end
4040
41+ " Include all the covariance matrices for the Kalman filters and moving horizon estimator."
42+ struct KalmanCovariances{
43+ NT<: Real ,
44+ # parameters to support both dense and Diagonal matrices (with specialization):
45+ Q̂C<: AbstractMatrix{NT} ,
46+ R̂C<: AbstractMatrix{NT} ,
47+ }
48+ P̂_0:: Hermitian{NT, Matrix{NT}}
49+ P̂:: Hermitian{NT, Matrix{NT}}
50+ Q̂:: Hermitian{NT, Q̂C}
51+ R̂:: Hermitian{NT, R̂C}
52+ invP̄:: Hermitian{NT, Matrix{NT}}
53+ invQ̂_He:: Hermitian{NT, Q̂C}
54+ invR̂_He:: Hermitian{NT, R̂C}
55+ function KalmanCovariances {NT} (
56+ Q̂:: Q ̂C, R̂:: R ̂C, P̂_0, He
57+ ) where {NT<: Real , Q̂C<: AbstractMatrix{NT} , R̂C<: AbstractMatrix{NT} }
58+ if isnothing (P̂_0)
59+ P̂_0 = zeros (NT, 0 , 0 )
60+ end
61+ P̂_0 = Hermitian (P̂_0, :L )
62+ P̂ = copy (P̂_0)
63+ Q̂ = Hermitian (Q̂, :L )
64+ R̂ = Hermitian (R̂, :L )
65+ # the following variables are only for the moving horizon estimator:
66+ invP̄, invQ̂, invR̂ = copy (P̂_0), copy (Q̂), copy (R̂)
67+ try
68+ inv! (invP̄)
69+ catch err
70+ if err isa PosDefException
71+ error (" P̂_0 is not positive definite"
72+ else
73+ rethrow ()
74+ end
75+ end
76+ try
77+ inv! (invQ̂)
78+ catch err
79+ if err isa PosDefException
80+ error (" Q̂ is not positive definite"
81+ else
82+ rethrow ()
83+ end
84+ end
85+ try
86+ inv! (invR̂)
87+ catch err
88+ if err isa PosDefException
89+ error (" R̂ is not positive definite"
90+ else
91+ rethrow ()
92+ end
93+ end
94+ invQ̂_He = repeatdiag (invQ̂, He)
95+ invR̂_He = repeatdiag (invR̂, He)
96+ invQ̂_He = Hermitian (invQ̂_He, :L )
97+ invR̂_He = Hermitian (invR̂_He, :L )
98+ return new {NT, Q̂C, R̂C} (P̂_0, P̂, Q̂, R̂, invP̄, invQ̂_He, invR̂_He)
99+ end
100+ end
101+
102+ " Outer constructor to validate and convert covariance matrices if necessary."
103+ function KalmanCovariances (
104+ model:: SimModel{NT} , i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0= nothing , He= 1
105+ ) where {NT<: Real }
106+ validate_kfcov (model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0)
107+ Q̂, R̂ = NT .(Q̂), NT .(R̂)
108+ ! isnothing (P̂_0) && (P̂_0 = NT .(P̂_0))
109+ return KalmanCovariances {NT} (Q̂, R̂, P̂_0, He)
110+ end
111+
112+ """
113+ validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing)
114+
115+ Validate sizes and Hermitianity of process `Q̂`` and sensor `R̂` noises covariance matrices.
116+
117+ Also validate initial estimate covariance `P̂_0`, if provided.
118+ """
119+ function validate_kfcov (model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0= nothing )
120+ nym = length (i_ym)
121+ nx̂ = model. nx + sum (nint_u) + sum (nint_ym)
122+ size (Q̂) ≠ (nx̂, nx̂) && error (" Q̂ size $(size (Q̂)) ≠ nx̂, nx̂ $((nx̂, nx̂)) "
123+ ! ishermitian (Q̂) && error (" Q̂ is not Hermitian"
124+ size (R̂) ≠ (nym, nym) && error (" R̂ size $(size (R̂)) ≠ nym, nym $((nym, nym)) "
125+ ! ishermitian (R̂) && error (" R̂ is not Hermitian"
126+ if ~ isnothing (P̂_0)
127+ size (P̂_0) ≠ (nx̂, nx̂) && error (" P̂_0 size $(size (P̂_0)) ≠ nx̂, nx̂ $((nx̂, nx̂)) "
128+ ! ishermitian (P̂_0) && error (" P̂_0 is not Hermitian"
129+ end
130+ end
131+
41132@doc raw """
42133 init_estimstoch(model, i_ym, nint_u, nint_ym) -> As, Cs_u, Cs_y, nxs, nint_u, nint_ym
43134
0 commit comments