Commit 3361be0c authored by MichelJuillard's avatar MichelJuillard
Browse files

update univariate step

parent 5d975ceb
......@@ -33,9 +33,9 @@ export diffuse_kalman_smoother!
abstract type KalmanWs{T, U} end
include("kalman_base.jl")
include("univariate_step.jl")
include("kalman_likelihood.jl")
include("kalman_filter.jl")
include("kalman_smoother.jl")
include("univariate_step.jl")
end #module
......@@ -106,11 +106,11 @@ function kalman_filter!(Y::AbstractArray{U},
att::AbstractArray{U},
P::AbstractArray{U},
Ptt::AbstractArray{U},
start::V,
last::V,
presample::V,
ws::KalmanWs,
data_pattern::Vector{Vector{V}}) where {U <: AbstractFloat, W <: Real, V <: Integer}
start::V,
last::V,
presample::V,
ws::KalmanWs,
data_pattern::Vector{Vector{V}}) where {U <: AbstractFloat, W <: Real, V <: Integer}
changeC = ndims(c) > 1
changeH = ndims(H) > 2
changeD = ndims(d) > 1
......@@ -120,6 +120,7 @@ function kalman_filter!(Y::AbstractArray{U},
changeA = ndims(a) > 1
changeP = ndims(P) > 2
changeK = ndims(ws.K) > 2
changeF = ndims(ws.F) > 2
changeiFv = ndims(ws.iFv) > 1
ny = size(Y, 1)
......@@ -136,7 +137,6 @@ function kalman_filter!(Y::AbstractArray{U},
copy!(ws.oldP, vP)
cholHset = false
while t <= last
pattern = data_pattern[t]
ndata = length(pattern)
vc = changeC ? view(c, :, t) : view(c, :)
......@@ -160,7 +160,7 @@ function kalman_filter!(Y::AbstractArray{U},
viFv = changeiFv ? view(ws.iFv, 1:ndata, t) : view(ws.iFv, 1:ndata)
vv = view(ws.v, 1:ndata, t)
vF = view(ws.F, 1:ndata, 1:ndata)
vF = changeF ? view(ws.F, 1:ndata, 1:ndata, t) : 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, t)
......@@ -321,7 +321,9 @@ function diffuse_kalman_filter_init!(Y::AbstractArray{U},
changeA = ndims(a) > 1
changePinf = ndims(Pinf) > 2
changePstar = ndims(Pstar) > 2
changeFinf = ndims(ws.F) > 2
changeFstar = ndims(ws.Fstar) > 2
ny = size(Y, 1)
t = start
vR = view(R, :, :, 1)
......@@ -358,15 +360,15 @@ function diffuse_kalman_filter_init!(Y::AbstractArray{U},
vvH = view(vH, pattern, pattern)
vcholF = view(ws.cholF, 1:ndata, 1:ndata, t)
viFv = view(ws.iFv, 1:ndata, t)
vFinf = view(ws.F, 1:ndata, 1:ndata)
vFstar = view(ws.Fstar, 1:ndata, 1:ndata)
vFinf = changeFinf ? view(ws.F, 1:ndata, 1:ndata, t) : view(ws.F, 1:ndata, 1:ndata)
vFstar = changeFstar ? view(ws.Fstar, 1:ndata, 1:ndata, t) : view(ws.Fstar, 1:ndata, 1:ndata)
vZPinf = view(ws.ZP, 1:ndata, :)
vZPstar = view(ws.ZPstar, 1:ndata, :)
vcholF = view(ws.cholF, 1:ndata, 1:ndata, t)
vcholH = changeH ? view(ws.cholH, 1:ndata, 1:ndata, t) : view(ws.cholH, 1:ndata, 1:ndata)
viFv = view(ws.iFv, 1:ndata, t)
vKinf = view(ws.Kinf, 1:ndata, :, 1)
vKstar = view(ws.K, 1:ndata, :, 1)
vKinf = view(ws.Kinf, 1:ndata, :, t)
vKstar = view(ws.K, 1:ndata, :, t)
# v = Y[:,t] - c - Z*a
get_v!(vv, Y, vc, vZsmall, va, t, pattern)
......@@ -374,9 +376,10 @@ function diffuse_kalman_filter_init!(Y::AbstractArray{U},
get_F!(vFinf, vZPinf, vZsmall, vPinf)
info = get_cholF!(vcholF, vFinf)
if info > 0
if norm(ws.F) < tol
if norm(vFinf) < tol
return t - 1
else
vcholF[1] = NaN
if !cholHset
get_cholF!(vcholH, vvH)
cholHset = true
......@@ -400,13 +403,16 @@ function diffuse_kalman_filter_init!(Y::AbstractArray{U},
update_a!(va1, vd, vT, vatt)
# Pinf_tt = Pinf - Kinf'*Z*Pinf %(5.14) DK(2012)
get_updated_Ptt!(vPinftt, vPinf, vKinf, vZPinf)
# Pinf = T*Ptt*T
# Pinf = T*Ptt*T'
update_P!(vPinf1, vT, vPinftt, ws.PTmp)
# Pstartt = Pstar-Pstar*Z'*Kinf-Pinf*Z'*Kstar %(5.14) DK(2012)
get_updated_Pstartt!(vPstartt, vPstar, vZPstar, vKinf, vZPinf,
vKstar, vPinftt, ws.PTmp)
# Pinf = T*Ptt*T + QQ
# Pstar = T*Pstartt*T' + QQ
update_P!(vPstar1, vT, vPstartt, ws.QQ, ws.PTmp)
if norm(vPinf1) < tol
return t
end
end
t += 1
end
......
......@@ -128,11 +128,12 @@ function kalman_smoother!(Y::AbstractArray{U},
changeAtt = ndims(att) > 1
changeP = ndims(P) > 2
changePtt = ndims(Ptt) > 2
@show "Entering filter"
kalman_filter!(Y,c, Z, H, d, T, R, Q, a, att,
P, Ptt, start, last, presample, ws,
data_pattern)
@show "Filter return"
fill!(ws.r_1,0.0)
fill!(ws.N_1,0.0)
......@@ -225,9 +226,9 @@ struct DiffuseKalmanSmootherWs{T, U} <: KalmanWs{T, U}
RQ::Matrix{T}
QQ::Matrix{T}
v::Matrix{T}
F::Matrix{T}
F::Array{T}
cholF::Array{T}
Fstar::Matrix{T}
Fstar::Array{T}
cholH::Matrix{T}
iF::Array{T}
iFv::Array{T}
......@@ -279,9 +280,9 @@ struct DiffuseKalmanSmootherWs{T, U} <: KalmanWs{T, U}
iZsmall = Vector{U}(undef, ny)
RQ = Matrix{T}(undef, ns, np)
QQ = Matrix{T}(undef, ns, ns)
F = Matrix{T}(undef, ny, ny)
F = Array{T}(undef, ny, ny, nobs)
cholF = Array{T}(undef, ny, ny, nobs)
Fstar = Matrix{T}(undef, ny, ny)
Fstar = Array{T}(undef, ny, ny, nobs)
cholH = Matrix{T}(undef, ny, ny)
iF = Array{T}(undef, ny, ny, nobs)
a1 = Vector{T}(undef, ns)
......@@ -417,40 +418,62 @@ function diffuse_kalman_smoother_coda!(Y::AbstractArray{U},
vKinf = view(ws.Kinf, 1:ndata, :, t)
vKDKinf = view(ws.KDKinf, :, 1:ndata) # amounts to K_t (5.12): here KDKinf = T*Kinf'
vKDK = view(ws.KDK, :, 1:ndata) # amounts to K_t (5.12): here KDK = T*K'
vFstar =view(ws.Fstar, 1:ndata, 1:ndata, t)
viF = view(ws.iF, 1:ndata, 1:ndata, t)
vcholF = view(ws.cholF, 1:ndata, 1:ndata, t)
viFv = view(ws.iFv, 1:ndata, t)
mul!(vKDKinf, vT, transpose(vKinf))
mul!(vKDK, vT, transpose(vK))
# iFv = Finf \ v
get_iFv!(viFv, vcholF, vv)
# L0_t = T - KDKinf*Z (DK 5.12)
get_L!(L0, vT, vKDKinf, vZsmall)
# L1_t = - KDK*Z (DK 5.12)
get_L1!(L1, vKDK, vZsmall)
# r1_{t-1} = Z_t'*iFinf_t*v_t + L0_t'r1_t + L1_t'*r0_t (DK 5.21)
update_r!(r1, vZsmall, viFv, L0, r1_1, L1, r0_1)
# r0_{t-1} = L0_t'r0_t (DK 5.21)
mul!(r0, L0, r0_1)
if (length(alphah) > 0 ||
length(epsilonh) > 0 ||
length(etah) > 0)
# N0_{t-1} = L0_t'N0_t*L0_t (DK 5.29)
update_N0!(N0, L0, N0_1, ws.PTmp)
# F1 = inv(Finf)
# N1_{t-1} = Z'*F1*Z + L0'*N1_t*L0 + L1'*N0_t*L0
viFZ = view(ws.iFZ, 1:ndata, :)
get_iFZ!(viFZ, vcholF, vZsmall)
update_N1!(N1, vZsmall, viFZ, L0, N1_1, L1, N0_1, ws.PTmp)
# F2 = -inv(Finf)*Fstar*inv(Finv)
# N2_{t-1} = Z'*F2*Z + L0'*N2_t*L0 + L0'*N1_t*L1
# + L1'*N1_t*L0 + L1'*N0_t*L1
vTmp = view(ws.tmp_ny_ns, 1:ndata, :)
update_N2!(N2, viFZ, vFstar, L0, N2_1, N1_1,
L1, N0_1, vTmp, ws.PTmp)
@show vcholF
if isnan(vcholF[1])
vFinf =view(ws.F, 1:ndata, 1:ndata, t)
vFstar =view(ws.Fstar, 1:ndata, 1:ndata, t)
if (length(alphah) > 0 ||
length(epsilonh) > 0 ||
length(etah) > 0)
@show vFinf
@show vFstar
univariate_diffuse_smoother_step!(vT, vFinf, vFstar,
vKinf, vK,
L0, L1, N0, N1, N2,
r0, r1, vv, vZsmall,
ws.kalman_tol, ws)
else
univariate_diffuse_smoother_step!(vT, vFinf, vFstar,
vKinf, vK,
L0, L1, r0, r1, vv,
vZsmall, ws.kalman_tol,
ws)
end
else
# iFv = Finf \ v
get_iFv!(viFv, vcholF, vv)
# L0_t = T - KDKinf*Z (DK 5.12)
get_L!(L0, vT, vKDKinf, vZsmall)
# L1_t = - KDK*Z (DK 5.12)
get_L1!(L1, vKDK, vZsmall)
# r1_{t-1} = Z_t'*iFinf_t*v_t + L0_t'r1_t + L1_t'*r0_t (DK 5.21)
update_r!(r1, vZsmall, viFv, L0, r1_1, L1, r0_1)
# r0_{t-1} = L0_t'r0_t (DK 5.21)
mul!(r0, transpose(L0), r0_1)
if (length(alphah) > 0 ||
length(epsilonh) > 0 ||
length(etah) > 0)
# N0_{t-1} = L0_t'N0_t*L0_t (DK 5.29)
update_N0!(N0, L0, N0_1, ws.PTmp)
# F1 = inv(Finf)
# N1_{t-1} = Z'*F1*Z + L0'*N1_t*L0 + L1'*N0_t*L0
viFZ = view(ws.iFZ, 1:ndata, :)
get_iFZ!(viFZ, vcholF, vZsmall)
update_N1!(N1, vZsmall, viFZ, L0, N1_1, L1, N0_1, ws.PTmp)
# F2 = -inv(Finf)*Fstar*inv(Finv)
# N2_{t-1} = Z'*F2*Z + L0'*N2_t*L0 + L0'*N1_t*L1
# + L1'*N1_t*L0 + L1'*N0_t*L1
vTmp = view(ws.tmp_ny_ns, 1:ndata, :)
vFstar =view(ws.Fstar, 1:ndata, 1:ndata, t)
update_N2!(N2, viFZ, vFstar, L0, N2_1, N1_1,
L1, N0_1, vTmp, ws.PTmp)
end
end
if length(epsilonh) > 0
vepsilonh = view(epsilonh, :, t)
......@@ -481,6 +504,16 @@ function diffuse_kalman_smoother_coda!(Y::AbstractArray{U},
valphah = view(alphah, :, t)
# alphah_t = a_t + Pstar_t*r0_{t-1} + Pinf_t*r1_{t-1} (DK 5.24)
get_alphah!(valphah, va, vPstar, vPinf, r0, r1)
@show t
@show va
@show vPstar
@show vPinf
@show r0
@show r1
@show valphah
@show va + vPstar*r0 + vPinf*r1
@show vPstar*r0
@show vPinf*r1
end
if length(Valpha) > 0
......
......@@ -229,7 +229,7 @@ function univariate_step!(Y, t, c, Z, H, d, T, RQR, a, P, kalman_tol, ws, patter
end
l2pi = log(2*pi)
llik = 0.0
ndata = size(pattern)
ndata = length(pattern)
for i=1:ndata
Zi = view(ws.Zstar, pattern[i], :)
v = get_v!(ws.ystar, c, ws.Zstar, a, pattern[i])
......@@ -241,7 +241,7 @@ function univariate_step!(Y, t, c, Z, H, d, T, RQR, a, P, kalman_tol, ws, patter
llik += ndata*l2pi + log(F) + v*v/F
end
end
copy!(ws.a1, c)
copy!(ws.a1, d)
mul!(ws.a1, T, a, 1.0, 1.0)
a .= ws.a1
mul!(ws.PTmp, T, P)
......@@ -250,7 +250,7 @@ function univariate_step!(Y, t, c, Z, H, d, T, RQR, a, P, kalman_tol, ws, patter
return llik + 2*log(detLTcholH)
end
function univariate_step(Y, t, c, Z, H, d, T, QQ, a, Pinf, Pstar, diffuse_kalman_tol, kalman_tol, ws)
function univariate_step(Y, t, c, Z, H, d, T, RQR, a, Pinf, Pstar, diffuse_kalman_tol, kalman_tol, ws)
ny = size(Y,1)
detLTcholH = 1.0
if !isdiag(H)
......@@ -263,9 +263,11 @@ function univariate_step(Y, t, c, Z, H, d, T, QQ, a, Pinf, Pstar, diffuse_kalman
llik = 0.0
for i=1:size(Y,1)
Zi = view(Z, i, :)
v = get_v!(ws.ystar, c, ws.Zstar, a, i)
Fstar = get_Fstar!(Zi, Pstar, H[i], ws.uKstar)
Finf = get_Finf!(Zi, Pstar, ws.uKstar)
uKinf = view(ws.Kinf, i, :, t)
uKstar = view(ws.K, i, :, t)
ws.v[i] = get_v!(ws.ystar, c, ws.Zstar, a, i)
Fstar = get_Fstar!(Zi, Pstar, H[i], uKstar)
Finf = get_Finf!(Zi, Pinf, uKinf)
# Conduct check of rank
# Pinf and Finf are always scaled such that their norm=1: Fstar/Pstar, instead,
# depends on the actual values of std errors in the model and can be badly scaled.
......@@ -273,30 +275,35 @@ function univariate_step(Y, t, c, Z, H, d, T, QQ, a, Pinf, Pstar, diffuse_kalman
# exiting the diffuse filter properly, avoiding tests that provide false non-zero rank for Pinf.
# Also the test for singularity is better set coarser for Finf than for Fstar for the same reason
if Finf > diffuse_kalman_tol # F_{\infty,t,i} = 0, use upper part of bracket on p. 175 DK (2012) for w_{t,i}
ws.Kinf_Finf .= ws.uKinf./Finf
a .+= ws.v[i].*ws.Kinf_Finf
ws.Kinf_Finf .= uKinf./Finf
a .+= ws.v[i]*ws.Kinf_Finf
# Pstar = Pstar + Kinf*(Kinf_Finf'*(Fstar/Finf)) - Kstar*Kinf_Finf' - Kinf_Finf*Kstar'
ger!( Fstar/Finf, ws.uKinf, ws.Kinf_Finf, Pstar)
ger!( -1.0, ws.uKstar, ws.Kinf_Finf, Pstar)
ger!( -1.0, ws.Kinf_Finf, ws.uKstar, Pstar)
ger!( Fstar/Finf, uKinf, ws.Kinf_Finf, Pstar)
ger!( -1.0, uKstar, ws.Kinf_Finf, Pstar)
ger!( -1.0, ws.Kinf_Finf, uKstar, Pstar)
# Pinf = Pinf - Kinf*Kinf_Finf'
ger!(-1.0, ws.uKinf, ws.Kinf_Finf, Pinf)
ger!(-1.0, uKinf, ws.Kinf_Finf, Pinf)
llik += log(Finf)
elseif Fstar > kalman_tol
llik += log(Fstar) + ws.v[i]*ws.v[i]/Fstar
a .+= ws.uKstar.*(ws.v[i]/Fstar)
ger!(-1/Fstar, ws.uKstar, ws.uKstar, Pstar)
a .+= uKstar.*(ws.v[i]/Fstar)
ger!(-1/Fstar, uKstar, uKstar, Pstar)
else
# do nothing as a_{t,i+1}=a_{t,i} and P_{t,i+1}=P_{t,i}, see
# p. 157, DK (2012)
end
ws.F[i, i, t] = Finf
@show Fstar
ws.Fstar[i, i, t] = Fstar
end
copy!(ws.a1, d)
mul!(ws.a1, T, a, 1.0, 1.0)
a .= ws.a1
mul!(ws.PTmp, T, P)
copy!(P, RQR)
mul!(P, ws.PTmp, T', 1.0, 1.0)
mul!(ws.PTmp, T, Pinf)
mul!(Pinf, ws.PTmp, T')
mul!(ws.PTmp, T, Pstar)
copy!(Pstar, RQR)
mul!(Pstar, ws.PTmp, T', 1.0, 1.0)
return llik + 2*log(detLTcholH)
end
......@@ -314,10 +321,12 @@ function univariate_step(Y, t, c, Z, H, d, T, QQ, a, Pinf, Pstar, diffuse_kalman
l2pi = log(2*pi)
ndata = length(pattern)
for i=1:ndata
uKinf = view(ws.Kinf, i, :, t)
uKstar = view(ws.K, i, :, t)
Zi = view(ws.Zstar, pattern[i], :)
v = get_v!(ws.ystar, c, ws.Zstar, a, pattern[i])
Fstar = get_Fstar!(Zi, Pstar, H[i], ws.uKstar)
Finf = get_Finf!(Zi, Pstar, ws.uKstar)
ws.v[i] = get_v!(ws.ystar, c, ws.Zstar, a, pattern[i])
Fstar = get_Fstar!(Zi, Pstar, H[i], uKstar)
Finf = get_Finf!(Zi, Pinf, uKinf)
# Conduct check of rank
# Pinf and Finf are always scaled such that their norm=1: Fstar/Pstar, instead,
# depends on the actual values of std errors in the model and can be badly scaled.
......@@ -325,24 +334,29 @@ function univariate_step(Y, t, c, Z, H, d, T, QQ, a, Pinf, Pstar, diffuse_kalman
# exiting the diffuse filter properly, avoiding tests that provide false non-zero rank for Pinf.
# Also the test for singularity is better set coarser for Finf than for Fstar for the same reason
if Finf > diffuse_kalman_tol # F_{\infty,t,i} = 0, use upper part of bracket on p. 175 DK (2012) for w_{t,i}
ws.Kinf_Finf .= ws.uKinf./Finf
a .+= v .* ws.Kinf_Finf
ws.Kinf_Finf .= uKinf./Finf
a .+= ws.v[i] .* ws.Kinf_Finf
# Pstar = Pstar + Kinf*(Kinf_Finf'*(Fstar/Finf)) - Kstar*Kinf_Finf' - Kinf_Finf*Kstar'
ger!( Fstar/Finf, ws.uKinf, ws.Kinf_Finf, Pstar)
ger!( -1.0, ws.uKstar, ws.Kinf_Finf, Pstar)
ger!( -1.0, ws.Kinf_Finf, ws.uKstar, Pstar)
ger!( Fstar/Finf, uKinf, ws.Kinf_Finf, Pstar)
ger!( -1.0, uKstar, ws.Kinf_Finf, Pstar)
ger!( -1.0, ws.Kinf_Finf, uKstar, Pstar)
# Pinf = Pinf - Kinf*Kinf_Finf'
ger!(-1.0, ws.uKinf, ws.Kinf_Finf, Pinf)
ger!(-1.0, uKinf, ws.Kinf_Finf, Pinf)
llik += ndata*l2pi + log(Finf)
elseif Fstar > kalman_tol
llik += log(Fstar) + v*v/Fstar
a .+= ws.uKstar.*(v/Fstar)
ger!(-1/Fstar, ws.uKstar, ws.uKstar, Pstar)
llik += log(Fstar) + ws.v[i]*ws.v[i]/Fstar
a .+= uKstar.*(ws.v[i]/Fstar)
ger!(-1/Fstar, uKstar, uKstar, Pstar)
else
# do nothing as a_{t,i+1}=a_{t,i} and P_{t,i+1}=P_{t,i}, see
# p. 157, DK (2012)
end
ws.F[i, i, t] = Finf
ws.Fstar[i, i, t] = Fstar
end
@show t
@show ws.F[:,:,t]
@show ws.Fstar[:,:,t]
copy!(ws.a1, d)
mul!(ws.a1, T, a, 1.0, 1.0)
a .= ws.a1
......@@ -353,3 +367,168 @@ function univariate_step(Y, t, c, Z, H, d, T, QQ, a, Pinf, Pstar, diffuse_kalman
mul!(Pstar, ws.PTmp, transpose(T), 1.0, 1.0)
return llik + 2*log(detLTcholH)
end
function univariate_diffuse_smoother_step!(T::AbstractMatrix{W},
Finf::AbstractMatrix{W},
Fstar::AbstractMatrix{W},
Kinf::AbstractMatrix{W},
Kstar::AbstractMatrix{W},
L0::AbstractMatrix{W},
L1::AbstractMatrix{W},
N0::AbstractMatrix{W},
N1::AbstractMatrix{W},
N2::AbstractMatrix{W},
r0::AbstractVector{W},
r1::AbstractVector{W},
v::AbstractVector{W},
Z::AbstractArray{U},
tol::W,
ws::DiffuseKalmanSmootherWs) where {W <: AbstractFloat,
U <: Real}
ny = size(Finf, 1)
ns = size(L0, 1)
for i = ny: -1: 1
@show Finf[i,i]
@show Fstar[i,i]
if Finf[i, i] > tol
iFinf = 1/Finf[i, i]
vKinf = view(Kinf, i, :)
vKstar = view(Kstar, i, :)
# get_L0!(L0, Kinf, Z, Finf, i)
copy!(L0, I(ns))
vZ = view(Z, i, :)
ger!(-iFinf, vKinf, vZ, L0)
# get_L1!(L1, Kinf, Kstar, Finf, Fstar, Z, i)
fill!(L1, 0.0)
lmul!(Fstar[i, i]/Finf[i, i], vKinf)
vKinf .-= vKstar
ger!(iFinf, vKinf, vZ, L1)
@show L1
# compute r1_{t,i-1} first because it depends
# upon r0{t,i}
# update_r1!(r1, r0, Z, v, Finv, L0, L1, i)
copy!(ws.tmp_ns, vZ)
rmul!(ws.tmp_ns, v[i]*iFinf)
mul!(ws.tmp_ns, transpose(L1), r0, 1.0, 1.0)
mul!(ws.tmp_ns, transpose(L0), r1, 1.0, 1.0)
copy!(r1, ws.tmp_ns)
# r0(:,t) = Linf'*r0(:,t); % KD (2000), eq. (25) for r_0
copy!(ws.tmp_ns, r0)
mul!(r0, transpose(L0), ws.tmp_ns)
# update_N2!(N2, viFZ, vFstar, L0, N2_1, N1_1,
# L1, N0_1, vTmp, ws.PTmp)
mul!(ws.PTmp, transpose(L0), N2)
mul!(ws.PTmp, transpose(L1), N1, 1.0, 1.0)
mul!(N2, ws.PTmp, L0)
mul!(ws.PTmp, transpose(L0), N1)
mul!(ws.PTmp, transpose(L1), N0, 1.0, 1.0)
mul!(N2, ws.PTmp, L1, 1.0, 1.0)
ger!(Fstar[i]/(Finf[i]*Finf[i]),vZ, vZ, N2)
# update_N1!(N1, vZ, Finf, L0, N1, L1, N0, ws.PTmp)
mul!(ws.PTmp, transpose(L0), N1)
mul!(ws.PTmp, transpose(L1), N0, 1.0, 1.0)
mul!(N1, ws.PTmp, L0)
ger!(iFinf, vZ, vZ, N1)
# update_N0!(N0, L1, ws.PTmp)
mul!(ws.PTmp, transpose(L0), N0)
mul!(N0, ws.PTmp, L0)
elseif Fstar[i, i] > tol
iFstar = 1/Fstar[i, i]
# get_L0!(L1, Kstar, Z, Fstar, i)
copy!(L0, I(ns))
vKstar = view(Kstar, i, :)
vZ = view(Z, i, :)
ger!(-iFstar, vKstar, vZ, L0)
# update_r0!(r0, Z, Fstar, v, L0, i)
copy!(ws.tmp_ns, r0)
r0 .= vZ
rmul!(r0, v[i]*iFstar)
@show r0
@show ws.tmp_ns
mul!(r0, transpose(L0), ws.tmp_ns, 1.0, 1.0)
# update_N0!(N0, L1, ws.PTmp)
mul!(ws.PTmp, transpose(L0), N0)
mul!(N0, ws.PTmp, L0)
ger!(iFstar, vZ, vZ, N0)
end
end
#=
copy!(ws.tmp_ns, r0)
mul!(r0, transpose(T), ws.tmp_ns)
copy!(ws.tmp_ns, r1)
mul!(r1, transpose(T), ws.tmp_ns)
mul!(ws.PTmp, transpose(T), N0)
mul!(N0, ws.PTmp, T)
mul!(ws.PTmp, transpose(T), N1)
mul!(N1, ws.PTmp, T)
mul!(ws.PTmp, transpose(T), N2)
mul!(N2, ws.PTmp, T)
=#
end
function univariate_diffuse_smoother_step!(T::AbstractMatrix{W},
Finf::AbstractMatrix{W},
Fstar::AbstractMatrix{W},
Kinf::AbstractMatrix{W},
Kstar::AbstractMatrix{W},
L0::AbstractMatrix{W},
L1::AbstractMatrix{W},
r0::AbstractVector{W},
r1::AbstractVector{W},
v::AbstractVector{W},
Z::AbstractArray{U},
tol::W,
ws::DiffuseKalmanSmootherWs) where {W <: AbstractFloat,
U <: Real}
ny = size(Finf, 1)
ns = size(L0, 1)
for i = 1: ny
if Finf[i, i] > tol
iFinf = 1/Finf[i,i]
# get_L1!(L1, Kinf, Z, Finf, i)
copy!(L1,I(ns))
vKinf = view(Kinf, :, i)
vZ = view(Z, i, :)
ger!(-iFinf, vKinf, vZ, L1)
# get_L0!(L0, Kinf, Kstar, Finf, Fstar, Z, i)
fill!(L0, 0.0)
vFstar = view(Fstar, i, :)
lmul!(Fstar[i]*Finf, vKinf)
vKinf .-= vKstar
ger!(1.0, vKstar, vZ, L0)
# update_r1!(r1, r0, Z, v, Finv, L0, L1, i)
r1 .= vZ
rmul!(r1, v[i]*iFinf)
mul!(r1, transpose(L0), r0, 1.0, 1.0)
mul!(r1, transpose(L1), r1, 1.0, 1.0)
# r0(:,t) = Linf'*r0(:,t); % KD (2000), eq. (25) for r_0
copy!(ws.tmp_ns, r0)
mul!(r0, transpose(L0), ws.tmp_ns)
elseif Fstar[i, i] > tol
iFstar = 1/Fstar[i,i]
# get_L1!(L1, Kstar, Z, Fstar, i)
copy!(L1, I(ny))
vKstar = view(Kstar, :, i)
vZ = view(Z, i, :)
ger!(-iFstar, vKstar, vZ)
# update_r0!(r0, Z, Fstar, v, L1, i)
r0 .= vZ
rmul!(r0, v[i]*iFstar)
mul!(r0, transpose(L1), r0, 1.0, 1.0)
end
end
#=
copy!(ws.tmp_ns, r0)
mul!(r0, transpose(T), ws.tmp_ns)
copy!(ws.tmp_ns, r1)
mul!(r1, transpose(T), ws.tmp_ns)
mul!(ws.PTmp, transpose(T), N0)
mul!(N0, ws.PTmp, T)
mul!(ws.PTmp, transpose(T), N1)
mul!(N1, ws.PTmp, T)
mul!(ws.PTmp, transpose(T), N2)
mul!(N2, ws.PTmp, T)
=#
end
......@@ -325,7 +325,8 @@ full_data_pattern = [collect(1:ny) for o = 1:nobs]
@test llk_3 -0.5*sum(ws5.lik[1:t1])
@test aa[:, t1 + 1] vars["a"]
@test PPstar[:, :, t1 + 1] vars["Pstar1"]
display(PPstar[:,:,t1+1])
display(vars["Pstar1"])
z = [4, 3]
a = copy(a_0)
Pinf = copy(Pinf_0)
......
using LinearAlgebra
using LinearAlgebra.BLAS
using KalmanFilterTools
using MAT