Commit 0e080af4 authored by MichelJuillard's avatar MichelJuillard
Browse files

bug corrections

parent b117d301
......@@ -29,8 +29,8 @@ export KalmanLikelihoodWs, FastKalmanLikelihoodWs, DiffuseKalmanLikelihoodWs, Ka
abstract type KalmanWs{T, U} end
struct KalmanLikelihoodWs{T, U} <: KalmanWs{T, U}
csmall::Vector{T}
Zsmall::Matrix{T}
# necessary for Z selecting vector with missing variables
iZsmall::Vector{U}
......@@ -57,6 +57,7 @@ struct KalmanLikelihoodWs{T, U} <: KalmanWs{T, U}
kalman_tol::T
function KalmanLikelihoodWs{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)
......@@ -81,12 +82,14 @@ struct KalmanLikelihoodWs{T, U} <: KalmanWs{T, U}
PZi = Vector{T}(undef, ns)
kalman_tol = 1e-12
new(Zsmall, iZsmall, RQ, QQ, v, F, cholF, cholH, LTcholH,
new(csmall, Zsmall, iZsmall, RQ, QQ, v, F, cholF, cholH, LTcholH,
iFv, a1, K, ZP, iFZ, PTmp, oldP, lik, cholHset, ystar,
Zstar, tmp_ns, PZi, kalman_tol)
end
end
KalmanLikelihoodWs(ny, ns, np, nobs) = KalmanLikelihoodWs{Float64, Integer}(ny, ns, np, nobs)
# Z can be either a matrix or a selection vector
function kalman_likelihood(Y::AbstractArray{U},
Z::AbstractArray{W},
......@@ -122,7 +125,7 @@ function kalman_likelihood(Y::AbstractArray{U},
get_cholF!(ws.cholH, H)
cholHset = true
end
ws.lik[t] = univariate_step!(t, Y, Z, H, T, ws.QQ, a, P, ws.kalman_tol, ws)
ws.lik[t] = univariate_step!(Y, t, Z, H, T, ws.QQ, a, P, ws.kalman_tol, ws)
else
# iFv = inv(F)*v
get_iFv!(ws.iFv, ws.cholF, ws.v)
......@@ -144,8 +147,7 @@ function kalman_likelihood(Y::AbstractArray{U},
LIK
end
function get_vZsmall(ws::KalmanWs, Z::Matrix{T}, pattern::Vector{U}, n::U, ny::U) where {T <: AbstractFloat, U <: Integer}
n = length(pattern)
function get_vZsmall(ws::KalmanWs, Z::AbstractMatrix{T}, pattern::Vector{U}, n::U, ny::U) where {T <: AbstractFloat, U <: Integer}
vZsmall = view(ws.Zsmall, 1:n, :)
if n == ny
copyto!(vZsmall, Z)
......@@ -154,8 +156,7 @@ function get_vZsmall(ws::KalmanWs, Z::Matrix{T}, pattern::Vector{U}, n::U, ny::U
end
end
function get_vZsmall(ws::KalmanWs, Z::Vector{U}, pattern::Vector{U}, n::U, ny::U) where {T <: AbstractFloat, U <: Integer}
n = length(pattern)
function get_vZsmall(ws::KalmanWs, Z::AbstractVector{U}, pattern::Vector{U}, n::U, ny::U) where {T <: AbstractFloat, U <: Integer}
vZsmall = view(ws.iZsmall, 1:n)
if n == ny
copyto!(vZsmall, Z)
......@@ -186,6 +187,7 @@ function kalman_likelihood(Y::AbstractArray{U},
t = start
iy = 1
ncolZ = size(Z, 2)
cholHset = false
@inbounds while t <= last
pattern = data_pattern[t]
ndata = length(pattern)
......@@ -194,6 +196,7 @@ function kalman_likelihood(Y::AbstractArray{U},
vF = view(ws.F, 1:ndata, 1:ndata)
vZP = view(ws.ZP, 1:ndata, :)
vcholF = view(ws.cholF, 1:ndata, 1:ndata)
vcholH = view(ws.cholF, 1:ndata, 1:ndata)
viFv = view(ws.iFv, 1:ndata)
vK = view(ws.K, 1:ndata, :)
vZsmall = get_vZsmall(ws, Z, pattern, ndata, ny)
......@@ -211,7 +214,7 @@ function kalman_likelihood(Y::AbstractArray{U},
get_cholF!(vcholH, vH)
cholHset = true
end
ws.lik[t] = univariate_step!(t, Y, Z, H, T, ws.QQ, a, P, ws.kalman_tol, ws)
ws.lik[t] = univariate_step!(Y, t, Z, H, T, ws.QQ, a, P, ws.kalman_tol, ws)
else
# iFv = inv(F)*v
get_iFv!(viFv, vcholF, vv)
......@@ -271,7 +274,8 @@ function kalman_likelihood_monitored(Y::Matrix{U},
get_cholF!(ws.cholH, H)
cholHset = true
end
ws.lik[t] = univariate_step!(t, Y, Z, H, T, ws.QQ, a, P, ws.kalman_tol, ws)
ws.lik[t] = univariate_step!(Y, t, Z, H, T, ws.QQ, a, P, ws.kalman_tol, ws)
t += 1
continue
end
end
......@@ -328,6 +332,7 @@ function kalman_likelihood_monitored(Y::AbstractArray{U},
iy = 1
steady = false
copy!(ws.oldP, P)
cholHset = false
@inbounds while t <= last
pattern = data_pattern[t]
ndata = length(pattern)
......@@ -347,13 +352,12 @@ function kalman_likelihood_monitored(Y::AbstractArray{U},
get_F!(vF, vZP, Z, P, vH)
info = get_cholF!(ws.cholF, ws.F)
if info != 0
@show info
# F is near singular
if !cholHset
get_cholF!(ws.cholH, H)
cholHset = true
end
ws.lik[t] = univariate_step!(t, Y, Z, H, T, ws.QQ, a, P, ws.kalman_tol, ws)
ws.lik[t] = univariate_step!(Y, t, Z, H, T, ws.QQ, a, P, ws.kalman_tol, ws)
t += 1
continue
end
......@@ -388,6 +392,7 @@ function kalman_likelihood_monitored(Y::AbstractArray{U},
end
struct FastKalmanLikelihoodWs{T, U} <: KalmanWs{T, U}
csmall::Vector{T}
Zsmall::Matrix{T}
iZsmall::Vector{U}
QQ::Matrix{T}
......@@ -410,6 +415,7 @@ struct FastKalmanLikelihoodWs{T, U} <: KalmanWs{T, U}
lik::Vector{T}
function FastKalmanLikelihoodWs{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)
QQ = Matrix{T}(undef, ns, ns)
......@@ -431,7 +437,7 @@ struct FastKalmanLikelihoodWs{T, U} <: KalmanWs{T, U}
iFZW = Matrix{T}(undef, ny, ny)
KtiFZW = Matrix{T}(undef, ns, ny)
lik = Vector{T}(undef, nobs)
new(Zsmall, iZsmall, QQ, v, F, cholF, iFv, a1, K, RQ, ZP, M, W, ZW, ZWM, iFZWM, TW, iFZW, KtiFZW, lik)
new(csmall, Zsmall, iZsmall, QQ, v, F, cholF, iFv, a1, K, RQ, ZP, M, W, ZW, ZWM, iFZWM, TW, iFZW, KtiFZW, lik)
end
end
......@@ -571,6 +577,7 @@ function fast_kalman_likelihood(Y::Matrix{U},
end
struct DiffuseKalmanLikelihoodWs{T, U} <: KalmanWs{T, U}
csmall::Vector{T}
Zsmall::Matrix{T}
iZsmall::Vector{U}
QQ::Matrix{T}
......@@ -593,6 +600,7 @@ struct DiffuseKalmanLikelihoodWs{T, U} <: KalmanWs{T, U}
Kinf_Finf::Vector{T}
lik::Vector{T}
function DiffuseKalmanLikelihoodWs{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)
QQ = Matrix{T}(undef, ns, ns)
......@@ -614,7 +622,7 @@ struct DiffuseKalmanLikelihoodWs{T, U} <: KalmanWs{T, U}
uKstar = Vector{T}(undef, ns)
Kinf_Finf = Vector{T}(undef, ns)
lik = zeros(T, nobs)
new(Zsmall, iZsmall, QQ, RQ, v, F, iF, iFv, a1, cholF, ZP, Fstar, ZPstar, K, iFZ, Kstar, PTmp, uKinf, uKstar, Kinf_Finf, lik)
new(csmall, Zsmall, iZsmall, QQ, RQ, v, F, iF, iFv, a1, cholF, ZP, Fstar, ZPstar, K, iFZ, Kstar, PTmp, uKinf, uKstar, Kinf_Finf, lik)
end
end
......@@ -650,7 +658,7 @@ function diffuse_kalman_likelihood_init!(Y::Matrix{U},
if norm(ws.F) < tol
return t - 1
else
ws.lik[t] += univariate_step(t, Y, Z, H, T, QQ, a, Pinf, Pstar, diffuse_kalman_tol, kalman_tol, ws)
ws.lik[t] += univariate_step(Y, t, Z, H, T, QQ, a, Pinf, Pstar, diffuse_kalman_tol, kalman_tol, ws)
end
else
ws.lik[t] = log(det_from_cholesky(ws.cholF))
......@@ -728,7 +736,7 @@ function diffuse_kalman_likelihood_init!(Y::Matrix{U},
if norm(vF) < tol
return t - 1
else
ws.lik[t] += univariate_step(t, Y, vZsmall, H, T, QQ, a, Pinf, Pstar, diffuse_kalman_tol, kalman_tol, pattern, ws)
ws.lik[t] += univariate_step(Y, t, vZsmall, H, T, QQ, a, Pinf, Pstar, diffuse_kalman_tol, kalman_tol, pattern, ws)
end
else
ws.lik[t] = log(det_from_cholesky(ws.cholF))
......@@ -863,12 +871,15 @@ function kalman_filter!(Y::AbstractArray{U},
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, pattern, t) : view(c, pattern)
vc = changeC ? view(c, :, t) : view(c, :)
ws.csmall .= view(vc, pattern)
vZ = changeZ ? view(Z, :, :, t) : view(Z, :, :)
get_vZsmall(ws, 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, :, :)
......@@ -890,23 +901,22 @@ function kalman_filter!(Y::AbstractArray{U},
vK = view(ws.K, 1:ndata, :, 1)
# v = Y[:,t] - c - Z*a
get_v!(vv, Y, vc, vZ, va, t, pattern)
get_v!(vv, Y, vc, ws.Zsmall, va, t, pattern)
iy += ny
if !steady
# F = Z*P*Z' + H
get_F!(vF, vZP, vZ, vP, vvH)
get_cholF!(vcholF, vF)
info = get_cholF!(ws.cholF, ws.F)
get_F!(vF, vZP, ws.Zsmall, vP, vvH)
info = get_cholF!(vcholF, vF)
if info != 0
@show info
# F is near singular
if !cholHset
get_cholF!(ws.cholH, H)
get_cholF!(ws.cholH, vvH)
cholHset = true
end
ws.lik[t] = univariate_step!(t, Y, Z, H, T, ws.QQ, a, P, ws.kalman_tol, ws)
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)
......@@ -941,6 +951,7 @@ function kalman_filter!(Y::AbstractArray{U},
end
struct KalmanSmootherWs{T, U} <: KalmanWs{T, U}
csmall::Vector{T}
Zsmall::Matrix{T}
# necessary for Z selecting vector with missing variables
iZsmall::Vector{U}
......@@ -974,6 +985,7 @@ struct KalmanSmootherWs{T, U} <: KalmanWs{T, U}
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)
......@@ -1004,13 +1016,15 @@ struct KalmanSmootherWs{T, U} <: KalmanWs{T, U}
tmp_ns_np = Matrix{T}(undef, ns, np)
tmp_ny_ny = Matrix{T}(undef, ny, ny)
new(Zsmall, iZsmall, RQ, QQ, v, F, cholF, iF,
new(csmall, Zsmall, iZsmall, RQ, QQ, v, F, cholF, iF,
iFv, r, r1, a1, K, L, L1, N, N1, ZP, B, Kv,
iFZ, PTmp, oldP, lik, tmp_np, tmp_ns,
tmp_ny, tmp_ns_np, tmp_ny_ny)
end
end
KalmanSmootherWs(ny, ns, np, nobs) = KalmanSmootherWs{Float64, Integer}(ny, ns, np, nobs)
function kalman_filter_2!(Y::AbstractArray{U},
c::AbstractArray{U},
Z::AbstractArray{W},
......@@ -1081,15 +1095,14 @@ function kalman_filter_2!(Y::AbstractArray{U},
if !steady
# F = Z*P*Z' + H
get_F!(vF, vZP, vZ, vP, vH)
info = get_cholF!(ws.cholF, ws.F)
info = get_cholF!(vcholF, ws.F)
if info != 0
@show info
# F is near singular
if !cholHset
get_cholF!(ws.cholH, H)
cholHset = true
end
ws.lik[t] = univariate_step!(t, Y, Z, H, T, ws.QQ, a, P, ws.kalman_tol, ws)
ws.lik[t] = univariate_step!(Y, t, vZ, vH, T, ws.QQ, a, P, ws.kalman_tol, ws)
t += 1
continue
end
......
......@@ -9,21 +9,24 @@ function univariate_step!(t, Y, Z, H, T, QQ, a, P, kalman_tol, ws)
end
end
using Test
function transformed_measurement!(ystar, Zstar, y, Z, cholH)
LTcholH = LowerTriangular(cholH)
copy!(ystar, y)
ldiv!(LTcholH, ystar)
@test ystar inv(LTcholH)*y
copy!(Zstar, Z)
ldiv!(LTcholH, Zstar)
return LTcholH
detLTcholH = 1
for i = 1:length(LTcholH)
detLTcholH *= detLTcholH
end
return detLTcholH
end
function univariate_step!(Y, t, Z, H, T, RQR, a, P, kalman_tol, ws)
ny = size(Y,1)
detLTcholH = 1.0
if !isdiag(H)
LTcholH = transformed_measurement!(ws.ystar, ws.Zstar, view(Y, :, t), Z, ws.cholH)
detLTcholH = transformed_measurement!(ws.ystar, ws.Zstar, view(Y, :, t), Z, ws.cholH)
H = I(ny)
else
copy!(ws.ystar, view(Y, :, t))
......@@ -42,11 +45,11 @@ function univariate_step!(Y, t, Z, H, T, RQR, a, P, kalman_tol, ws)
end
end
mul!(ws.a1, T, a)
copy!(a, ws.a1)
a .= ws.a1
mul!(ws.PTmp, T, P)
copy!(P, RQR)
mul!(P, ws.PTmp, T', 1.0, 1.0)
return llik + 2*log(det(LTcholH))
return llik + 2*log(detLTcholH)
end
function univariate_step(t, Y, Z, H, T, QQ, a, Pinf, Pstar, diffuse_kalman_tol, kalman_tol, ws)
......
......@@ -57,6 +57,82 @@ s = copy(s_0)
end
H_0 = copy(H)
# Singular F matrix
@testset "Singular F matrix diagonal H" begin
H = zeros(ny, ny) + I(ny)
ws1 = KalmanLikelihoodWs(ny, ns, np, nobs)
P = zeros(ns, ns, nobs+1)
s = copy(s_0)
llk_1 = kalman_filter!(y, zeros(ny), Z, H, zeros(ns), T, R, Q, s, P, 1, nobs, 0, ws1, full_data_pattern)
@test P[:, :, 2] R*Q*R'
P = zeros(ns, ns)
s = copy(s_0)
llk_2 = kalman_likelihood(y, Z, H, T, R, Q, s, P, 1, nobs, 0, ws1)
@test llk_2 llk_1
P = zeros(ns, ns)
s = copy(s_0)
llk_3 = kalman_likelihood(y, Z, H, T, R, Q, s, P, 1, nobs, 0, ws1, full_data_pattern)
@test llk_3 llk_1
P = zeros(ns, ns)
s = copy(s_0)
llk_4 = kalman_likelihood_monitored(y, Z, H, T, R, Q, s, P, 1, nobs, 0, ws1)
@test llk_4 llk_1
P = zeros(ns, ns)
s = copy(s_0)
llk_5 = kalman_likelihood_monitored(y, Z, H, T, R, Q, s, P, 1, nobs, 0, ws1, full_data_pattern)
@test llk_5 llk_1
end
@testset "Singular F matrix Full H" begin
H = randn(ny, ny)
H = H'*H
ws1 = KalmanLikelihoodWs(ny, ns, np, nobs)
P = zeros(ns, ns, nobs+1)
s = copy(s_0)
llk_1 = kalman_filter!(y, zeros(ny), Z, H, zeros(ns), T, R, Q, s, P, 1, nobs, 0, ws1, full_data_pattern)
@test P[:, :, 2] R*Q*R'
P = zeros(ns, ns)
s = copy(s_0)
llk_2 = kalman_likelihood(y, Z, H, T, R, Q, s, P, 1, nobs, 0, ws1)
@test llk_2 llk_1
P = zeros(ns, ns)
s = copy(s_0)
llk_3 = kalman_likelihood(y, Z, H, T, R, Q, s, P, 1, nobs, 0, ws1, full_data_pattern)
@test llk_3 llk_1
P = zeros(ns, ns)
s = copy(s_0)
llk_4 = kalman_likelihood_monitored(y, Z, H, T, R, Q, s, P, 1, nobs, 0, ws1)
@test llk_4 llk_1
P = zeros(ns, ns)
s = copy(s_0)
llk_5 = kalman_likelihood_monitored(y, Z, H, T, R, Q, s, P, 1, nobs, 0, ws1, full_data_pattern)
@test llk_5 llk_1
end
H = copy(H_0)
# Fast Kalman Filter
@testset "Fast Kalman Filter" begin
ws1 = KalmanLikelihoodWs{Float64, Integer}(ny, ns, np, nobs)
......@@ -253,3 +329,4 @@ full_data_pattern = [collect(1:ny) for o = 1:nobs]
end
nothing
......@@ -64,12 +64,14 @@ tmp = zeros(np)
KalmanFilterTools.get_etah!(eta, Q, R, r, tmp)
@test eta Q*R'*r
Z = randn(ny, ns)
H = randn(ny, ny)
H = H'*H
F = zeros(ny, ny)
ZP = zeros(ny, ns)
F = randn(ny, ny)
ZP = randn(ny, ns)
P = randn(ns, ns)
KalmanFilterTools.get_F!(F, ZP, Z, P, H)
@test ZP == Z*P
@test ZP Z*P
@test F Z*P*Z' + H
# Z as selection matrix
......
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