Commit 6978f117 authored by Michel Juillard's avatar Michel Juillard
Browse files

adding diffsuse_kalman_filter_init!()

parent 82819f20
......@@ -23,6 +23,7 @@ using LinearAlgebra
using LinearAlgebra.BLAS
export KalmanLikelihoodWs, FastKalmanLikelihoodWs, DiffuseKalmanLikelihoodWs
export DiffuseKalmanFilterWs
export KalmanSmootherWs, kalman_likelihood, kalman_likelihood_monitored
export fast_kalman_likelihood, diffuse_kalman_likelihood, kalman_filter!
export kalman_smoother!
......
......@@ -297,22 +297,28 @@ function get_Veta!(Veta, Q, R, N, RQ, tmp)
mul!(Veta, transpose(RQ), tmp, -1.0, 1.0)
end
function get_vZsmall(Zsmall::AbstractMatrix{T}, iZsmall::AbstractVector{U}, Z::AbstractMatrix{T}, pattern::AbstractVector{Int64}, n::U, ny::U) where {T <: AbstractFloat, U <: Integer}
function get_vZsmall(Zsmall::AbstractMatrix{T}, iZsmall::AbstractVector{U}, Z::AbstractArray{T}, pattern::AbstractVector{U}, n::U, ny::U) where {T <: AbstractFloat, U <: Integer}
changeZ = ndims(Z) > 2
vZ = changeZ ? view(Z, :, :, t) : view(Z, :, :)
vZsmall = view(Zsmall, 1:n, :)
if n == ny
copyto!(vZsmall, Z)
copyto!(vZsmall, vZ)
else
vZsmall .= view(Z, pattern, :)
vZsmall .= view(vZ, pattern, :)
end
return vZsmall
end
function get_vZsmall(Zsmall::AbstractMatrix{T}, iZsmall::AbstractVector{U}, Z::AbstractVector{Int64}, pattern::AbstractVector{Int64}, n::U, ny::U) where {T <: AbstractFloat, U <: Integer}
function get_vZsmall(Zsmall::AbstractMatrix{T}, iZsmall::AbstractVector{U}, z::AbstractArray{U}, pattern::AbstractVector{U}, n::U, ny::U) where {T <: AbstractFloat, U <: Integer}
changeZ = ndims(z) > 1
vz = changeZ ? view(z, :, t) : view(z, :,)
vZsmall = view(iZsmall, 1:n)
if n == ny
copyto!(vZsmall, Z)
copyto!(vZsmall, vz)
else
vZsmall .= view(Z, pattern)
end
return vZsmall
end
# a = T(a + K'*v)
......@@ -396,6 +402,24 @@ function update_P!(P::AbstractArray{U}, P1::AbstractArray{U}, T::AbstractArray{U
gemm!('N', 'T', 1.0, Ptmp, T, 1.0, P)
end
# Pstar = T*(Pstar-Pstar*Z'*Kinf-Pinf*Z'*Kstar)*T'+QQ; %(5.14) DK(2012)
function update_Pstar!(Pstar1, Pstar, T, ZPinf, ZPstar, Kinf, Kstar, QQ, PTmp)
copy!(PTmp, Pstar)
mul!(PTmp, transpose(ZPstar), Kinf, -1.0, 1.0)
mul!(PTmp, transpose(ZPinf), Kstar, -1.0, 1.0)
copy!(Pstar, PTmp)
mul!(PTmp, T, Pstar)
copy!(Pstar1, QQ)
mul!(Pstar1, PTmp, transpose(T), 1.0, 1.0)
end
# Pinf = T*(Pinf-Pinf*Z'*Kinf)*T'; %(5.14) DK(2012)
function update_Pinf!(Pinf1, Pinf, T, ZPinf, Kinf, PTmp)
mul!(Pinf, transpose(ZPinf), Kinf, -1.0, 1.0)
mul!(PTmp, T, Pinf)
mul!(Pinf1, PTmp, transpose(T))
end
# r_{t-1} = Z_t'*iF_t*v_t + L_t'r_t
function update_r!(r1::AbstractVector{T}, Z::AbstractArray{T}, iFv::AbstractVector{T}, L::AbstractArray{T}, r::AbstractVector{T}) where T <: AbstractFloat
mul!(r1, transpose(Z), iFv)
......
......@@ -122,16 +122,18 @@ struct DiffuseKalmanFilterWs{T, U} <: KalmanWs{T, U}
iZsmall::Vector{U}
QQ::Matrix{T}
RQ::Matrix{T}
c::Vector{T}
v::Vector{T}
F::Matrix{T}
iF::Matrix{T}
iFv::Vector{T}
a1::Vector{T}
cholF::Matrix{T}
cholH::Matrix{T}
ZP::Matrix{T}
Fstar::Matrix{T}
ZPstar::Matrix{T}
K::Matrix{T}
Kinf::Matrix{T}
iFZ::Matrix{T}
Kstar::Matrix{T}
PTmp::Matrix{T}
......@@ -151,16 +153,18 @@ struct DiffuseKalmanFilterWs{T, U} <: KalmanWs{T, U}
iZsmall = Vector{U}(undef, ny)
QQ = Matrix{T}(undef, ns, ns)
RQ = Matrix{T}(undef, ns, np)
c = Vector{T}(undef, ny)
v = Vector{T}(undef, ny)
F = Matrix{T}(undef, ny, ny)
iF = Matrix{T}(undef, ny,ny )
iFv = Vector{T}(undef, ny)
a1 = Vector{T}(undef, ns)
cholF = Matrix{T}(undef, ny, ny)
cholH = Matrix{T}(undef, ny, ny)
ZP = Matrix{T}(undef, ny, ns)
Fstar = Matrix{T}(undef, ny, ny)
ZPstar = Matrix{T}(undef, ny, ns)
K = Matrix{T}(undef, ny, ns)
Kinf = Matrix{T}(undef, ny, ns)
iFZ = Matrix{T}(undef, ny, ns)
Kstar = Matrix{T}(undef, ny, ns)
PTmp = Matrix{T}(undef, ns, ns)
......@@ -173,9 +177,9 @@ struct DiffuseKalmanFilterWs{T, U} <: KalmanWs{T, U}
PZi = Vector{T}(undef, ns)
lik = zeros(T, nobs)
kalman_tol = 1e-12
new(csmall, Zsmall, iZsmall, QQ, RQ, v, F, iF, iFv, a1, cholF, ZP, Fstar,
ZPstar, K, iFZ, Kstar, PTmp, uKinf, uKstar, Kinf_Finf, ystar, Zstar, Hstar,
PZI, lik, kalman_tol)
new(csmall, Zsmall, iZsmall, QQ, RQ, c, v, F, iF, iFv, a1, cholF, cholH, ZP, Fstar,
ZPstar, Kinf, iFZ, Kstar, PTmp, uKinf, uKstar, Kinf_Finf, ystar, Zstar, Hstar,
PZi, lik, kalman_tol)
end
end
......@@ -187,7 +191,8 @@ function diffuse_kalman_filter_init!(Y::AbstractArray{U},
H::AbstractArray{U},
d::AbstractArray{U},
T::AbstractArray{U},
QQ::AbstractArray{U},
R::AbstractArray{U},
Q::AbstractArray{U},
a::AbstractArray{U},
Pinf::AbstractArray{U},
Pstar::AbstractArray{U},
......@@ -195,129 +200,96 @@ function diffuse_kalman_filter_init!(Y::AbstractArray{U},
last::V,
presample::V,
tol::U,
ws::KalmanWs) where {U <: AbstractFloat,
V <: Integer,
W <: Real}
ny = size(Y, 1)
t = start
iy = (start - 1)*ny + 1
diffuse_kalman_tol = 1e-8
kalman_tol = 1e-8
while t <= last
# v = Y[:,t] - Z*a
get_v!(ws.v, Y, Z, a, iy, ny)
iy += ny
# Finf = Z*Pinf*Z'
get_F!(ws.F, ws.ZP, Z, Pinf)
info = get_cholF!(ws.cholF, ws.F)
if info > 0
if norm(ws.F) < tol
return t - 1
else
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))
# Kinf = iFinf*Z*Pinf %define Kinf'=T^{-1}*K_0 with M_{\infty}=Pinf*Z'
copy!(ws.K, ws.ZP)
LAPACK.potrs!('U', ws.cholF, ws.K)
# Fstar = Z*Pstar*Z' + H; %(5.7) DK(2012)
get_F!(ws.Fstar, ws.ZPstar, Z, Pstar, H)
# Kstar = iFinf*(Z*Pstar - Fstar*Kinf) %(5.12) DK(2012); note that there is a typo in DK (2003) with "+ Kinf" instead of "- Kinf", but it is correct in their appendix
get_Kstar!(ws.Kstar, Z, Pstar, ws.Fstar, ws.K, ws.cholF)
# Pstar = T*(Pstar-Pstar*Z'*Kinf-Pinf*Z'*Kstar)*T'+QQ; %(5.14) DK(2012)
copy!(ws.PTmp, Pstar)
gemm!('T','N',-1.0,ws.ZPstar, ws.K, 1.0, ws.PTmp)
gemm!('T','N',-1.0,ws.ZP, ws.Kstar, 1.0, ws.PTmp)
copy!(Pstar, ws.PTmp)
mul!(ws.PTmp,T,Pstar)
copy!(Pstar, QQ)
gemm!('N','T',1.0,ws.PTmp,T,1.0,Pstar)
# Pinf = T*(Pinf-Pinf*Z'*Kinf)*T'; %(5.14) DK(2012)
gemm!('T','N', -1.0,ws.ZP, ws.K,1.0,Pinf)
mul!(ws.PTmp,T,Pinf)
mul!(Pinf,ws.PTmp,transpose(T))
# a = T*(a+Kinf*v); %(5.13) DK(2012)
update_a!(a, ws.K, ws.v, ws.a1, T)
end
t += 1
end
t
end
function diffuse_kalman_filter_init!(Y::Matrix{U},
Z::AbstractArray{W},
H::Matrix{U},
T::Matrix{U},
QQ::Matrix{U},
a::Vector{U},
Pinf::Matrix{U},
Pstar::Matrix{U},
start::V,
last::V,
tol::U,
ws::KalmanWs,
ws::DiffuseKalmanFilterWs,
data_pattern::Vector{Vector{V}}) where {U <: AbstractFloat,
V <: Integer,
W <: Real}
changeC = ndims(c) > 1
changeH = ndims(H) > 2
changeD = ndims(d) > 1
changeT = ndims(T) > 2
changeR = ndims(R) > 2
changeQ = ndims(Q) > 2
changeA = ndims(a) > 1
changePinf = ndims(Pinf) > 2
changePstar = ndims(Pstar) > 2
ny = size(Y, 1)
t = start
LIK = 0
iy = (start - 1)*ny + 1
vR = view(R, :, :, 1)
vQ = view(Q, :, :, 1)
get_QQ!(ws.QQ, vR, vQ, ws.RQ)
diffuse_kalman_tol = 1e-8
kalman_tol = 1e-8
cholHset = false
while t <= last
pattern = data_pattern[t]
ndata = length(pattern)
vH = view(H, pattern, pattern)
vc = changeC ? view(c, :, t) : view(c, :)
ws.csmall .= view(vc, pattern)
vZsmall = get_vZsmall(ws.Zsmall, ws.iZsmall, Z, 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, :)
vPinf = changePinf ? view(Pinf, :, :, t) : view(Pinf, :, :)
vPinf1 = changePinf ? view(Pinf, :, :, t + 1) : view(Pinf, :, :)
vPstar = changePstar ? view(Pstar, :, :, t) : view(Pstar, :, :)
vPstar1 = changePstar ? view(Pstar, :, :, t + 1) : view(Pstar, :, :)
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)
vFstar = view(ws.Fstar, 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)
vFinf = view(ws.F, 1:ndata, 1:ndata)
vFstar = 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)
vcholF = view(ws.cholF, 1:ndata, 1:ndata, 1)
vcholH = view(ws.cholH, 1:ndata, 1:ndata, 1)
viFv = view(ws.iFv, 1:ndata)
vK = view(ws.K, 1:ndata, :)
vKstar = view(ws.Kstar, 1:ndata, :)
vZsmall = get_vZsmall(ws.Zsmall, ws.iZsmall, Z, pattern, ndata, ny)
vKinf = view(ws.Kinf, 1:ndata, :, 1)
vKstar = view(ws.Kstar, 1:ndata, :, 1)
# v = Y[:,t] - Z*a
get_v!(vv, Y, vZsmall, a, t, pattern)
iy += ny
# v = Y[:,t] - c - Z*a
get_v!(vv, Y, vc, vZsmall, va, t, pattern)
# Finf = Z*Pinf*Z'
get_F!(vF, vZP, vZsmall, Pinf)
info = get_cholF!(vcholF, vF)
get_F!(vFinf, vZPinf, vZsmall, vPinf)
info = get_cholF!(vcholF, vFinf)
if info > 0
if norm(vF) < tol
if norm(ws.F) < tol
return t - 1
else
ws.lik[t] += univariate_step(Y, t, vZsmall, H, T, QQ, a, Pinf, Pstar, diffuse_kalman_tol, kalman_tol, pattern, ws)
if !cholHset
get_cholF!(vcholH, vvH)
cholHset = true
end
ws.lik[t] += univariate_step(Y, t, vZsmall, vvH, T, QQ, a, Pinf, Pstar, diffuse_kalman_tol, kalman_tol, ws)
end
else
ws.lik[t] = log(det_from_cholesky(ws.cholF))
ws.lik[t] = log(det_from_cholesky(vcholF))
# Kinf = iFinf*Z*Pinf %define Kinf'=T^{-1}*K_0 with M_{\infty}=Pinf*Z'
copy!(vK, vZP)
LAPACK.potrs!('U', vcholF, vK)
copy!(vKinf, vZP)
LAPACK.potrs!('U', vcholF, vKinf)
# Fstar = Z*Pstar*Z' + H; %(5.7) DK(2012)
get_F!(vFstar, vZPstar, vZsmall, Pstar, vH)
# Kstar = iFinf*(Z*Pstar - Fstar*Kinf) %(5.12) DK(2012); note that there is a typo in DK (2003) with "+ Kinf" instead of "- Kinf", but it is correct in their appendix
get_Kstar!(vKstar, Z, Pstar, vFstar, vK, vcholF)
get_F!(vFstar, vZPstar, vZsmall, vPstar, vH)
# Kstar = iFinf*(Z*Pstar - Fstar*Kinf) %(5.12) DK(2012);
# note that there is a typo in DK (2003) with "+ Kinf" instead of "- Kinf",
# but it is correct in their appendix
get_Kstar!(vKstar, vZsmall, vPstar, vFstar, vKinf, vcholF)
# Pstar = T*(Pstar-Pstar*Z'*Kinf-Pinf*Z'*Kstar)*T'+QQ; %(5.14) DK(2012)
copy!(ws.PTmp, Pstar)
gemm!('T','N',-1.0, vZPstar, vK, 1.0, ws.PTmp)
gemm!('T','N',-1.0, vZP, vKstar, 1.0, ws.PTmp)
copy!(Pstar, ws.PTmp)
mul!(ws.PTmp,T,Pstar)
copy!(Pstar, QQ)
gemm!('N','T',1.0,ws.PTmp,T,1.0,Pstar)
update_Pstar!(vPstar1, vPstar, vT, vZPinf, vZPstar, vKinf, vKstar, ws.QQ, ws.PTmp)
# Pinf = T*(Pinf-Pinf*Z'*Kinf)*T'; %(5.14) DK(2012)
gemm!('T','N', -1.0, vZP, ws.K,1.0,Pinf)
mul!(ws.PTmp,T,Pinf)
mul!(Pinf,ws.PTmp,transpose(T))
# a = T*(a+Kinf*v); %(5.13) DK(2012)
update_a!(a, vK, vv, ws.a1, T)
update_Pinf!(vPinf1, vPinf, vT, vZPinf, vKinf, ws.PTmp)
# a = d + T*(a+Kinf*v); %(5.13) DK(2012)
update_a!(va1, va, vd, vKinf, vv, ws.a1, vT)
end
t += 1
end
......@@ -355,7 +327,7 @@ function diffuse_kalman_filter(Y::Matrix{U},
LIK
end
function diffuse_kalman_filter(Y::Matrix{U},
function diffuse_kalman_filter!(Y::Matrix{U},
Z::AbstractArray{W},
H::Matrix{U},
T::Matrix{U},
......@@ -368,7 +340,7 @@ function diffuse_kalman_filter(Y::Matrix{U},
last::V,
presample::V,
tol::U,
ws::DiffuseKalmanLikelihoodWs,
ws::DiffuseKalmanFilterWs,
data_pattern::Vector{Vector{V}}) where {U <: AbstractFloat,
V <: Integer,
W <: Real}
......@@ -377,7 +349,7 @@ function diffuse_kalman_filter(Y::Matrix{U},
get_QQ!(ws.QQ, R, Q, ws.RQ)
lik_cst = (nobs - presample)*ny*log(2*pi)
fill!(ws.lik, 0.0)
t = diffuse_kalman_likelihood_init!(Y, Z, H, T, ws.QQ, a, Pinf, Pstar, start, last, tol, ws, data_pattern)
t = diffuse_kalman_filter_init!(Y, Z, H, T, ws.QQ, a, Pinf, Pstar, start, last, tol, ws, data_pattern)
kalman_likelihood(Y, Z, H, T, R, Q, a, Pstar, t, last, presample, ws, data_pattern)
@inbounds if presample > 0
LIK = -0.5*(lik_cst + sum(view(ws.lik, (presample+1):nobs)))
......
......@@ -31,7 +31,7 @@ full_data_pattern = [collect(1:ny) for o = 1:nobs]
P = copy(P_0)
s = copy(s_0)
@testset "Basic Kalman Filter" begin
ws1 = KalmanLikelihoodWs{Float64, Integer}(ny, ns, np, nobs)
ws1 = KalmanLikelihoodWs{Float64, Int64}(ny, ns, np, nobs)
copy!(P, P_0)
llk_1 = kalman_likelihood(y, Z, H, T, R, Q, s, P, 1, nobs, 0, ws1)
......@@ -135,8 +135,8 @@ end
H = copy(H_0)
# Fast Kalman Filter
@testset "Fast Kalman Filter" begin
ws1 = KalmanLikelihoodWs{Float64, Integer}(ny, ns, np, nobs)
ws2 = FastKalmanLikelihoodWs{Float64, Integer}(ny, ns, np, nobs)
ws1 = KalmanLikelihoodWs{Float64, Int64}(ny, ns, np, nobs)
ws2 = FastKalmanLikelihoodWs{Float64, Int64}(ny, ns, np, nobs)
P = copy(P_0)
s = copy(s_0)
......@@ -158,8 +158,8 @@ end
@testset "Z as selection matrix" begin
ws1 = KalmanLikelihoodWs{Float64, Integer}(ny, ns, np, nobs)
ws2 = FastKalmanLikelihoodWs{Float64, Integer}(ny, ns, np, nobs)
ws1 = KalmanLikelihoodWs{Float64, Int64}(ny, ns, np, nobs)
ws2 = FastKalmanLikelihoodWs{Float64, Int64}(ny, ns, np, nobs)
fill!(Z, 0.0)
Z[1, 4] = 1
......@@ -199,7 +199,7 @@ end
s = copy(s_0)
P = copy(P_0)
nobs1 = 1
ws1 = KalmanLikelihoodWs{Float64, Integer}(ny, ns, np, nobs1)
ws1 = KalmanLikelihoodWs{Float64, Int64}(ny, ns, np, nobs1)
kalman_filter!(y, c, Z, H, d, T, R, Q, s, P, 1, nobs1, 0, ws1, full_data_pattern)
......@@ -255,7 +255,8 @@ end
full_data_pattern = [collect(1:ny) for o = 1:nobs]
@testset "Diffuse Kalman Filter" begin
ws4 = DiffuseKalmanLikelihoodWs{Float64, Integer}(ny, ns, np, nobs)
ws4 = DiffuseKalmanLikelihoodWs{Float64, Int64}(ny, ns, np, nobs)
ws5 = DiffuseKalmanFilterWs{Float64, Int64}(ny, ns, np, nobs)
a = copy(a_0)
Pinf = copy(Pinf_0)
......@@ -270,6 +271,17 @@ full_data_pattern = [collect(1:ny) for o = 1:nobs]
@test a vars["a"]
@test Pstar vars["Pstar1"]
c = zeros(ny)
d = zeros(ns)
aa = repeat(a_0, 1, nobs)
PPinf = repeat(Pinf_0, 1, 1, nobs)
PPstar = repeat(Pstar_0, 1, 1, nobs)
t1 = KalmanFilterTools.diffuse_kalman_filter_init!(Y, c, Z, H, d, T, R, Q, aa, PPinf, PPstar, 1, nobs, 0, 1e-8, ws5, full_data_pattern)
@test t1 == t
@test llk_3 -0.5*(t*ny*log(2*pi) + sum(ws5.lik[1:t1]))
@test aa[:, t1 + 1] vars["a"]
@test PPstar[:, :, t1 + 1] vars["Pstar1"]
z = [4, 3]
a = copy(a_0)
Pinf = copy(Pinf_0)
......@@ -282,6 +294,15 @@ full_data_pattern = [collect(1:ny) for o = 1:nobs]
@test a vars["a"]
@test Pstar vars["Pstar1"]
aa = repeat(a_0, 1, nobs)
PPinf = repeat(Pinf_0, 1, 1, nobs)
PPstar = repeat(Pstar_0, 1, 1, nobs)
t1 = KalmanFilterTools.diffuse_kalman_filter_init!(Y, c, z, H, d, T, R, Q, aa, PPinf, PPstar, 1, nobs, 0, 1e-8, ws5, full_data_pattern)
@test t1 == t
@test llk_3 -0.5*(t*ny*log(2*pi) + sum(ws5.lik[1:t1]))
@test aa[:, t1 + 1] vars["a"]
@test PPstar[:, :, t1 + 1] vars["Pstar1"]
a = copy(a_0)
Pinf = copy(Pinf_0)
Pstar = copy(Pstar_0)
......@@ -330,7 +351,7 @@ end
@testset "start and last" begin
nobs1 = nobs - 1
ws1 = KalmanLikelihoodWs{Float64, Integer}(ny, ns, np, nobs)
ws1 = KalmanLikelihoodWs{Float64, Int64}(ny, ns, np, nobs)
P_0 = randn(ns, ns)
P_0 = P_0'P_0
......@@ -367,7 +388,7 @@ end
llk_2 = kalman_likelihood_monitored(Y, Z, H, T, R, Q, a, P, 2, nobs-1, 0, ws1, full_data_pattern)
@test llk_2 llk_1
ws4 = DiffuseKalmanLikelihoodWs{Float64, Integer}(ny, ns, np, nobs)
ws4 = DiffuseKalmanLikelihoodWs{Float64, Int64}(ny, ns, np, nobs)
z = [4, 3]
a = copy(a_0)
......
......@@ -330,5 +330,25 @@ ZW = randn(ny, ny)
gemm!('N', 'T', 1.0, ZWM, ZW, 1.0,F)
@test F F_0 + ZWM*ZW'
# Pstar = T*(Pstar-Pstar*Z'*Kinf-Pinf*Z'*Kstar)*T'+QQ; %(5.14) DK(2012)
Pstar = randn(ns, ns)
PstarOrig = copy(Pstar)
T = randn(ns, ns)
ZPinf = randn(ny, ns)
ZPstar = randn(ny, ns)
Kinf = randn(ny, ns)
Kstar = randn(ny, ns)
QQ = randn(ns, ns)
PTmp = randn(ns, ns)
KalmanFilterTools.update_Pstar!(Pstar, T, ZPinf, ZPstar, Kinf, Kstar, QQ, PTmp)
@test Pstar T*(PstarOrig-ZPstar'*Kinf-ZPinf'*Kstar)*T'+QQ
# Pinf = T*(Pinf-Pinf*Z'*Kinf)*T'; %(5.14) DK(2012)
Pinf = randn(ns, ns)
PinfOrig = copy(Pinf)
T = randn(ns, ns)
ZPinf = randn(ny, ns)
Kinf = randn(ny,ns)
PTmp = randn(ns, ns)
KalmanFilterTools.update_Pinf!(Pinf, T, ZPinf, Kinf, PTmp)
@test Pinf T*(PinfOrig - ZPinf'*Kinf)*T'
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