Skip to content

GitLab

Commit 9a404ebe authored by Michel Juillard's avatar Michel Juillard
Browse files

removed kalman_filter_2!()

parent 250d204c
Hide whitespace changes
Inline Side-by-side
src/KalmanFilterTools.jl
View file @ 9a404ebe
......@@ -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
......
src/kalman_filter.jl
View file @ 9a404ebe
# 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)
......
src/kalman_smoother.jl
View file @ 9a404ebe
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
......
test/test_KalmanFilterTools.jl
View file @ 9a404ebe
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment