diff --git a/src/KalmanFilterTools.jl b/src/KalmanFilterTools.jl index 5f6c3f61deb3fc46a4b1b14b31147a56594ddce4..8bb1362926c425c5ca0a0923adcb95b003d47d4f 100644 --- a/src/KalmanFilterTools.jl +++ b/src/KalmanFilterTools.jl @@ -23,11 +23,11 @@ using LinearAlgebra using LinearAlgebra.BLAS export KalmanLikelihoodWs, FastKalmanLikelihoodWs, DiffuseKalmanLikelihoodWs +export KalmanFilterWs, kalman_likelihood, kalman_likelihood_monitored export DiffuseKalmanFilterWs -export KalmanSmootherWs, kalman_likelihood, kalman_likelihood_monitored export fast_kalman_likelihood, diffuse_kalman_likelihood export kalman_filter!, diffuse_kalman_filter! -export kalman_smoother! +export KalmanSmootherWs, DiffuseKalmanSmootherWs, kalman_smoother! abstract type KalmanWs{T, U} end diff --git a/src/kalman_filter.jl b/src/kalman_filter.jl index ad4643afd44f240f5ba95efd6448313148b47685..bc6b71494c3378cb8120df3a8da366d5c745df04 100644 --- a/src/kalman_filter.jl +++ b/src/kalman_filter.jl @@ -1,4 +1,98 @@ # Filters +struct KalmanFilterWs{T, U} <: KalmanWs{T, U} + csmall::Vector{T} + Zsmall::Matrix{T} + # necessary for Z selecting vector with missing variables + iZsmall::Vector{U} + RQ::Matrix{T} + QQ::Matrix{T} + v::Matrix{T} + F::Matrix{T} + cholF::Matrix{T} + cholH::Matrix{T} + iF::Array{T} + iFv::Array{T} + a1::Vector{T} + r::Vector{T} + r1::Vector{T} + at_t::Matrix{T} + K::Array{T} + KDK::Array{T} + L::Matrix{T} + L1::Matrix{T} + N::Matrix{T} + N1::Matrix{T} + ZP::Matrix{T} + Kv::Matrix{T} + iFZ::Matrix{T} + PTmp::Matrix{T} + oldP::Matrix{T} + lik::Vector{T} + KT::Matrix{T} + D::Matrix{T} + ystar::Vector{T} + Zstar::Matrix{T} + Hstar::Matrix{T} + PZi::Vector{T} + tmp_np::Vector{T} + tmp_ns::Vector{T} + tmp_ny::Vector{T} + tmp_ns_np::AbstractArray{T} + tmp_ny_ny::AbstractArray{T} + tmp_ny_ns::AbstractArray{T} + kalman_tol::T + + function KalmanFilterWs{T, U}(ny::U, ns::U, np::U, nobs::U) where {T <: AbstractFloat, U <: Integer} + csmall = Vector{T}(undef, ny) + Zsmall = Matrix{T}(undef, ny, ns) + iZsmall = Vector{U}(undef, ny) + RQ = Matrix{T}(undef, ns, np) + QQ = Matrix{T}(undef, ns, ns) + F = Matrix{T}(undef, ny, ny) + cholF = Matrix{T}(undef, ny, ny) + cholH = Matrix{T}(undef, ny, ny) + iF = Array{T}(undef, ny, ny, nobs) + a1 = Vector{T}(undef, ns) + v = Matrix{T}(undef, ny, nobs) + iFv = Array{T}(undef, ny) + r = zeros(T, ns) + r1 = zeros(T, ns) + at_t = zeros(T, ns, nobs) + K = Array{T}(undef, ny, ns, nobs) + KDK = Array{T}(undef, ns, ny, nobs) + L = Matrix{T}(undef, ns, ns) + L1 = Matrix{T}(undef, ns, ns) + N = zeros(T, ns, ns) + N1 = zeros(T, ns, ns) + Kv = Matrix{T}(undef, ns, nobs) + PTmp = Matrix{T}(undef, ns, ns) + oldP = Matrix{T}(undef, ns, ns) + ZP = Matrix{T}(undef, ny, ns) + iFZ = Matrix{T}(undef, ny, ns) + lik = Vector{T}(undef, nobs) + KT = Matrix{T}(undef, ny, ns) + D = Matrix{T}(undef, ny, ny) + ystar = Vector{T}(undef, ny) + Zstar = Matrix{T}(undef, ny, ns) + Hstar = Matrix{T}(undef, ny, ny) + PZi = Vector{T}(undef, ns) + tmp_np = Vector{T}(undef, np) + tmp_ns = Vector{T}(undef, ns) + tmp_ny = Vector{T}(undef, ny) + tmp_ns_np = Matrix{T}(undef, ns, np) + tmp_ny_ny = Matrix{T}(undef, ny, ny) + tmp_ny_ns = Matrix{T}(undef, ny, ns) + kalman_tol = 1e-12 + + new(csmall, Zsmall, iZsmall, RQ, QQ, v, F, cholF, cholH, iF, + iFv, a1, r, r1, at_t, K, KDK, L, L1, N, N1, ZP, Kv, + iFZ, PTmp, oldP, lik, KT, D, ystar, Zstar, Hstar, PZi, + tmp_np, tmp_ns, tmp_ny, tmp_ns_np, tmp_ny_ny, tmp_ny_ns, + kalman_tol) + end +end + +KalmanFilterWs(ny, ns, np, nobs) = KalmanFilterWs{Float64, Int64}(ny, ns, np, nobs) function kalman_filter!(Y::AbstractArray{U}, c::AbstractArray{U}, @@ -26,7 +120,9 @@ function kalman_filter!(Y::AbstractArray{U}, changeQ = ndims(Q) > 2 changeA = ndims(a) > 1 changeP = ndims(P) > 2 - + changeK = ndims(ws.K) > 2 + changeiFv = ndims(ws.iFv) > 1 + ny = size(Y, 1) nobs = last - start + 1 ns = size(T,1) @@ -59,17 +155,18 @@ function kalman_filter!(Y::AbstractArray{U}, vP = changeP ? view(P, :, :, t) : view(P, :, :) vPtt = changeP ? view(Ptt, :, :, t) : view(Ptt, :, :) vP1 = changeP ? view(P, :, :, t + 1) : view(P, :, :) + vK = changeK ? view(ws.K, 1:ndata, :, t) : view(ws.K, 1:ndata, :) if changeR || changeQ get_QQ!(ws.QQ, vR, vQ, ws.RQ) end + viFv = changeiFv ? view(ws.iFv, 1:ndata, t) : view(ws.iFv, 1:ndata) + vv = view(ws.v, 1:ndata) vF = view(ws.F, 1:ndata, 1:ndata) vvH = view(vH, pattern, pattern) vZP = view(ws.ZP, 1:ndata, :) vcholF = view(ws.cholF, 1:ndata, 1:ndata, 1) - viFv = view(ws.iFv, 1:ndata) - vK = view(ws.K, 1:ndata, :, 1) - + # v = Y[:,t] - c - Z*a get_v!(vv, Y, vc, vZsmall, va, t, pattern) if !steady @@ -106,13 +203,15 @@ function kalman_filter!(Y::AbstractArray{U}, # P = T*Ptt*T + QQ update_P!(vP1, vT, vPtt, ws.QQ, ws.PTmp) ws.oldP .-= vP1 - if norm(ws.oldP) < ns*eps() + if norm(ws.oldP) < 0#ns*eps() steady = true end elseif t > 1 - copy!(vP1, vP) - vPtt1 = view(ws.Ptt, :, : , t-1) - copy!(vPtt, vPtt1) + if changeP + copy!(vP1, vP) + vPtt1 = view(Ptt, :, : , t-1) + copy!(vPtt, vPtt1) + end end end t += 1 @@ -131,7 +230,7 @@ struct DiffuseKalmanFilterWs{T, U} <: KalmanWs{T, U} v::Vector{T} F::Matrix{T} iF::Matrix{T} - iFv::Vector{T} + iFv::Matrix{T} a1::Vector{T} cholF::Matrix{T} cholH::Matrix{T} @@ -163,7 +262,7 @@ struct DiffuseKalmanFilterWs{T, U} <: KalmanWs{T, U} v = Vector{T}(undef, ny) F = Matrix{T}(undef, ny, ny) iF = Matrix{T}(undef, ny,ny ) - iFv = Vector{T}(undef, ny) + iFv = Matrix{T}(undef, ny, nobs) a1 = Vector{T}(undef, ns) cholF = Matrix{T}(undef, ny, ny) cholH = Matrix{T}(undef, ny, ny) diff --git a/src/kalman_smoother.jl b/src/kalman_smoother.jl index 0c2d14e7e0cb6fedfecfd1d2093a157e13ddfec1..9943a8ca52224152d91ad934a7cad09b01721c6f 100644 --- a/src/kalman_smoother.jl +++ b/src/kalman_smoother.jl @@ -1,3 +1,4 @@ +using Test struct KalmanSmootherWs{T, U} <: KalmanWs{T, U} csmall::Vector{T} Zsmall::Matrix{T} @@ -10,7 +11,7 @@ struct KalmanSmootherWs{T, U} <: KalmanWs{T, U} cholF::Matrix{T} cholH::Matrix{T} iF::Array{T} - iFv::Matrix{T} + iFv::Array{T} a1::Vector{T} r::Vector{T} r1::Vector{T} @@ -22,7 +23,6 @@ struct KalmanSmootherWs{T, U} <: KalmanWs{T, U} N::Matrix{T} N1::Matrix{T} ZP::Matrix{T} - Pt_t::Array{T} Kv::Matrix{T} iFZ::Matrix{T} PTmp::Matrix{T} @@ -54,7 +54,7 @@ struct KalmanSmootherWs{T, U} <: KalmanWs{T, U} iF = Array{T}(undef, ny, ny, nobs) a1 = Vector{T}(undef, ns) v = Matrix{T}(undef, ny, nobs) - iFv = Matrix{T}(undef, ny, nobs) + iFv = Array{T}(undef, ny, nobs) r = zeros(T, ns) r1 = zeros(T, ns) at_t = zeros(T, ns, nobs) @@ -65,7 +65,6 @@ struct KalmanSmootherWs{T, U} <: KalmanWs{T, U} N = zeros(T, ns, ns) N1 = zeros(T, ns, ns) Kv = Matrix{T}(undef, ns, nobs) - Pt_t = zeros(T, ns, ns, nobs) PTmp = Matrix{T}(undef, ns, ns) oldP = Matrix{T}(undef, ns, ns) ZP = Matrix{T}(undef, ny, ns) @@ -86,7 +85,7 @@ struct KalmanSmootherWs{T, U} <: KalmanWs{T, U} kalman_tol = 1e-12 new(csmall, Zsmall, iZsmall, RQ, QQ, v, F, cholF, cholH, iF, - iFv, a1, r, r1, at_t, K, KDK, L, L1, N, N1, ZP, Pt_t, Kv, + iFv, a1, r, r1, at_t, K, KDK, L, L1, N, N1, ZP, Kv, iFZ, PTmp, oldP, lik, KT, D, ystar, Zstar, Hstar, PZi, tmp_np, tmp_ns, tmp_ny, tmp_ns_np, tmp_ny_ny, tmp_ny_ns, kalman_tol) @@ -94,134 +93,6 @@ struct KalmanSmootherWs{T, U} <: KalmanWs{T, U} end KalmanSmootherWs(ny, ns, np, nobs) = KalmanSmootherWs{Float64, Int64}(ny, ns, np, nobs) -using Test -function kalman_filter_2!(Y::AbstractArray{U}, - c::AbstractArray{U}, - Z::AbstractArray{W}, - H::AbstractArray{U}, - d::AbstractArray{U}, - T::AbstractArray{U}, - R::AbstractArray{U}, - Q::AbstractArray{U}, - a::AbstractArray{U}, - P::AbstractArray{U}, - start::V, - last::V, - presample::V, - ws::KalmanSmootherWs, - data_pattern::Vector{Vector{V}}) where {U <: AbstractFloat, W <: Real, V <: Integer} - changeC = ndims(c) > 1 - changeZ = ndims(Z) > 2 - changeH = ndims(H) > 2 - changeD = ndims(d) > 1 - changeT = ndims(T) > 2 - changeR = ndims(R) > 2 - changeQ = ndims(Q) > 2 - changeA = ndims(a) > 1 - changeP = ndims(P) > 2 - - if !changeH - get_cholF!(ws.cholH, H) - end - ny = size(Y, 1) - nobs = last - start + 1 - ns = size(T,1) - # QQ = R*Q*R' - vR = view(R, :, :, 1) - vQ = view(Q, :, :, 1) - get_QQ!(ws.QQ, vR, vQ, ws.RQ) - l2pi = log(2*pi) - fill!(ws.lik, 0.0) - t = start - steady = false - vP = view(P, :, :, 1) - cholHset = false -# copy!(ws.oldP, vP) - while t <= last - #inputs - pattern = data_pattern[t] - ndata = length(pattern) - vc = changeC ? view(c, pattern, t) : view(c, pattern) - vZ = changeZ ? view(Z, :, :, t) : view(Z, :, :) - vH = changeH ? view(H, :, :, t) : view(H, :, :) - vT = changeT ? view(T, :, :, t) : view(T, :, :) - vd = changeD ? view(d, :, t) : view(d, :) - if changeR || changeQ - vR = view(R, :, :, t) - vQ = view(Q, :, :, t) - get_QQ!(ws.QQ, vR, vQ, ws.RQ) - end - va = view(a, :, t) - va1 = view(a, :, t + 1) - # outputs - vat = view(ws.at_t, :, t) - vP = view(P, :, :, t) - vP1 = view(P, :, :, t + 1) - vPt = view(ws.Pt_t, :, :, t) - vv = view(ws.v, 1:ndata, t) - vF = view(ws.F, 1:ndata, 1:ndata) - vZP = view(ws.ZP, 1:ndata, :) - vcholF = view(ws.cholF, 1:ndata, 1:ndata) - viF = view(ws.iF, 1:ndata, 1:ndata, t) - viFv = view(ws.iFv, 1:ndata, t) - vK = view(ws.K, 1:ndata, :, t) - vKDK = view(ws.KDK, :, 1:ndata, t) # Kalman Filter Gain according to Durbin & Koopman (4.22) - - # v = Y[:,t] - c - Z*a - get_v!(vv, Y, vc, vZ, va, t, pattern) -# iy += ny -# if !steady - # F = Z*P*Z' + H - # builds also ZP - get_F!(vF, vZP, vZ, vP, vH) - info = get_cholF!(vcholF, vF) - if info != 0 - - # F is near singular - if changeH - get_cholF!(ws.cholH, vH) - end - ws.lik[t] = univariate_step!(Y, t, vZ, vH, vT, ws.QQ, va, vP, ws.kalman_tol, ws) - t += 1 - continue - end -# end - # iFv = inv(F)*v - get_iF!(viF, vcholF) - mul!(viFv,viF,vv) -# get_iFv!(viFv, vcholF, vv) - ws.lik[t] = -.5*ndata*l2pi -.5*log(det_from_cholesky(vcholF)) -.5*LinearAlgebra.dot(vv, viFv) -# if t <= last -# if !steady - # K = iF*ZP - mul!(vK,viF,vZP) - # amounts to K_t in DK (4.22): here KDK = T*K' - mul!(vKDK,vT,transpose(vK)) - # end - # a{t_t} = a_t + K'*v - filtered_a!(vat, va, vd, vK, vv, ws.tmp_ns) - # a_{t+1} = d + T a_{t_t} - copy!(va1, vd) - mul!(va1, vT, vat, 1.0, 1.0) -# if !steady - # P_{t|t} = P_t - K'*Z*P_t - filtered_P!(vPt, vP, vK, vZP, ws.PTmp) - # P_{t+1} = T*P_{t|t}*T'+ QQ - update_P!(vP1, vT, vPt, ws.QQ, ws.PTmp) - -#= - ws.oldP .-= vP1 - if norm(ws.oldP) < ns*eps() - steady = true - else - copy!(ws.oldP, vP) - end -=# -# end -# end - t += 1 - end -end function kalman_smoother!(Y::AbstractArray{U}, c::AbstractArray{U}, @@ -232,7 +103,9 @@ function kalman_smoother!(Y::AbstractArray{U}, R::AbstractArray{U}, Q::AbstractArray{U}, a::AbstractArray{U}, + att::AbstractArray{U}, P::AbstractArray{U}, + Ptt::AbstractArray{U}, alphah::AbstractArray{U}, epsilonh::AbstractArray{U}, etah::AbstractArray{U}, @@ -252,12 +125,15 @@ function kalman_smoother!(Y::AbstractArray{U}, changeT = ndims(T) > 2 changeR = ndims(R) > 2 changeQ = ndims(Q) > 2 -# changeA = ndims(a) > 1 -# changeP = ndims(P) > 2 + changeA = ndims(a) > 1 + changeAtt = ndims(att) > 1 + changeP = ndims(P) > 2 + changePtt = ndims(Ptt) > 2 + + kalman_filter!(Y,c, Z, H, d, T, R, Q, a, att, + P, Ptt, start, last, presample, ws, + data_pattern) - kalman_filter_2!(Y,c, Z, H, d, T, R, Q, a, - P, start, last, presample, ws, - data_pattern) fill!(ws.r1,0.0) fill!(ws.N1,0.0) @@ -268,14 +144,19 @@ function kalman_smoother!(Y::AbstractArray{U}, vZ = changeZ ? view(Z, :, :, t) : view(Z, :, :) vH = changeH ? view(H, :, :, t) : view(H, :, :) vT = changeT ? view(T, :, :, t) : view(T, :, :) - va = view(a, :, t) - vP = view(P, :, :, t) + va = changeA ? view(a, :, t) : view(a, :) + vatt = changeAtt ? view(att, :, t) : view(att, :) + vP = changeP ? view(P, :, :, t) : view(P, :, :) + vPtt = changePtt ? view(Ptt, :, :, t) : view(Ptt, :, :) vQ = changeQ ? view(Q, :, :, t) : view(Q, :, :) vR = changeR ? view(R, :, :, t) : view(R, :, :) - vKDK = view(ws.KDK, :, 1:ndata, t) # amounts to K_t (4.22): here KDK = T*K' + vK = view(ws.K, 1:ndata, :, t) + vKDK = view(ws.KDK, :, 1:ndata, 1) # amounts to K_t (4.22): here KDK = T*K' viF = view(ws.iF, 1:ndata, 1:ndata, t) viFv = view(ws.iFv, 1:ndata, t) + mul!(vKDK, vT, transpose(vK)) + # L_t = T - KDK_t*Z (DK 4.29) get_L!(ws.L, vT, vKDK, vZ, ws.L1) # r_{t-1} = Z_t'*iF_t*v_t + L_t'r_t (DK 4.44) @@ -312,13 +193,10 @@ function kalman_smoother!(Y::AbstractArray{U}, if length(alphah) > 0 valphah = view(alphah, :, t) -# if t==last -# valphah .= va -# else - # alphah_t = a_t + P_t*r_{t-1} (DK 4.44) + # alphah_t = a_t + P_t*r_{t-1} (DK 4.44) get_alphah!(valphah, va, vP, ws.r) -# end end + if length(Valpha) > 0 vValpha = view(Valpha, :, :, t) if t==last @@ -328,18 +206,7 @@ function kalman_smoother!(Y::AbstractArray{U}, get_Valpha!(vValpha, vP, ws.N1, ws.PTmp) end end -#= - if length(alphah) > 0 - valphah = view(alphah, :, t) - # alphah_t = a_t + P_t*r_{t-1} (DK 4.44) - get_alphah!(valphah, va, vP, ws.r) - end - if length(Valpha) > 0 - vValpha = view(Valpha, :, :, t) - # Valpha_t = P_t - P_t*N_{t-1}*P_t (DK 4.44) - get_Valpha!(vValpha, vP, ws.N, ws.PTmp) - end -=# + copy!(ws.r1,ws.r) copy!(ws.N1,ws.N) end diff --git a/test/test_KalmanFilterTools.jl b/test/test_KalmanFilterTools.jl index 7d783f47de964025b6b31b1e5361a62c2e535c4e..6d11a64d013eee177b5910038635641d9bd521d1 100644 --- a/test/test_KalmanFilterTools.jl +++ b/test/test_KalmanFilterTools.jl @@ -63,13 +63,14 @@ H_0 = copy(H) H = zeros(ny, ny) + I(ny) ws1 = KalmanLikelihoodWs(ny, ns, np, nobs) + ws2 = KalmanFilterWs(ny, ns, np, nobs) P = zeros(ns, ns, nobs + 1) s = zeros(ns, nobs + 1) s[:, 1] .= s_0 Ptt = zeros(ns, ns, nobs) stt = zeros(ns, nobs) - llk_1 = kalman_filter!(y, zeros(ny), Z, H, zeros(ns), T, R, Q, s, stt, P, Ptt, 1, nobs, 0, ws1, full_data_pattern) + llk_1 = kalman_filter!(y, zeros(ny), Z, H, zeros(ns), T, R, Q, s, stt, P, Ptt, 1, nobs, 0, ws2, full_data_pattern) @test P[:, :, 2] ≈ R*Q*R' P = zeros(ns, ns) @@ -103,13 +104,14 @@ end H = H'*H ws1 = KalmanLikelihoodWs(ny, ns, np, nobs) + ws2 = KalmanFilterWs(ny, ns, np, nobs) P = zeros(ns, ns, nobs+1) s = zeros(ns, nobs + 1) s[:, 1] .= s_0 Ptt = zeros(ns, ns, nobs) stt = zeros(ns, nobs) - llk_1 = kalman_filter!(y, zeros(ny), Z, H, zeros(ns), T, R, Q, s, stt, P, Ptt, 1, nobs, 0, ws1, full_data_pattern) + llk_1 = kalman_filter!(y, zeros(ny), Z, H, zeros(ns), T, R, Q, s, stt, P, Ptt, 1, nobs, 0, ws2, full_data_pattern) @test P[:, :, 2] ≈ R*Q*R' P = zeros(ns, ns) @@ -208,7 +210,7 @@ end Ptt = similar(P) nobs1 = 1 - ws1 = KalmanLikelihoodWs{Float64, Int64}(ny, ns, np, nobs1) + ws1 = KalmanFilterWs{Float64, Int64}(ny, ns, np, nobs1) kalman_filter!(y, c, Z, H, d, T, R, Q, s, stt, P, Ptt, 1, nobs1, 0, ws1, full_data_pattern) @@ -244,22 +246,24 @@ end ws2 = KalmanSmootherWs{Float64, Int64}(ny, ns, np, nobs) ss1 = zeros(ns, nobs+1) ss1[:, 1] = s_0 + stt = similar(ss1) Ps1 = similar(Ps) Ps1[:, :, 1] = P_0 + Ptt = similar(Ps1) alphah = zeros(ns, nobs) epsilonh = zeros(ny, nobs) etah = zeros(np, nobs) Valpha = zeros(ns, ns, nobs) Vepsilon = zeros(ny, ny, nobs) Veta = zeros(np, np, nobs) - kalman_smoother!(y, cs, Zs, Hs, ds, Ts, Rs, Qs, ss1, Ps1, alphah, epsilonh, etah, Valpha, Vepsilon, Veta, 1, nobs, 0, ws2, full_data_pattern) + kalman_smoother!(y, cs, Zs, Hs, ds, Ts, Rs, Qs, ss1, stt, Ps1, Ptt, alphah, epsilonh, etah, Valpha, Vepsilon, Veta, 1, nobs, 0, ws2, full_data_pattern) @test ss1[:, nobs1 + 1] ≈ s @test Ps1[:, : , nobs1+1] ≈ P for i = 1:nobs Hs[:, :, i] = zeros(ny, ny) end - kalman_smoother!(y, cs, Zs, Hs, ds, Ts, Rs, Qs, ss1, Ps1, alphah, epsilonh, etah, Valpha, Vepsilon, Veta, 1, nobs, 0, ws2, full_data_pattern) + kalman_smoother!(y, cs, Zs, Hs, ds, Ts, Rs, Qs, ss1, stt, Ps1, Ptt, alphah, epsilonh, etah, Valpha, Vepsilon, Veta, 1, nobs, 0, ws2, full_data_pattern) @test y ≈ view(alphah, [4, 3, 2], :) end