diff --git a/src/kalman_base.jl b/src/kalman_base.jl index bb56530..ba65f2b 100644 --- a/src/kalman_base.jl +++ b/src/kalman_base.jl @@ -330,7 +330,7 @@ function get_updated_Finfnull1!(a, Pinf, Pstar, ZPstar, cholF, Fstar, Z, H, T, K # a = T(a + K'*v) update_a!(a, K, v, a1, T) # Pinf = T*Pinf*T' - update_P!(Pinf, T, QQ, Pinf, Ptmp) + update_P!(Pinf, T, Pinf, Ptmp) # Pstar = T*(Pstar - K'*Z*Pstar)*T'+ QQ update_P!(Pstar, T, QQ, K, ZPstar, Ptmp) end @@ -697,7 +697,7 @@ function update_P!(P::AbstractArray{U}, T::AbstractArray{U}, Ptt::AbstractArray{ end # Pinf = T*Pinftt*T' -function update_P!(P::AbstractArray{U}, T::AbstractArray{U}, Ptt::AbstractArray{U}, Ptmp::AbstractArray{U}) where {U<:AbstractFloat} +function update_P!(P::AbstractArray{U}, T::AbstractArray{U}, Ptmp::AbstractArray{U}) where {U<:AbstractFloat} mul!(Ptmp, Ptt, transpose(T)) mul!(P, T, Ptmp) end diff --git a/test/test_KalmanFilterTools.jl b/test/test_KalmanFilterTools.jl index 1490c52..fe84a45 100644 --- a/test/test_KalmanFilterTools.jl +++ b/test/test_KalmanFilterTools.jl @@ -639,26 +639,26 @@ end Pstar = T*Pstartt*T' KalmanFilterTools.get_updated_Finfnull2!(att, - Pinftt, - Pstartt, - ZPinf, - ZPstar, - cholF, - Fstar, - Zsmall, - Pstar, - H, - K0, - a, - v, - Pinf, - 1e-12) + Pinftt, + Pstartt, + ZPinf, + ZPstar, + cholF, + Fstar, + Zsmall, + Pstar, + H, + K0, + a, + v, + Pinf, + 1e-12) # K0 = iFsar*Z*Pstar Fstar_2 = ZPstar*Z' + H K0_2 = Fstar_2\ZPstar att2 = a + K0'*v # Pinf_tt = Pinf - Pinftt2 = Pinf + Pinftt2 = copy(Pinf) # Pstartt = Pstar*L0' # = Pstar*(I - Z'*inv(Fstar)*Z*Pstar) # = Pstar - K0_2'*ZPstar @@ -670,6 +670,15 @@ end @test att2 ≈ att @test Pinftt2 ≈ Pinftt @test isapprox(Pstartt2, Pstartt, atol = 1e-15) + + a1 = similar(a) + K = similar(K0) + QQ = R*Q*R' + Ptmp = similar(Pinf) + KalmanFilterTools.get_updated_Finfnull1!(a1, Pinf, Pstar, ZPstar, cholF, Fstar, Z, H, T, K, QQ, a, v, Ptmp, 1e-12) + @test a1 ≈ T*att2 + @test Pinf ≈ T*Pinftt2*T' + @test Pstar ≈ T*Pstartt2*T' + QQ end @testset "start and last" begin @@ -728,9 +737,6 @@ end @test llk_5 ≈ llk_4 end -@testset "smoother" begin -end - nothing