Commit 27d99973 by Michel Juillard

### restructure KalmanFilterTools

parent 0edf10f0
This diff is collapsed.
 # Filters function kalman_filter!(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::KalmanWs, 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 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) copy!(ws.oldP, vP) cholHset = false @inbounds while t <= last pattern = data_pattern[t] ndata = length(pattern) vc = changeC ? view(c, :, t) : view(c, :) ws.csmall .= view(vc, pattern) vZ = changeZ ? view(Z, :, :, t) : view(Z, :, :) vZsmall = get_vZsmall(ws.Zsmall, ws.iZsmall, vZ, pattern, ndata, ny) vH = changeH ? view(H, :, :, t) : view(H, :, :) vT = changeT ? view(T, :, :, t) : view(T, :, :) vR = changeR ? view(R, :, :, t) : view(R, :, :) vQ = changeQ ? view(Q, :, :, t) : view(Q, :, :) va = changeA ? view(a, :, t) : view(a, :) va1 = changeA ? view(a, :, t + 1) : view(a, :) vd = changeD ? view(d, :, t) : view(d, :) vP = changeP ? view(P, :, :, t) : view(P, :, :) vP1 = changeP ? view(P, :, :, t + 1) : view(P, :, :) if changeR || changeQ get_QQ!(ws.QQ, vR, vQ, ws.RQ) end 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) iy += ny if !steady # F = Z*P*Z' + H get_F!(vF, vZP, vZsmall, vP, vvH) info = get_cholF!(vcholF, vF) if info != 0 # F is near singular if !cholHset get_cholF!(ws.cholH, vvH) cholHset = true end ws.lik[t] = univariate_step!(Y, t, ws.Zsmall, vvH, T, ws.QQ, va, vP, ws.kalman_tol, ws) t += 1 continue end end # iFv = inv(F)*v get_iFv!(viFv, vcholF, vv) ws.lik[t] = ndata*l2pi + log(det_from_cholesky(vcholF)) + LinearAlgebra.dot(vv, viFv) if t <= last if !steady # K = iF*Z*P get_K!(vK, vZP, vcholF) end # a = d + T(a + K'*v) update_a!(va1, va, vd, vK, vv, ws.a1, vT) if !steady # P = T*(P - K'*Z*P)*T'+ QQ copy!(vP1, vP) update_P!(vP1, vT, ws.QQ, vK, vZP, ws.PTmp) ws.oldP .-= vP1 if norm(ws.oldP) < ns*eps() steady = true else copy!(ws.oldP, vP) end end end t += 1 end @inbounds if presample > 0 LIK = -0.5*(sum(view(ws.lik,(presample+1):nobs))) else LIK = -0.5*(sum(ws.lik)) end LIK end
This diff is collapsed.
 struct KalmanSmootherWs{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::Matrix{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} Pt_t::Array{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 KalmanSmootherWs{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 = Matrix{T}(undef, ny, nobs) 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) 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) 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, Pt_t, 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 KalmanSmootherWs(ny, ns, np, nobs) = KalmanSmootherWs{Float64, Int64}(ny, ns, np, nobs) 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 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 # outputs vat = view(ws.at_t, :, t) 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 !cholHset get_cholF!(ws.cholH, H) cholHset = true end ws.lik[t] = univariate_step!(Y, t, vZ, vH, vT, ws.QQ, a, P, 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} = d + a_t + K'*v filtered_a!(vat, va, vd, vK, vv, ws.tmp_ns) # a_{t+1} = T a_{t_t} mul!(va,vT,vat) # 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!(vP, vPt, vT, 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}, Z::AbstractArray{W}, H::AbstractArray{U}, d::AbstractArray{U}, T::AbstractArray{U}, R::AbstractArray{U}, Q::AbstractArray{U}, a::AbstractArray{U}, P::AbstractArray{U}, alphah::AbstractArray{U}, epsilonh::AbstractArray{U}, etah::AbstractArray{U}, Valpha::AbstractArray{U}, Vepsilon::AbstractArray{U}, Veta::AbstractArray{U}, start::X, last::X, presample::X, ws::KalmanSmootherWs, data_pattern::Vector{Vector{X}}) where {U <: AbstractFloat, W <: Real, X <: 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 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) for t = last: -1: 1 #inputs pattern = data_pattern[t] ndata = length(pattern) vZ = changeZ ? view(Z, :, :, t) : view(Z, :, :) vH = changeH ? view(H, :, :, t) : view(H, :, :) vT = changeT ? view(T, :, :, t) : view(T, :, :) va = view(ws.at_t, :, t) vP = view(ws.Pt_t, :, :, t) 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' viF = view(ws.iF, 1:ndata, 1:ndata, t) viFv = view(ws.iFv, 1:ndata, t) # 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) update_r!(ws.r, vZ, viFv, ws.L, ws.r1) if (length(alphah) > 0 || length(epsilonh) > 0 || length(etah) > 0) # N_{t-1} = Z_t'iF_t*Z_t + L_t'N_t*L_t (DK 4.44) mul!(ws.iFZ,viF,vZ) update_N!(ws.N, vZ, ws.iFZ, ws.L, ws.N1, ws.PTmp) end if length(epsilonh) > 0 vepsilonh = view(epsilonh, :, t) # epsilon_t = H*(iF_t*v_t - KDK_t'*r_t) (DK 4.69) get_epsilonh!(vepsilonh, vH, viFv, vKDK, ws.r1, ws.tmp_ny, ws.tmp_ns) if length(Vepsilon) > 0 vVepsilon = view(Vepsilon,:,:,t) # D_t = inv(F_t) + KDK_t'*N_t*KDK_t (DK 4.69) get_D!(ws.D, viF, vKDK, ws.N1, ws.tmp_ny_ns) # Vepsilon_t = H - H*D_t*H (DK 4.69) get_Vepsilon!(vVepsilon, vH, ws.D, ws.tmp_ny_ny) end end if length(etah) > 0 vetah = view(etah, :, t) # eta_t = Q*R'*r_t (DK 4.69) get_etah!(vetah, vQ, vR, ws.r1, ws.tmp_np) if length(Veta) > 0 vVeta = view(Veta, :, :, t) # Veta_t = Q - Q*R'*N_t*R*Q (DK 4.69) get_Veta!(vVeta, vQ, vR, ws.N1, ws.RQ, ws.tmp_ns_np) end end 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) get_alphah!(valphah, va, vP, ws.r1) end end if length(Valpha) > 0 vValpha = view(Valpha, :, :, t) if t==last copy!(vValpha,vP) else # Valpha_t = P_t - P_t*N_{t-1}*P_t (DK 4.44) 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 end
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!