Commit a3df8a90 authored by MichelJuillard's avatar MichelJuillard
Browse files

fixed kalman base and test

parent f4234412
......@@ -6,17 +6,6 @@ function det_from_cholesky(achol::AbstractMatrix{T}) where T <: AbstractFloat
x*x
end
function det_from_cholesky(achol::AbstractMatrix{T}, tol::T) where T <: AbstractFloat
x = 1.0
@inbounds @simd for i = 1:size(achol,1)
a = abs(achol[i,i])
if a > tol
x *= achol[i,i]
end
end
x*x
end
# alphah_t = a_t + P_t*r_{t-1}
function get_alphah!(alphah::AbstractVector{T}, a::AbstractVector{T}, P::AbstractArray{T}, r::AbstractVector{T}) where T <: AbstractFloat
alphah .= a
......@@ -25,14 +14,15 @@ end
function get_cholF!(cholF::AbstractArray{T}, F::AbstractArray{T}) where T <: AbstractFloat
cholF .= 0.5.*(F .+ transpose(F))
LAPACK.potrf!('U', cholF)
info = LAPACK.potrf!('U', cholF)
return info[2]
end
# D = inv(F_t) + K_t*T*N_t*T'*K'
# D = inv(F_t) + K_t*T*N_t*T'*K_t'
function get_D!(D, cholF, K, T, N, KT, tmp)
copy!(D, cholF)
LAPACK.potri!('U', D)
# complete lower trianle and change sign of entire matrix
# complete lower triangle
n = size(D,1)
@inbounds for i = 1:n
@simd for j = i:n
......@@ -64,6 +54,12 @@ function get_etah!(etah::AbstractVector{T}, Q::AbstractMatrix{T},
mul!(etah, Q, tmp)
end
function get_F(Zi, P, h, tmp)
mul!(tmp, P, Zi)
F = dot(Zi, tmp) + h
return F
end
function get_F!(f::AbstractArray{T}, zp::AbstractArray{T}, z::AbstractArray{T}, p::AbstractArray{T}) where T <: AbstractFloat
mul!(zp, z, p)
mul!(f, zp, transpose(z))
......@@ -74,14 +70,18 @@ function get_F!(f::AbstractArray{T}, zp::AbstractArray{T}, z::AbstractVector{U},
f .= view(zp, :, z)
end
function get_Fstar!(z::AbstractVector{T}, p::AbstractArray{T}, h::T, ukstar::AbstractVector{T}) where T <: AbstractFloat
mul!(ukstar, p, z)
Fstar = BLAS.dot(z, ukstar) + h # F_{*,t} in 5.7 in DK (2012), relies on H being diagonal
# F = Z*P*Z' + H
function get_F!(f::AbstractArray{T}, zp::AbstractArray{T}, z::AbstractArray{T}, p::AbstractArray{T}, h::AbstractArray{T}) where T <: AbstractFloat
copy!(f, h)
mul!(zp, z, p)
gemm!('N', 'T', 1.0, zp, z, 1.0, f)
end
function get_Fstar!(z::U, p::AbstractArray{T}, h::T, ukstar::AbstractVector{T}) where {T <: AbstractFloat, U <: Integer}
ukstar .= view(p, :, z)
Fstar = BLAS.dot(z, ukstar) + h # F_{*,t} in 5.7 in DK (2012), relies on H being diagonal
# F = P(z,z) + H
function get_F!(f::AbstractArray{T}, zp::AbstractArray{T}, z::AbstractVector{I}, p::AbstractArray{T}, h::AbstractArray{T}) where {I <: Integer, T <: AbstractFloat}
copy!(f, h)
zp .= view(p, z, :)
f .+= view(zp, :, z)
end
function get_Finf!(z::AbstractVector{T}, p::AbstractArray{T}, ukinf::AbstractVector{T}) where T <: AbstractFloat
......@@ -94,16 +94,37 @@ function get_Finf!(z::U, p::AbstractArray{T}, ukinf::AbstractVector{T}) where {T
Finf = BLAS.dot(z, ukinf) # F_{\infty,t} in 5.7 in DK (2012), relies on H being diagonal
end
function get_Fstar!(z::AbstractVector{T}, p::AbstractArray{T}, h::T, ukstar::AbstractVector{T}) where T <: AbstractFloat
mul!(ukstar, p, z)
Fstar = BLAS.dot(z, ukstar) + h # F_{*,t} in 5.7 in DK (2012), relies on H being diagonal
end
function get_Fstar!(z::U, p::AbstractArray{T}, h::T, ukstar::AbstractVector{T}) where {T <: AbstractFloat, U <: Integer}
ukstar .= view(p, :, z)
Fstar = BLAS.dot(z, ukstar) + h # F_{*,t} in 5.7 in DK (2012), relies on H being diagonal
end
function get_iF!(iF::AbstractArray{T}, cholF::AbstractArray{T}) where T <: AbstractFloat
copy!(iFZ, Z)
LAPACK.potri!('U', cholF)
end
function get_iFv!(iFv::AbstractVector{T}, cholF::AbstractArray{T}, v::AbstractVector{T}) where T <: AbstractFloat
iFv .= v
LAPACK.potrs!('U', cholF, iFv)
end
function get_iFZ!(iFZ::AbstractArray{T}, cholF::AbstractArray{T}, Z::AbstractArray{T}) where T <: AbstractFloat
copy!(iFZ, Z)
LAPACK.potrs!('U', cholF, iFZ)
end
# K = iF*Z*P
function get_K!(K::AbstractArray{T}, ZP::AbstractArray{T}, cholF::AbstractArray{T}) where T <: AbstractFloat
copy!(K, ZP)
LAPACK.potrs!('U', cholF, K)
end
function get_Kstar!(Kstar::AbstractArray{T}, Z::AbstractArray{T}, Pstar::AbstractArray{T}, Fstar::AbstractArray{T}, K::AbstractArray{T}, cholF::AbstractArray{T}) where T <: AbstractFloat
mul!(Kstar, Z, Pstar)
gemm!('N', 'N', -1.0, Fstar, K, 1.0, Kstar)
......@@ -142,20 +163,19 @@ function get_L!(L::AbstractArray{U}, T::AbstractArray{U}, K::AbstractArray{U}, z
mul!(L, T, L1)
end
function get_iFv!(iFv::AbstractVector{T}, cholF::AbstractArray{T}, v::AbstractVector{T}) where T <: AbstractFloat
iFv .= v
LAPACK.potrs!('U', cholF, iFv)
end
# K = iF*Z*P
function get_K!(K::AbstractArray{T}, ZP::AbstractArray{T}, cholF::AbstractArray{T}) where T <: AbstractFloat
copy!(K, ZP)
LAPACK.potrs!('U', cholF, K)
end
function get_QQ!(c::AbstractMatrix{T}, a::AbstractMatrix{T}, b::AbstractMatrix{T}, work::Matrix{T}) where T <: AbstractFloat
mul!(work, a, b)
mul!(c, work, transpose(a))
function get_M!(y::AbstractArray{T}, x::AbstractArray{T}, work::AbstractArray{T}) where T <: AbstractFloat
copy!(work, x)
LAPACK.potri!('U', work)
n = size(x,1)
# complete lower trianle and change sign of entire matrix
@inbounds for i = 1:n
@simd for j = 1:i-1
y[j, i] = -work[j, i]
end
@simd for j = i:n
y[j, i] = -work[i, j]
end
end
end
function get_prediction_error(Y::AbstractArray{T}, Z::AbstractArray{T}, a::AbstractVector{T}, i::U, t::U) where {T <: AbstractFloat, U <: Integer}
......@@ -167,82 +187,67 @@ function get_prediction_error(Y::AbstractArray{T}, z::AbstractVector{U}, a::Abst
prediction_error = Y[i, t] - a[z[i]] # nu_{t,i} in 6.13 in DK (2012)
end
# v = y - c - Z*a -- basic
function get_v!(v::AbstractArray{T}, y::AbstractMatrix{T}, c::AbstractArray{T}, z::AbstractArray{T}, a::AbstractArray{T}, iy::U, ny::U) where {T <: AbstractFloat, U <: Integer}
copyto!(v, 1, y, iy, ny)
v .-= c
gemm!('N', 'N', -1.0, z, a, 1.0, v)
end
# v = y - c - a[z] -- Z selection matrix
function get_v!(v::AbstractArray{T}, y::AbstractMatrix{T}, c::AbstractArray{T}, z::AbstractVector{U}, a::AbstractArray{T}, iy::U, ny::U) where {T <: AbstractFloat, U <: Integer}
copyto!(v, 1, y, iy, ny)
az = view(a,z)
v .-= c .+ az
end
# v = y - c - Z*a -- missing observations
function get_v!(v::AbstractArray{T}, y::AbstractMatrix{T}, c::AbstractArray{T}, z::AbstractArray{T}, a::AbstractArray{T}, t::U, pattern::Vector{U}) where {T <: AbstractFloat, U <: Integer}
v .= view(y, pattern, t) .- view(c, pattern)
gemm!('N', 'N', -1.0, z, a, 1.0, v)
end
# v = y - c - a[z] -- Z selection matrix and missing variables
function get_v!(v::AbstractArray{T}, y::AbstractMatrix{T}, c::AbstractArray{T}, z::AbstractVector{U}, a::AbstractArray{T}, t::U, pattern::Vector{Int64}) where {T <: AbstractFloat, U <: Integer}
v .= view(y, pattern, t) .- view(c, pattern) .- view(a, z)
function get_QQ!(c::AbstractMatrix{T}, a::AbstractMatrix{T}, b::AbstractMatrix{T}, work::Matrix{T}) where T <: AbstractFloat
mul!(work, a, b)
mul!(c, work, transpose(a))
end
# v = y - Z*a -- basic
function get_v!(v::AbstractVector{T}, y::AbstractMatrix{T}, z::AbstractMatrix{T}, a::AbstractVector{T}, iy::U, ny::U) where {T <: AbstractFloat, U <: Integer}
function get_v!(v::AbstractVector{T}, y::AbstractVecOrMat{T}, z::AbstractVecOrMat{T}, a::AbstractVector{T}, iy::U, ny::U) where {T <: AbstractFloat, U <: Integer}
copyto!(v, 1, y, iy, ny)
gemv!('N', -1.0, z, a, 1.0, v)
end
# v = y - Z*a -- basic -- univariate
function get_v!(Y::AbstractVecOrMat{T}, Z::AbstractVecOrMat{T}, a::AbstractVector{T}, i::U) where {T <: AbstractFloat, U <: Integer}
v = Y[i]
for j = 1:length(a)
v -= Z[i, j]*a[j]
end
return v
end
# v = y - a[z] -- Z selection matrix
function get_v!(v::AbstractVector{T}, y::AbstractMatrix{T}, z::AbstractVector{U}, a::AbstractVector{T}, iy::U, ny::U) where {T <: AbstractFloat, U <: Integer}
function get_v!(v::AbstractVector{T}, y::AbstractVecOrMat{T}, z::AbstractVector{U}, a::AbstractVector{T}, iy::U, ny::U) where {T <: AbstractFloat, U <: Integer}
copyto!(v, 1, y, iy, ny)
az = view(a,z)
v .= v .- az
end
# v = y - Z*a -- missing observations
function get_v!(v::AbstractVector{T}, y::AbstractMatrix{T}, z::AbstractMatrix{T}, a::AbstractVector{T}, t::U, pattern::Vector{U}) where {T <: AbstractFloat, U <: Integer}
function get_v!(v::AbstractVector{T}, y::AbstractVecOrMat{T}, z::AbstractMatrix{T}, a::AbstractVector{T}, t::U, pattern::Vector{U}) where {T <: AbstractFloat, U <: Integer}
v .= view(y, pattern, t)
gemv!('N', -1.0, z, a, 1.0, v)
end
# v = y - a[z] -- Z selection matrix and missing variables
function get_v!(v::AbstractVector{T}, y::AbstractMatrix{T}, z::AbstractVector{U}, a::AbstractVector{T}, t::U, pattern::Vector{Int64}) where {T <: AbstractFloat, U <: Integer}
function get_v!(v::AbstractVector{T}, y::AbstractVecOrMat{T}, z::AbstractVector{U}, a::AbstractVector{T}, t::U, pattern::Vector{Int64}) where {T <: AbstractFloat, U <: Integer}
v .= view(y, pattern, t) .- view(a, z)
end
# F = Z*P*Z' + H
function get_F!(f::AbstractArray{T}, zp::AbstractArray{T}, z::AbstractArray{T}, p::AbstractArray{T}, h::AbstractArray{T}) where T <: AbstractFloat
copy!(f, h)
mul!(zp, z, p)
gemm!('N', 'T', 1.0, zp, z, 1.0, f)
# v = y - c - Z*a -- basic
function get_v!(v::AbstractArray{T}, y::AbstractVecOrMat{T}, c::AbstractArray{T}, z::AbstractArray{T}, a::AbstractArray{T}, iy::U, ny::U) where {T <: AbstractFloat, U <: Integer}
copyto!(v, 1, y, iy, ny)
v .-= c
gemm!('N', 'N', -1.0, z, a, 1.0, v)
end
# F = P(z,z) + H
function get_F!(f::AbstractArray{T}, zp::AbstractArray{T}, z::AbstractVector{I}, p::AbstractArray{T}, h::AbstractArray{T}) where {I <: Integer, T <: AbstractFloat}
copy!(f, h)
zp .= view(p, z, :)
f .+= view(zp, :, z)
# v = y - c - a[z] -- Z selection matrix
function get_v!(v::AbstractArray{T}, y::AbstractVecOrMat{T}, c::AbstractArray{T}, z::AbstractVector{U}, a::AbstractArray{T}, iy::U, ny::U) where {T <: AbstractFloat, U <: Integer}
copyto!(v, 1, y, iy, ny)
az = view(a,z)
v .-= c .+ az
end
function get_M!(y::AbstractArray{T}, x::AbstractArray{T}, work::AbstractArray{T}) where T <: AbstractFloat
copy!(work, x)
LAPACK.potri!('U', work)
n = size(x,1)
# complete lower trianle and change sign of entire matrix
@inbounds for i = 1:n
@simd for j = 1:i-1
y[j, i] = -work[j, i]
end
@simd for j = i:n
y[j, i] = -work[i, j]
end
end
# v = y - c - Z*a -- missing observations
function get_v!(v::AbstractArray{T}, y::AbstractVecOrMat{T}, c::AbstractArray{T}, z::AbstractArray{T}, a::AbstractArray{T}, t::U, pattern::Vector{U}) where {T <: AbstractFloat, U <: Integer}
v .= view(y, pattern, t) .- view(c, pattern)
gemm!('N', 'N', -1.0, z, a, 1.0, v)
end
# v = y - c - a[z] -- Z selection matrix and missing variables
function get_v!(v::AbstractArray{T}, y::AbstractVecOrMat{T}, c::AbstractArray{T}, z::AbstractVector{U}, a::AbstractArray{T}, t::U, pattern::Vector{Int64}) where {T <: AbstractFloat, U <: Integer}
v .= view(y, pattern, t) .- view(c, pattern) .- view(a, z)
end
# V_t = P_t - P_t*N_{t-1}*P_t
......
......@@ -8,107 +8,45 @@ ns = 10
np = 2
nobs = 50
Z = randn(ny, ns)
T = randn(ns, ns)
K = randn(ny, ns)
L = Matrix{Float64}(undef, ns, ns)
L1 = similar(L)
KalmanFilterTools.get_L!(L, T, K, Z, L1)
@test L T - T*K'*Z
z = [1, 3]
K1 = K[z, :]
K2 = zeros(ns, ns)
K2[z,:] .= K1
KalmanFilterTools.get_L!(L, T, K1, z, L1)
@test L T - T*K2'
# r_{t-1} = Z_t'*iF_t*v_t + L_t'r_t
r = randn(ns)
r1 = similar(r)
iFv = randn(ny)
KalmanFilterTools.update_r!(r1, Z, iFv, L, r)
@test r1 Z'*iFv + L'r
# r_{t-1} = Z_t'*iF_t*v_t + L_t'r_t
iFv1 = iFv[z]
KalmanFilterTools.update_r!(r1, z, iFv1, L, r)
ZiF = zeros(ns)
ZiF[z] .= iFv1
@test r1 ZiF + L'r
a = randn(nobs, nobs)
achol = cholesky(a'*a)
d = KalmanFilterTools.det_from_cholesky(achol.U)
@test det(a'*a) d
# alphah_t = a_t + P_t*r_{t-1}
alphah = Vector{Float64}(undef, ns)
a = randn(ns)
P = randn(ns, ns)
P = P'*P
r1 = randn(ns)
KalmanFilterTools.get_alphah!(alphah, a, P, r1)
@test alphah a + P*r1
# N_{t-1} = Z_t'iF_t*Z_t + L_t'N_t*L_t
N = randn(ns, ns)
N = N'*N
N1 = similar(N)
iFZ = randn(ny, ns)
Ptmp = similar(P)
KalmanFilterTools.update_N!(N1, Z, iFZ, L, N, Ptmp)
@test N1 Z'*iFZ + L'*N*L
iFZ1 = iFZ[z, :]
ZiFZ = zeros(ns, ns)
ZiFZ[z, :] .= iFZ1
KalmanFilterTools.update_N!(N1, z, iFZ1, L, N, Ptmp)
@test N1 ZiFZ + L'*N*L
# V_t = P_t - P_t*N_{t-1}*P_t
V = Matrix{Float64}(undef, ns, ns)
KalmanFilterTools.get_Valpha!(V, P, N1, Ptmp)
@test V P - P*N1*P
R = randn(ns, np)
Q = randn(np, np)
Q = Q'*Q
QQ = zeros(ns, ns)
RQ = zeros(ns, np)
KalmanFilterTools.get_QQ!(QQ, R, Q, RQ)
@test QQ R*Q*R'
# Vepsilon_t = H - H*D_t*H
Vepsilon = zeros(ny, ny)
H = randn(ny, ny)
D = randn(ny, ny)
tmp = zeros(ny, ny)
KalmanFilterTools.get_Vepsilon!(Vepsilon, H, D, tmp)
@test Vepsilon H - H*D*H
# Veta_t = Q - Q*R'*N_t*R*Q
Veta = zeros(np, np)
Q = zeros(np, np)
R = randn(ns, np)
N = randn(ns, ns)
RQ = zeros(ns, np)
tmp = zeros(ns, np)
KalmanFilterTools.get_Veta!(Veta, Q, R, N, RQ, tmp)
@test Veta Q - Q*R'*N*R*Q
F = randn(ny, ny)
F = F'*F
cholF = zeros(ny, ny)
KalmanFilterTools.get_cholF!(cholF, F)
CF = cholesky(0.5*(F + F'))
@test triu(cholF) CF.U
# D = inv(F_t) + K_t*T*N_t*T'*K'
D = zeros(ny, ny)
A = randn(ny, ny)
cholF = cholesky(A'*A)
D = randn(ny, ny)
F = randn(ny, ny)
F = F'*F
cholF = cholesky(F)
K = randn(ny, ns)
T = randn(ns, ns)
N = randn(ns, ns)
tmp = zeros(ny, ns)
KT = zeros(ny, ns)
KT = randn(ny, ns)
tmp = randn(ny, ns)
KalmanFilterTools.get_D!(D, cholF.U, K, T, N, KT, tmp)
@test D inv(A'A) + K*T*N*T'*K'
@test D inv(F) + K*T*N*T'*K'
# epsilonh_t = H*(iF_t*v_t - K_t*T*r_t)
epsilon = zeros(ny)
epsilon = randn(ny)
H = randn(ny, ny)
v = randn(ny)
iFv = randn(ny)
K = randn(ny, ns)
T = randn(ns, ns)
r = randn(ns)
......@@ -118,7 +56,7 @@ KalmanFilterTools.get_epsilonh!(epsilon, H, iFv, K, T, r, tmp1, tmp2)
@test epsilon H*(iFv - K*T*r)
# etah = Q*R'*r_t
eta = zeros(np)
eta = randn(np)
Q = randn(np, np)
R = randn(ns, np)
r = randn(ns)
......@@ -134,31 +72,102 @@ KalmanFilterTools.get_F!(F, ZP, Z, P, H)
@test ZP == Z*P
@test F Z*P*Z' + H
cholF = zeros(ny, ny)
# Z as selection matrix
fill!(Z, 0.0)
Z[1, 4] = 1
Z[2, 3] = 1
Z[3, 2] = 1
z = [4, 3, 2]
P_0 = randn(ns, ns)
P = copy(P_0)
KalmanFilterTools.get_F!(F, ZP, z, P, H)
@test F Z*P*Z' + H
F = zeros(ny, ny)
ZP = zeros(ny, ns)
Pinf = randn(ns, ns)
KalmanFilterTools.get_F!(F, ZP, Z, Pinf)
@test F Z*Pinf*Z'
@test ZP Z*Pinf
# iFv = inv(F)*v
F = randn(ny, ny)
F = F'*F
cholF = similar(F)
KalmanFilterTools.get_cholF!(cholF, F)
CF = cholesky(0.5*(F + F'))
@test triu(cholF) CF.U
v = randn(ny)
iFv = similar(v)
KalmanFilterTools.get_iFv!(iFv, cholF, v)
@test iFv F\v
K = zeros(ny, ns)
Z = randn(ny, ns)
P = randn(ns, ns)
ZP = Z*P
F = randn(ny, ny)
F = F'*F
cholF = similar(F)
KalmanFilterTools.get_cholF!(cholF, F)
KalmanFilterTools.get_K!(K, ZP, cholF)
@test K inv(F)*Z*P
P_0 = similar(P)
PTmp = similar(P)
copy!(P_0, P)
KalmanFilterTools.update_P!(P, T, QQ, K, ZP, PTmp)
@test P T*(P_0 - K'*ZP)*T' + QQ
K = F\Z*Pinf
Kstar = similar(K)
Fstar = zeros(ny, ny)
ZPstar = zeros(ny, ns)
Z = randn(ny, ns)
Pstar = randn(ns, ns)
H = randn(ny, ny)
KalmanFilterTools.get_F!(Fstar, ZPstar, Z, Pstar, H)
@test Fstar Z*Pstar*Z' + H
W = rand(ns, ny)
K = Z*P
mul!(W, T, transpose(K))
@test W T*P*Z'
KalmanFilterTools.get_Kstar!(Kstar, Z, Pstar, Fstar, K, cholF)
@test Kstar F\(Z*Pstar - Fstar*K)
Kstar = randn(ny, ns)
z = [4, 3, 2]
F = 0.5*(F + F')
cholF = copy(F)
LAPACK.potrf!('U', cholF)
KalmanFilterTools.get_Kstar!(Kstar, z, Pstar, Fstar, K, cholF)
@test Kstar F\(Pstar[z,:] - Fstar*K)
Z = randn(ny, ns)
T = randn(ns, ns)
K = randn(ny, ns)
L = Matrix{Float64}(undef, ns, ns)
L1 = similar(L)
KalmanFilterTools.get_L!(L, T, K, Z, L1)
@test L T - T*K'*Z
z = [1, 3, 2]
K1 = K[z, :]
K2 = zeros(ns, ns)
K2[z,:] .= K1
KalmanFilterTools.get_L!(L, T, K1, z, L1)
@test L T - T*K2'
M = rand(ny, ny)
ZW = rand(ny, ny)
KalmanFilterTools.get_M!(M, cholF, ZW)
@test M -inv(F)
# V_t = P_t - P_t*N_{t-1}*P_t
V = Matrix{Float64}(undef, ns, ns)
P = Matrix{Float64}(undef, ns, ns)
N1 = Matrix{Float64}(undef, ns, ns)
Ptmp = Matrix{Float64}(undef, ns, ns)
KalmanFilterTools.get_Valpha!(V, P, N1, Ptmp)
@test V P - P*N1*P
R = randn(ns, np)
Q = randn(np, np)
Q = Q'*Q
QQ = zeros(ns, ns)
RQ = zeros(ns, np)
KalmanFilterTools.get_QQ!(QQ, R, Q, RQ)
@test QQ R*Q*R'
#v = Y[:,t] - Z*a
a = rand(ns)
v = rand(ny)
......@@ -166,10 +175,35 @@ y = rand(ny, 1)
KalmanFilterTools.get_v!(v, y, Z, a, 1, ny)
@test v y[:,1] - Z*a
# iFv = inv(F)*v
iFv = similar(v)
KalmanFilterTools.get_iFv!(iFv, cholF, v)
@test iFv F\v
# missing observations
vv = view(v, 1:ny)
c = randn(ny)
vc = view(c, 1:ny)
vZ = view(Z, 1:ny, :)
a_0 = randn(ns)
va = view(a_0, :)
full_data_pattern = [collect(1:ny)]
pattern = full_data_pattern[1]
KalmanFilterTools.get_v!(vv, y, vc, vZ, va, 1, pattern)
@test vv y[:, 1] - c - Z*a_0
# Vepsilon_t = H - H*D_t*H
Vepsilon = zeros(ny, ny)
H = randn(ny, ny)
D = randn(ny, ny)
tmp = zeros(ny, ny)
KalmanFilterTools.get_Vepsilon!(Vepsilon, H, D, tmp)
@test Vepsilon H - H*D*H
# Veta_t = Q - Q*R'*N_t*R*Q
Veta = zeros(np, np)
Q = zeros(np, np)
R = randn(ns, np)
N = randn(ns, ns)
RQ = zeros(ns, np)
tmp = zeros(ns, np)
KalmanFilterTools.get_Veta!(Veta, Q, R, N, RQ, tmp)
@test Veta Q - Q*R'*N*R*Q
# a = T(a + K'*iFv)
a_0 = copy(a)
......@@ -177,6 +211,23 @@ a1 = similar(a)
KalmanFilterTools.update_a!(a, K, iFv, a1, T)
@test a T*(a_0 + K'*iFv)
va1 = zeros(ns)
va = randn(ns)
vd = zeros(ns)
vK = randn(ny,ns)
vv = randn(ny)
vT = view(T, :, :)
KalmanFilterTools.update_a!(va1, va, vd, vK, vv, a1, vT)
@test va1 vd + vT*(va + vK'*vv)
# K = K + Z*W*M*W'
K = randn(ny, ns)
K_0 = copy(K)
ZWM = randn(ny, ny)
W = randn(ns, ny)
KalmanFilterTools.update_K!(K, ZWM, W)
@test K K_0 + ZWM*W'
# M = M + M*W'*Z'iF*Z*W*M
M_0 = copy(M)
ZWM = similar(M)
......@@ -184,15 +235,51 @@ iFZWM = similar(M)