diff --git a/.gitignore b/.gitignore index 4461875..8486632 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ output test/perf/*.jld +.* \ No newline at end of file diff --git a/Manifest.toml b/Manifest.toml new file mode 100644 index 0000000..2289878 --- /dev/null +++ b/Manifest.toml @@ -0,0 +1,259 @@ +# This file is machine-generated - editing it directly is not advised + +[[Arpack]] +deps = ["BinaryProvider", "Libdl", "LinearAlgebra", "Random", "SparseArrays", "Test"] +git-tree-sha1 = "1ce1ce9984683f0b6a587d5bdbc688ecb480096f" +uuid = "7d9fca2a-8960-54d3-9f78-7d1dccf2cb97" +version = "0.3.0" + +[[Base64]] +uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" + +[[BinDeps]] +deps = ["Compat", "Libdl", "SHA", "URIParser"] +git-tree-sha1 = "12093ca6cdd0ee547c39b1870e0c9c3f154d9ca9" +uuid = "9e28174c-4ba2-5203-b857-d8d62c4213ee" +version = "0.8.10" + +[[BinaryProvider]] +deps = ["Libdl", "SHA"] +git-tree-sha1 = "c7361ce8a2129f20b0e05a89f7070820cfed6648" +uuid = "b99e7846-7c00-51b0-8f62-c81ae34c0232" +version = "0.5.4" + +[[ColorTypes]] +deps = ["FixedPointNumbers", "Random", "Test"] +git-tree-sha1 = "f73b0e10f2a5756de7019818a41654686da06b09" +uuid = "3da002f7-5984-5a60-b8a6-cbb66c0b333f" +version = "0.7.5" + +[[Colors]] +deps = ["ColorTypes", "FixedPointNumbers", "InteractiveUtils", "Printf", "Reexport", "Test"] +git-tree-sha1 = "9f0a0210450acb91c730b730a994f8eef1d3d543" +uuid = "5ae59095-9a9b-59fe-a467-6f913c188581" +version = "0.9.5" + +[[Compat]] +deps = ["Base64", "Dates", "DelimitedFiles", "Distributed", "InteractiveUtils", "LibGit2", "Libdl", "LinearAlgebra", "Markdown", "Mmap", "Pkg", "Printf", "REPL", "Random", "Serialization", "SharedArrays", "Sockets", "SparseArrays", "Statistics", "Test", "UUIDs", "Unicode"] +git-tree-sha1 = "84aa74986c5b9b898b0d1acaf3258741ee64754f" +uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" +version = "2.1.0" + +[[DataStructures]] +deps = ["InteractiveUtils", "OrderedCollections", "Random", "Serialization", "Test"] +git-tree-sha1 = "ca971f03e146cf144a9e2f2ce59674f5bf0e8038" +uuid = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" +version = "0.15.0" + +[[Dates]] +deps = ["Printf"] +uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" + +[[DelimitedFiles]] +deps = ["Mmap"] +uuid = "8bb1440f-4735-579b-a4ab-409b98df4dab" + +[[Distributed]] +deps = ["Random", "Serialization", "Sockets"] +uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" + +[[Distributions]] +deps = ["LinearAlgebra", "PDMats", "Printf", "QuadGK", "Random", "SpecialFunctions", "Statistics", "StatsBase", "StatsFuns"] +git-tree-sha1 = "56a158bc0abe4af5d4027af2275fde484261ca6d" +uuid = "31c24e10-a181-5473-b8eb-7969acd0382f" +version = "0.19.2" + +[[FixedPointNumbers]] +deps = ["Test"] +git-tree-sha1 = "b8045033701c3b10bf2324d7203404be7aef88ba" +uuid = "53c48c17-4a7d-5ca2-90c5-79b7896eea93" +version = "0.5.3" + +[[InteractiveUtils]] +deps = ["Markdown"] +uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" + +[[IteratorInterfaceExtensions]] +git-tree-sha1 = "a3f24677c21f5bbe9d2a714f95dcd58337fb2856" +uuid = "82899510-4779-5014-852e-03e436cf321d" +version = "1.0.0" + +[[LibGit2]] +uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" + +[[Libdl]] +uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" + +[[LinearAlgebra]] +deps = ["Libdl"] +uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" + +[[Logging]] +uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" + +[[Markdown]] +deps = ["Base64"] +uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" + +[[Missings]] +deps = ["Dates", "InteractiveUtils", "SparseArrays", "Test"] +git-tree-sha1 = "d1d2585677f2bd93a97cfeb8faa7a0de0f982042" +uuid = "e1d29d7a-bbdc-5cf2-9ac0-f12de2c33e28" +version = "0.4.0" + +[[Mmap]] +uuid = "a63ad114-7e13-5084-954f-fe012c677804" + +[[OrderedCollections]] +deps = ["Random", "Serialization", "Test"] +git-tree-sha1 = "c4c13474d23c60d20a67b217f1d7f22a40edf8f1" +uuid = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" +version = "1.1.0" + +[[PDMats]] +deps = ["Arpack", "LinearAlgebra", "SparseArrays", "SuiteSparse", "Test"] +git-tree-sha1 = "8b68513175b2dc4023a564cb0e917ce90e74fd69" +uuid = "90014a1f-27ba-587c-ab20-58faa44d9150" +version = "0.9.7" + +[[Pkg]] +deps = ["Dates", "LibGit2", "Markdown", "Printf", "REPL", "Random", "SHA", "UUIDs"] +uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" + +[[Polynomials]] +deps = ["LinearAlgebra", "SparseArrays", "Test"] +git-tree-sha1 = "62142bd65d3f8aeb2226ec64dd8493349147df94" +uuid = "f27b6e38-b328-58d1-80ce-0feddd5e7a45" +version = "0.5.2" + +[[Printf]] +deps = ["Unicode"] +uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" + +[[QuadGK]] +deps = ["DataStructures", "LinearAlgebra", "Test"] +git-tree-sha1 = "3ce467a8e76c6030d4c3786e7d3a73442017cdc0" +uuid = "1fd47b50-473d-5c70-9696-f719f8f3bcdc" +version = "2.0.3" + +[[REPL]] +deps = ["InteractiveUtils", "Markdown", "Sockets"] +uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" + +[[Random]] +deps = ["Serialization"] +uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" + +[[RecipesBase]] +deps = ["Random", "Test"] +git-tree-sha1 = "0b3cb370ee4dc00f47f1193101600949f3dcf884" +uuid = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" +version = "0.6.0" + +[[Reexport]] +deps = ["Pkg"] +git-tree-sha1 = "7b1d07f411bc8ddb7977ec7f377b97b158514fe0" +uuid = "189a3867-3050-52da-a836-e630ba90ab69" +version = "0.2.0" + +[[Requires]] +deps = ["Test"] +git-tree-sha1 = "f6fbf4ba64d295e146e49e021207993b6b48c7d1" +uuid = "ae029012-a4dd-5104-9daa-d747884805df" +version = "0.5.2" + +[[Rmath]] +deps = ["BinaryProvider", "Libdl", "Random", "Statistics", "Test"] +git-tree-sha1 = "9a6c758cdf73036c3239b0afbea790def1dabff9" +uuid = "79098fc4-a85e-5d69-aa6a-4863f24498fa" +version = "0.5.0" + +[[SHA]] +uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" + +[[Serialization]] +uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" + +[[SharedArrays]] +deps = ["Distributed", "Mmap", "Random", "Serialization"] +uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" + +[[Sockets]] +uuid = "6462fe0b-24de-5631-8697-dd941f90decc" + +[[SortingAlgorithms]] +deps = ["DataStructures", "Random", "Test"] +git-tree-sha1 = "03f5898c9959f8115e30bc7226ada7d0df554ddd" +uuid = "a2af1166-a08f-5f64-846c-94a0d3cef48c" +version = "0.3.1" + +[[SparseArrays]] +deps = ["LinearAlgebra", "Random"] +uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" + +[[SpecialFunctions]] +deps = ["BinDeps", "BinaryProvider", "Libdl", "Test"] +git-tree-sha1 = "0b45dc2e45ed77f445617b99ff2adf0f5b0f23ea" +uuid = "276daf66-3868-5448-9aa4-cd146d93841b" +version = "0.7.2" + +[[StaticArrays]] +deps = ["InteractiveUtils", "LinearAlgebra", "Random", "Statistics", "Test"] +git-tree-sha1 = "3841b39ed5f047db1162627bf5f80a9cd3e39ae2" +uuid = "90137ffa-7385-5640-81b9-e52037218182" +version = "0.10.3" + +[[Statistics]] +deps = ["LinearAlgebra", "SparseArrays"] +uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" + +[[StatsBase]] +deps = ["DataStructures", "LinearAlgebra", "Missings", "Printf", "Random", "SortingAlgorithms", "SparseArrays", "Statistics"] +git-tree-sha1 = "8a0f4b09c7426478ab677245ab2b0b68552143c7" +uuid = "2913bbd2-ae8a-5f71-8c99-4fb6c76f3a91" +version = "0.30.0" + +[[StatsFuns]] +deps = ["Rmath", "SpecialFunctions", "Test"] +git-tree-sha1 = "b3a4e86aa13c732b8a8c0ba0c3d3264f55e6bb3e" +uuid = "4c63d2b9-4356-54db-8cca-17b64c39e42c" +version = "0.8.0" + +[[SuiteSparse]] +deps = ["Libdl", "LinearAlgebra", "SparseArrays"] +uuid = "4607b0f0-06f3-5cda-b6b1-a6196a1729e9" + +[[TableTraits]] +deps = ["IteratorInterfaceExtensions"] +git-tree-sha1 = "b1ad568ba658d8cbb3b892ed5380a6f3e781a81e" +uuid = "3783bdb8-4a98-5b6b-af9a-565f29a5fe9c" +version = "1.0.0" + +[[Tables]] +deps = ["IteratorInterfaceExtensions", "LinearAlgebra", "Requires", "TableTraits", "Test"] +git-tree-sha1 = "9e748316f5aa7b7753c90de612ef98fe8b0ea297" +uuid = "bd369af6-aec1-5ad0-b16a-f7cc5008161c" +version = "0.2.1" + +[[Test]] +deps = ["Distributed", "InteractiveUtils", "Logging", "Random"] +uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" + +[[Trajectories]] +deps = ["RecipesBase", "Tables", "Test"] +git-tree-sha1 = "12e44c9a3517570935b14f9e3969fe2134cafff2" +uuid = "2c80a279-213e-54d7-a557-e9a14725db56" +version = "0.1.0" + +[[URIParser]] +deps = ["Test", "Unicode"] +git-tree-sha1 = "6ddf8244220dfda2f17539fa8c9de20d6c575b69" +uuid = "30578b45-9adc-5946-b283-645ec420af67" +version = "0.4.0" + +[[UUIDs]] +deps = ["Random", "SHA"] +uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" + +[[Unicode]] +uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" diff --git a/Project.toml b/Project.toml new file mode 100644 index 0000000..1632a2c --- /dev/null +++ b/Project.toml @@ -0,0 +1,31 @@ +name = "Bridge" +uuid = "2d3116d5-4b8f-5680-861c-71f149790274" +authors = ["Moritz Schauer "] +version = "0.10.0" + +[deps] +Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" +DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab" +Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f" +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +Polynomials = "f27b6e38-b328-58d1-80ce-0feddd5e7a45" +Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" +RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" +SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" +StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" +Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" +SuiteSparse = "4607b0f0-06f3-5cda-b6b1-a6196a1729e9" +Trajectories = "2c80a279-213e-54d7-a557-e9a14725db56" + +[compat] +julia = "≥ 0.7.0" + +[extras] +Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" +Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" +Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" + +[targets] +test = ["Statistics", "Documenter", "LinearAlgebra", "Test", "Random"] diff --git a/REQUIRE b/REQUIRE index 9728097..acf984c 100644 --- a/REQUIRE +++ b/REQUIRE @@ -3,4 +3,5 @@ Polynomials Distributions StaticArrays RecipesBase -Colors \ No newline at end of file +Colors +Trajectories diff --git a/docs/make.jl b/docs/make.jl index 7e1160f..620b108 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -3,7 +3,7 @@ using Bridge makedocs( modules = [Bridge], - format = :html, + format = Documenter.HTML(), sitename = "Bridge.jl", authors = "Moritz Schauer and contributors", pages = Any[ # Compat: `Any` for 0.4 compat @@ -18,7 +18,6 @@ makedocs( # for more information. deploydocs( repo = "github.com/mschauer/Bridge.jl.git", - julia = "0.7", target = "build", deps = nothing, make = nothing, diff --git a/docs/src/library.md b/docs/src/library.md index ed927c3..e6f1049 100644 --- a/docs/src/library.md +++ b/docs/src/library.md @@ -55,6 +55,7 @@ Bridge.NoDrift Bridge.R3! Bridge.σ! Bridge.b! +Bridge.kernelr3! ``` ## Levy processes diff --git a/landmarks/LowrankRiccati.jl b/landmarks/LowrankRiccati.jl new file mode 100644 index 0000000..9478b95 --- /dev/null +++ b/landmarks/LowrankRiccati.jl @@ -0,0 +1,93 @@ +module LowrankRiccati + +#= +include("LowrankRiccati.jl") +using .LowrankRiccati +=# +export lowrankriccati, lowrankriccati! + +using SparseArrays +using SuiteSparse +using LinearAlgebra +using Expokit # for matrix exponentials +using BenchmarkTools +using Distributions + + +# we use Expokit, which gives the funciton expmv! +# w = expmv!{T}( w::Vector{T}, t::Number, A, v::Vector{T}; kwargs...) +# The function expmv! calculates w = exp(t*A)*v, where A is a matrix or any type that supports size, eltype and mul! and v is a dense vector by using Krylov subspace projections. The result is stored in w. + +""" +Compute exp(A τ) * U +""" +function expmat(τ,A,U) + M = [] + for j in 1:size(U,2) + push!(M, expmv!(zeros(size(U,1)),τ,A,U[:,j])) + end + hcat(M...) +end + +""" + one step of low rank ricatti + implement Mena et al. for solving + (d P)/(d t) = A P(t) + P(t) A' + Q, P(t_0) = P0 + with a low rank approximation for P +""" +function lowrankriccati!(s, t, A, Q, (S,U), (Sout, Uout)) + τ = t-s # time step + Uᴬ, R = qr!(expmat(τ,A,U)) + Uᴬ = Matrix(Uᴬ) # convert from square to tall matrix + # step 4 + Sᴬ = R * S * R' + # step 5a (gives new U) + U, Ŝ = qr!(Uᴬ * Sᴬ + (τ * Q) * Uᴬ) + Uout .= Matrix(U) + # step 5b + Ŝ = Ŝ - Uout' * (τ * Q) * Uᴬ + # step 5c (gives new S) + L = Ŝ * Uᴬ' + Uout' * (τ * Q) + Sout .= L * Uout + + Sout, Uout +end +lowrankriccati(s, t, A, Q, (S, U)) = lowrankriccati!(s, t, A, Q, (S, U), (copy(S), copy(U))) + +using Test +@testset "low rank riccati" begin + + # Test problem + d = 30 + Q = sprand(d,d,.01) - I# Diagonal(randn(d)) + Q = Q * Q' + A = sprand(d,d,0.1)-I #Diagonal(diagels) + diagels = 0.4.^(1:d) + P0 = Diagonal(diagels) + + ld = 30 # low rank dimension + + s = 0.3 + t = 0.31 + τ = t - s + + # step 1 + M0 = eigen(Matrix(P0)) + S = Matrix(Diagonal(M0.values[1:ld])) + U = M0.vectors[:,1:ld] + + + # Low rank riccati solution to single time step + S, U = lowrankriccati(s, t, A, Q, (S, U)) + P = U * S * U' + + # Compute exact solution + λ = lyap(Matrix(A), -Matrix(Q)) + P0 + Φ = exp(Matrix(A)*τ) + + Pexact = Φ*λ*Φ' - λ + P0 + # assess difference + @test norm(P - Pexact) < 0.001 +end + +end diff --git a/landmarks/autodiff.jl b/landmarks/autodiff.jl new file mode 100644 index 0000000..cb34bdb --- /dev/null +++ b/landmarks/autodiff.jl @@ -0,0 +1,49 @@ + +using DualNumbers + +# small test to understand +ff(x) = x^3 +ff(Dual(2,1)) + +# Redefine +# const Point = SArray{Tuple{d},Float64,1,d} # point in R2 +# const Unc = SArray{Tuple{d,d},Float64,d,d*d} # Matrix presenting uncertainty +# to + +const Point{T} = SArray{Tuple{d},T,1,d} +const Unc{T} = SArray{Tuple{d,d},T,d,d*d} +# > Point{Dual{Float64}}(rand(3)) + +# matrix multiplication of mat of Uncs +function Base.:*(A::Array{Unc{T},2},B::Array{Unc{T},2}) where T + C = zeros(Unc{T},size(A,1), size(B,2)) + for i in 1:size(A,1) + for j in 1:size(B,2) + for k in 1:size(A,2) + C[i,j] += A[i,k] * B[k,j] + end + end + end + C +end + +function Base.:*(A::Array{Unc,2},x::State) + vecofpoints2state(A*vec(x)) +end + + +# DON'T NEED THIS PROBABLY +# +# function ll(x0,XX,Q) +# XX.yy[1] = deepvec2state(x0) +# llikelihood(LeftRule(), XX, Q; skip = 0) +# end +# y0 = deepvec(x0) +# ll(y0,XX,Q) +# +# F(XX,Q) = (x) -> ll(x, XX,Q) +# F(XX,Q)(y0) +# using ForwardDiff +# +# g = x0 -> ForwardDiff.gradient(F(XX,Q), x0) # g = ∇f +# g(deepvec(x0)) diff --git a/landmarks/automaticdiff_lm.jl b/landmarks/automaticdiff_lm.jl new file mode 100755 index 0000000..fecdf57 --- /dev/null +++ b/landmarks/automaticdiff_lm.jl @@ -0,0 +1,91 @@ +""" +Stochastic approximation to transition density. +Provide Wiener process. +""" +function slogpW(x0deepv, Lt0, Mt⁺0, μt0, xobst0, Q, Wᵒ) + x0 = deepvec2state(x0deepv) + Xᵒ = Bridge.solve(EulerMaruyama!(), x0, Wᵒ, Q)# this causes the problem + lptilde(vec(x0), Lt0, Mt⁺0, μt0, xobst0) + llikelihood(LeftRule(), Xᵒ, Q; skip = 1) +end +slogpW(Lt0, Mt⁺0, μt0, xobst0, Q, Wᵒ) = (x) -> slogpW(x, Lt0, Mt⁺0, μt0, xobst0, Q, Wᵒ) +∇slogpW(Lt0, Mt⁺0, μt0, xobst0, Q, Wᵒ) = (x) -> gradient(slogpW(Lt0, Mt⁺0, μt0, xobst0, Q, Wᵒ), x) + +function slogpWX(x0deepv, Lt0, Mt⁺0, μt0, xobst0, Q, Wᵒ,Xᵒ) # preferred way + x0 = deepvec2state(x0deepv) +# println(fieldnames(typeof(x0))) + solve!(EulerMaruyama!(), Xᵒ, x0, Wᵒ, Q) + lptilde(vec(x0), Lt0, Mt⁺0, μt0, xobst0) + llikelihood(LeftRule(), Xᵒ, Q; skip = 1) +end + + +""" + update initial momenta and/or guided proposals using either + sgd, sgld or mcmc +""" +function updatepath!(X,Xᵒ,W,Wᵒ,Wnew,ll,x,xᵒ,∇x, ∇xᵒ, + sampler,(Lt0, Mt⁺0, μt0, xobst0, Q),mask, mask_id, δ, ρ, acc) + if sampler in [:sgd, :sgld] + sample!(W, Wiener{Vector{StateW}}()) + cfg = GradientConfig(slogpW(Lt0, Mt⁺0, μt0, xobst0, Q, W), x, Chunk{d*P.n}()) # 2*d*P.n is maximal + @time gradient!(∇x, slogpW(Lt0, Mt⁺0, μt0, xobst0, Q, W),x,cfg) + + if sampler==:sgd + x .+= δ*mask.*∇x + end + if sampler==:sgld + x .+= .5*δ*mask.*∇x + sqrt(δ)*mask.*randn(2d*Q.target.n) + end + xstate = deepvec2state(x) + Bridge.solve!(EulerMaruyama!(), X, xstate, W, Q) + obj = lptilde(vec(xstate), Lt0, Mt⁺0, μt0, xobst0) + + llikelihood(LeftRule(), X, Q; skip = 1) + println("ll ", obj) + end + if sampler==:mcmc + # Update W + sample!(Wnew, Wiener{Vector{PointF}}()) + Wᵒ.yy .= ρ * W.yy + sqrt(1-ρ^2) * Wnew.yy + solve!(EulerMaruyama!(), Xᵒ, deepvec2state(x), Wᵒ, Q) + + llᵒ = llikelihood(Bridge.LeftRule(), Xᵒ, Q,skip=sk) + print("ll $ll $llᵒ, diff_ll: ",round(llᵒ-ll;digits=3)) + + if log(rand()) <= llᵒ - ll + X.yy .= Xᵒ.yy + W.yy .= Wᵒ.yy + ll = llᵒ + print("update innovation acceptedœ“") + acc[1] +=1 + else + print("update innovation rejected") + end + println() + + # MALA step (update x) + ∇x .= ∇slogpW(Lt0, Mt⁺0, μt0, xobst0, Q, W)(x) + xᵒ .= x .+ .5*δ * mask.* (∇x .+ sqrt(δ) * randn(length(x))) + ∇xᵒ .= ∇slogpW(Lt0, Mt⁺0, μt0, xobst0, Q, W)(xᵒ) + + xstate = deepvec2state(x) + xᵒstate = deepvec2state(xᵒ) + solve!(EulerMaruyama!(), Xᵒ, xᵒstate, W, Q) + ainit = lptilde(vec(xᵒstate), Lt0, Mt⁺0, μt0, xobst0) + llikelihood(LeftRule(), Xᵒ, Q; skip = 1) - + lptilde(vec(xstate), Lt0, Mt⁺0, μt0, xobst0) - llikelihood(LeftRule(), X, Q; skip = 1) - + logpdf(MvNormal(d*P.n,sqrt(δ)),(xᵒ - x - .5*δ* mask.* ∇x)[mask_id]) + + logpdf(MvNormal(d*P.n,sqrt(δ)),(x - xᵒ - .5*δ* mask.* ∇xᵒ)[mask_id]) + # compute acc prob + print("ainit: ", ainit) + if log(rand()) <= ainit + x .= xᵒ + xstate = xᵒstate + X.yy .= Xᵒ.yy + println("mala step acceptedœ“") + acc[2] +=1 + else + println("mala step rejectedœ“") + end + obj = lptilde(vec(xstate), Lt0, Mt⁺0, μt0, xobst0) + + llikelihood(LeftRule(), X, Q; skip = 1) + end + X,Xᵒ,W,Wᵒ,ll,x,xᵒ,∇x,∇xᵒ, obj,acc +end diff --git a/landmarks/back/lmpar.jl b/landmarks/back/lmpar.jl new file mode 100755 index 0000000..053fa2d --- /dev/null +++ b/landmarks/back/lmpar.jl @@ -0,0 +1,235 @@ +# reminder, to type H*, do H\^+ +#outdir="output/landmarks/" +#cd("/Users/Frank/.julia/dev/Bridge/landmarks") + +using Bridge, StaticArrays, Distributions +using Bridge:logpdfnormal +using Test, Statistics, Random, LinearAlgebra +using Bridge.Models +using DelimitedFiles +using DataFrames +using CSV +using RCall +using Base.Iterators +using SparseArrays +using LowRankApprox + +models = [:ms, :ahs] +model = models[1] +TEST = false#true +partialobs = true + + +const d = 2 +const itostrat = false + +n = 30 # nr of landmarks + +θ = 2π/5# π/6 0#π/5 # angle to rotate endpoint + +σobs = 10^(-2) # noise on observations +println(model) + +T = 0.75#1.0#0.5 +t = 0.0:0.01:T # time grid + +#Random.seed!(5) +include("state.jl") +include("models.jl") +include("patches.jl") +include("lmguiding.jl") + +### Specify landmarks models + +### Specify landmarks models +a = 3.0 # the larger, the stronger landmarks behave similarly +λ = 0.0; #= not the lambda of noise fields =# γ = 8.0 +db = 6.0 # domainbound +nfstd = 2.0 # tau , widht of noisefields +r1 = -db:nfstd:db +r2 = -db:nfstd:db +nfloc = Point.(collect(product(r1, r2)))[:] +nfscales = [.1Point(1.0, 1.0) for x in nfloc] # intensity + +nfs = [Noisefield(δ, λ, nfstd) for (δ, λ) in zip(nfloc, nfscales)] +Pms = MarslandShardlow(a, γ, λ, n) +Pahs = Landmarks(a, λ, n, nfs) +### + +StateW = Point{Float64} +if model == :ms + dwiener = n + P = Pms +else + dwiener = length(nfloc) + P = Pahs +end + +w0 = zeros(StateW, dwiener) +W = SamplePath(t, [copy(w0) for s in t]) +sample!(W, Wiener{Vector{StateW}}()) + +# specify initial landmarks configuration +q0 = [Point(2.0cos(t), sin(t)) for t in (0:(2pi/n):2pi)[1:n]] #q0 = circshift(q0, (1,)) +p_ = 2*Point(-0.1, 0.1) +p0 = [p_ for i in 1:n] # +#p0 = [randn(Point) for i in 1:n] +x0 = State(q0, p0) + +#Random.seed!(1234) +X = SamplePath(t, [copy(x0) for s in t]) +println("Solve for forward provess:") +@time solve!(EulerMaruyama!(), X, x0, W, P) + #@time solve!(StratonovichHeun!(), X, x0, W, P) + +tc(t,T) = t.* (2 .-t/T) +tt_ = tc(t,T)#tc(t,T)# 0:dtimp:(T) +#tt_ = t + +# observe positions WITH noise +v0 = q(X.yy[1]) + σobs * randn(PointF,n) +rot = SMatrix{2,2}(cos(θ), sin(θ), -sin(θ), 2*cos(θ)) +#vT = [rot * X.yy[end].q[i] for i in 1:P.n ] +trans = SMatrix{2,2}(1.5, 1.0, 0.0, 1.0) +vT = [rot * trans * X.yy[end].q[i] for i in 1:P.n ] + + + +#################### +# solve backward recursion on [0,T] +if partialobs==true + L = [(i==j)*one(Unc) for i in 1:2:2n, j in 1:2n] + Σ = [(i==j)*σobs^2*one(Unc) for i in 1:n, j in 1:n] + #Σ = 10 * reshape(rand(Unc,n^2),n,n) + μend = zeros(Point{Float64},P.n) + xobs = vT + mT = zero(vT)#rand(Point, n)# + Pahsaux = LandmarksAux(Pahs, State(vT, mT)) + Pmsaux = MarslandShardlowAux(Pms, State(vT, mT)) +else + # full observation case + L = [(i==j)*one(Unc) for i in 1:2n, j in 1:2n] + Σ = [(i==j)*σobs^2*one(Unc) for i in 1:2n, j in 1:2n] + μend = zeros(Point{Float64}, 2P.n) + xobs = vec(X.yy[end]) + Pahsaux = LandmarksAux(Pahs, X.yy[end]) + Pmsaux = MarslandShardlowAux(Pms, X.yy[end]) +end + +if model == :ms + Paux = Pmsaux +else + Paux = Pahsaux +end + +# initialise Lt and M⁺t +Lt = [copy(L) for s in tt_] +Mt⁺ = [copy(Σ) for s in tt_] +μt = [copy(μend) for s in tt_] +println("compute guiding term:") +@time (Lend, Mend⁺, μend) = guidingbackwards!(Lm(), tt_, (Lt, Mt⁺,μt), Paux, (L, Σ, μend)) +(L0, M0⁺, μ0, V) = lmgpupdate(Lend, Mend⁺, μend, vT, Σ, L, v0) +# issymmetric(deepmat(Bridge.a(0,Paux))) +# isposdef(deepmat(Bridge.a(0,Paux))) +# map(x->minimum(eigen(deepmat(x)).values),Mt⁺) +#Mt = map(X -> deepmat2unc(inv(deepmat(X))),Mt⁺) +println("Compute Cholesky for Mt⁺:") +@time Mt = map(X -> InverseCholesky(lchol(X)),Mt⁺) + +Q = GuidedProposall!(P, Paux, tt_, Lt, Mt, μt, xobs) +if partialobs + xinit = State(v0, rand(Point{Float64}, P.n)) +else + xinit = x0 +end +winit = zeros(StateW, dwiener) +XX = SamplePath(tt_, [copy(xinit) for s in tt_]) +WW = SamplePath(tt_, [copy(winit) for s in tt_]) +sample!(WW, Wiener{Vector{StateW}}()) + +println("Sample guided proposal:") +@time Bridge.solve!(EulerMaruyama!(), XX, xinit, WW, Q) +#guid = guidingterms(XX,Q) + +include("plotlandmarks.jl") + + +if model==:ms + @time llikelihood(LeftRule(), XX, Q; skip = 0) # won't work for AHS because matrix multilication for Htilde is not defined yet +end + + + +using ForwardDiff +dual(x, i, n) = ForwardDiff.Dual(x, ForwardDiff.Chunk{n}(), Val(i)) +dual(x, n) = ForwardDiff.Dual(x, ForwardDiff.Chunk{n}(), Val(0)) +#= +#using Flux +xinitv = deepvec(xinit) + +xinitv = map(i->dual(xinitv[i], i <= 2 ? i : 0, 2), 1:length(xinitv)) + +xinitnew = deepvec2state(xinitv) +x = copy(xinitnew) + +#lux.Tracker.gradient(x -> Bridge._b!((1,0.0), deepvec2state(x), deepvec2state(x), P), deepvec(xinit)) +Bridge.b!(0.0, x, copy(x), P) + +import Bridge; + +#include(joinpath(dirname(pathof(Bridge)), "..", "landmarks/patches.jl")) +#include(joinpath(dirname(pathof(Bridge)), "..", "landmarks/models.jl")) + +XX = Bridge.solve(EulerMaruyama!(), xinitnew, WW, P) +=# + + + +function obj(xinitv) + xinit = deepvec2state(xinitv) + sample!(WW, Wiener{Vector{StateW}}()) + XXᵒ = Bridge.solve(EulerMaruyama!(), xinit, WW, Q) + ( + (lptilde(vec(xinit), L0, M0⁺, μ0, V, Q) - lptilde(vec(x0), L0, M0⁺, μ0, V, Q)) + + llikelihood(LeftRule(), XXᵒ, Q; skip = 1) + ) +end +using Makie +using Random +Random.seed!(2) +let + x = deepvec(x0) + x = x .* (1 .+ 0.2*randn(length(x))) + + x = deepvec(State(x0.q, deepvec2state(x).p)) + + s = deepvec2state(x) + n = Node(s.q) + n2 = Node(s.p) + sc = scatter(x0.q, color=:red) # plot locations of x0 in red + + scatter!(sc, n2) # plot momenta of x0 in black + scatter!(sc, n, color=:blue) # plot locations of x0 (again) in blue + display(sc) + + # only optimize momenta + mask = deepvec(State(0 .- 0*xinit.q, 1 .- 0*(xinit.p))) + ϵ = 6.e-4 + @show o = obj(x) + + for i in 1:1000 + #record(sc, "output/gradientdescent.mp4", 1:100) do i + #i % 10 == 0 && (o = obj(x)) + for k in 1:1 + ∇x = ForwardDiff.gradient(obj, x) + x .+= ϵ*mask.*∇x + end + s = deepvec2state(x) + n[] = s.q + n2[] = s.p + display(s-x0) + println("$i d(x,xtrue) = ", norm(deepvec(x0)-x))#, " ", o) + end +end + +# should first plot v0 and vT, should randomly initialise momenta and see whether these convert to p0 diff --git a/landmarks/forward.jl b/landmarks/forward.jl new file mode 100644 index 0000000..7107b8a --- /dev/null +++ b/landmarks/forward.jl @@ -0,0 +1,357 @@ +# reminder, to type H*, do H\^+ +#outdir="output/landmarks/" +#cd("/Users/Frank/.julia/dev/Bridge/landmarks") +#cd("landmarks") + + +using Bridge, StaticArrays, Distributions +using Bridge:logpdfnormal +using Test, Statistics, Random, LinearAlgebra +using Bridge.Models +using DelimitedFiles +using DataFrames +using CSV +using RCall +using Base.Iterators +using SparseArrays +using Trajectories +using LowRankApprox + +models = [:ms, :ahs] +model = models[2] +TEST = false#true + +discrmethods = [:ralston, :lowrank, :psd, :lm, :marcin] # not a good name anymore +discrmethod = discrmethods[1] +LM = discrmethod == :lm + +obsschemes =[:full, :partial] +obsscheme = obsschemes[2] + +const d = 2 +const itostrat = false + +n = 15 # nr of landmarks +ldim = 40 # dimension of low-rank approximation to H\^+ + +cheat = false #true#false # if cheat is true, then we initialise at x0 (true value) and +# construct the guiding term based on xT (true value) + +θ = 0.0 #-π/6# π/6 0#π/5 # angle to rotate endpoint + + +ϵ = 10.0^(-4) # parameter for initialising Hend⁺ +σobs = 10.0^(-2) # noise on observations + +println(model) +println(discrmethod) +println(obsscheme) + +T = 1.0#1.0#0.5 +t = 0.0:0.01:T # time grid + +Random.seed!(5) +#include("nstate.jl") +include("ostate.jl") +include("state.jl") + +include("models.jl") +include("patches.jl") +if discrmethod == :lm + include("lmguiding.jl") +else + include("guiding.jl") +end +include("LowrankRiccati.jl") +using .LowrankRiccati + + +### Specify landmarks models +a = 3.0 # the larger, the stronger landmarks behave similarly +λ = 0.0 # not the lambda of noise fields, but the mean reversion +γ = 1.0 +db = 3.0 # domainbound +nfstd = 1.0 # tau , width of noisefields +r1 = -db:nfstd:db +r2 = -db:nfstd:db +nfloc = PointF.(collect(product(r1, r2)))[:] +nfscales = [.1PointF(1.0, 1.0) for x in nfloc] # intensity + +nfs = [Noisefield(δ, λ, nfstd) for (δ, λ) in zip(nfloc, nfscales)] +Pms = MarslandShardlow(a, γ, λ, n) +Pahs = Landmarks(a, λ, n, nfs) +### + +StateW = PointF +if model == :ms + dwiener = n + P = Pms +else + dwiener = length(nfloc) + P = Pahs +end + +# specify initial landmarks configuration +q0 = [PointF(2.5cos(t), sin(t)) for t in (0:(2pi/n):2pi)[1:n]] #q0 = circshift(q0, (1,)) +p_ = 10*PointF(0.1, 0.1) +p0 = [p_ for i in 1:n] # +#p0 = [randn(PointF) for i in 1:n] +x0 = State(q0, p0) + +#Random.seed!(1234) +w0 = zeros(StateW, dwiener) +W = SamplePath(t, [copy(w0) for s in t]) +X = SamplePath(t, [copy(x0) for s in t]) +sample!(W, Wiener{Vector{StateW}}()) +println("Sample forward process:") +@time solve!(EulerMaruyama!(), X, x0, W, P) +#@time solve!(StratonovichHeun!(), X, x0, W, P) + +# compute Hamiltonian along path +ham = [hamiltonian(X.yy[i], Pms) for i in 1:length(t)] + +tc(t,T) = t.*(2 .-t/T) +tt_ = tc(t,T)#tc(t,T)# 0:dtimp:(T) + + + +#################### +if obsscheme==:partial + L = deepmat( [(i==j)*one(Unc) for i in 1:2:2n, j in 1:2n]) + Σ = Diagonal(σobs^2*ones(n*d)) + + # observe positions + v0 = q(X.yy[1]) + σobs * randn(PointF,n) + rot = SMatrix{2,2}(cos(θ), sin(θ), -sin(θ), cos(θ)) + shft = 0. + vT = [shft .+ rot * X.yy[end].q[i] + σobs * randn(d) for i in 1:P.n ] + + Pmsaux = MarslandShardlowAux(Pms, State(vT, zero(vT))) + if cheat + Pahsaux = LandmarksAux(Pahs, X.yy[end]) + else + Pahsaux = LandmarksAux(Pahs, State(vT, zero(vT))) + #Pahsaux = LandmarksAux(Pahs, State(vT, rand(PointF,Pahs.n))) + end +elseif obsscheme==:full + L = deepmat( [(i==j)*one(Unc) for i in 1:2n, j in 1:2n]) + Σ = Diagonal(σobs^2*ones(2n*d)) + Pmsaux = MarslandShardlowAux(Pms,X.yy[end]) + Pahsaux = LandmarksAux(Pahs, X.yy[end]) + v0 = vec(X.yy[1]) + vT = vec(X.yy[end]) +end + + + +#Paux = (model==:ms) ? Pmsaux : Pahsaux +# solve backward recursion on [0,T] +if LM + + if obsscheme == :partial + Lend = deepmat2unc(L) + Σend = deepmat2unc(Matrix(Σ)) + μend = zeros(PointF, n) + xobs = vT + else + # full observation case +# L = [(i==j)*one(Unc) for i in 1:2n, j in 1:2n] +# Σ = [(i==j)*σobs^2*one(Unc) for i in 1:2n, j in 1:2n] + μend = zeros(PointF, 2n) + end +else + if obsscheme == :partial + #νT = State(zero(vT), zero(vT)) + νT = State(randn(PointF,Pahs.n), 0randn(PointF,Pahs.n)) + elseif obsscheme == :full + νT = X.yy[end] + end + HT⁺ = reshape(zeros(Unc,4n^2),2n,2n) + for i in 1:n + HT⁺[2i-1,2i-1] = one(Unc)/ϵ # high variance on positions + if discrmethod==:lowrank + HT⁺[2i,2i] = one(Unc)*10^(-4) # still need to figure out why + else + HT⁺[2i,2i] = one(Unc)/10^(-4) + end + end + #### perform gpupdate step + νT , HT⁺, CT = gpupdate(νT, HT⁺, 0.0, Σ, L, vT) + C = CT + ν = copy(νT) + H⁺ = copy(HT⁺) + Pahsaux = LandmarksAux(Pahs, copy(ν)) # this might be a good idea + Pmsaux = MarslandShardlowAux(Pms, copy(ν)) # this might be a good idea +end + +if model == :ms + Paux = Pmsaux +else + Paux = Pahsaux +end + + +# L, and Σ are ordinary matrices, vT an array of Points +# ν is a state , H⁺ a UncMat +if LM + + # initialise Lt and M⁺t + Lt = [copy(Lend) for s in tt_] + Mt⁺ = [Matrix(Σend) for s in tt_] + μt = [copy(μend) for s in tt_] + println("compute guiding term:") + @time (Lend, Mend⁺, μend) = guidingbackwards!(Lm(), tt_, (Lt, Mt⁺,μt), Paux, (L, Σ, μend)) + + # issymmetric(deepmat(Bridge.a(0,Paux))) + # isposdef(deepmat(Bridge.a(0,Paux))) + # map(x->minimum(eigen(deepmat(x)).values),Mt⁺) + #Mt = map(X -> deepmat2unc(inv(deepmat(X))),Mt⁺) + println("Compute Cholesky for Mt⁺:") + @time Mt = map(X -> InverseCholesky(lchol(X)),Mt⁺) + + Q = GuidedProposall!(P, Paux, tt_, Lt, Mt, μt, xobs) + (Lstart, Mstart⁺, μstart) = lmgpupdate(Lend, Mend⁺, μend, Σ, L, v0) + xinit = x0 + + + +else + νt = [copy(ν) for s in tt_] + println("Compute guiding term:") + if discrmethod==:lowrank + M0 = eigen(deepmat(H⁺)) + largest = sortperm(M0.values)[end-ldim+1:end] + S = Matrix(Diagonal(M0.values[largest])) + U = M0.vectors[:,largest] + St = [copy(S) for s in tt_] + Ut = [copy(U) for s in tt_] + @time ν, (S, U) = bucybackwards!(LRR(), tt_, νt, (St, Ut), Paux, ν, (S, U)) + H⁺ = deepmat2unc(U * S * U') + Ht = map((S,U) -> deepmat2unc(U * inv(S) * U'), St, Ut) # directly compute Mt + #Ht = map((S,U) -> LowRank(S,U), St,Ut) + elseif discrmethod==:ralston + H⁺t = [copy(H⁺) for s in tt_] + Ht = map(H⁺ -> InverseCholesky(lchol(H⁺ + I)), H⁺t) + @time ν , H⁺, H, C = bucybackwards!(Bridge.R3!(), tt_, νt, H⁺t, Ht, Paux, ν, H⁺, C) +# Ht = map(H⁺ -> InverseCholesky(lchol(H⁺)),H⁺t) + elseif discrmethod==:psd + H⁺t = [copy(H⁺) for s in tt_] + @time ν , H⁺, C = bucybackwards!(Lyap(), tt_, νt, H⁺t, Paux, ν, H⁺) + # println(map(x->isposdef(deepmat(x)),H⁺t)) + Ht = map(H⁺ -> InverseCholesky(lchol(H⁺)),H⁺t) + end + +# careful, not a state + νstart , Hstart⁺, Cstart = gpupdate(ν , H⁺, C, Σ, L, v0) + #also update C?? + Q = GuidedProposal!(P, Paux, tt_, νt, Ht, νstart, InverseCholesky(lchol(Hstart⁺)), Cstart) + xinit = cheat ? x0 : State(νstart.q, 0νstart.q)#νstart # or xinit ~ N(νstart, Hstart⁺) +end +winit = zeros(StateW, dwiener) +XX = SamplePath(tt_, [copy(xinit) for s in tt_]) +WW = SamplePath(tt_, [copy(winit) for s in tt_]) +sample!(WW, Wiener{Vector{StateW}}()) + +# adjust xinit as test +xinit = State(v0, [Point(3,3) for i in 1:P.n] + rand(Point{Float64}, P.n)) + +println("Sample guided bridge proposal:") +@time Bridge.solve!(EulerMaruyama!(), XX, xinit, WW, Q) +#error("STOP EARLY") +include("plotlandmarks.jl") + +if model==:ms + @time llikelihood(LeftRule(), XX, Q; skip = 0) # won't work for AHS because matrix multilication for Htilde is not defined yet +end + +using ForwardDiff +dual(x, i, n) = ForwardDiff.Dual(x, ForwardDiff.Chunk{n}(), Val(i)) +dual(x, n) = ForwardDiff.Dual(x, ForwardDiff.Chunk{n}(), Val(0)) +#= +#using Flux +xinitv = deepvec(xinit) + +xinitv = map(i->dual(xinitv[i], i <= 2 ? i : 0, 2), 1:length(xinitv)) + +xinitnew = deepvec2state(xinitv) +x = copy(xinitnew) + +#lux.Tracker.gradient(x -> Bridge._b!((1,0.0), deepvec2state(x), deepvec2state(x), P), deepvec(xinit)) +Bridge.b!(0.0, x, copy(x), P) + +import Bridge; + +#include(joinpath(dirname(pathof(Bridge)), "..", "landmarks/patches.jl")) +#include(joinpath(dirname(pathof(Bridge)), "..", "landmarks/models.jl")) + +XX = Bridge.solve(EulerMaruyama!(), xinitnew, WW, P) +=# + + +function obj(xinitv) + xinit = deepvec2state(xinitv) + sample!(WW, Wiener{Vector{StateW}}()) + if !isdefined(Main, :XXᵒ_) + XXᵒ_ = Bridge.solve(EulerMaruyama!(), xinit, WW, Q) + else + Bridge.solve!(EulerMaruyama!(), XXᵒ_, xinit, WW, Q) + end + ( + (lptilde(xinit, Q) - lptilde(x0, Q)) + llikelihood(LeftRule(), XXᵒ_, Q; skip = 1) + ) +end + +MAKIE = false +if MAKIE + using Makie +end +using Random, Profile +Random.seed!(2) +let + x = deepvec(x0) + #x = deepvec(State(x0.q, 0.5 * x0.p)) + x = x .* (1 .+ 0.2*randn(length(x))) + + x = deepvec(State(x0.q, deepvec2state(x).p)) + + + s = deepvec2state(x) + if MAKIE + n = Node(s.q) + n2 = Node(s.p) + sc = scatter(x0.q, color=:red) + + scatter!(sc, n2) + scatter!(sc, n, color=:blue) + display(sc) + end + # only optimize momenta + mask = deepvec(State(0 .- 0*xinit.q, 1 .- 0*(xinit.p))) + ϵ = 6.e-2 + #@show o = obj(x) + ∇x = ForwardDiff.gradient(obj, x) + x .+= ϵ*mask.*∇x + s = deepvec2state(x) + if MAKIE + n[] = s.q + n2[] = s.p + end + display(s-x0) + + + @profile for i in 1:1000 + #record(sc, "output/gradientdescent.mp4", 1:100) do i + #i % 10 == 0 && (o = obj(x)) + for k in 1:1 + ∇x = ForwardDiff.gradient(obj, x) + x .+= ϵ*mask.*∇x + end + s = deepvec2state(x) + if MAKIE + n[] = s.q + n2[] = s.p + end + display(s-x0) + println("$i d(x,xtrue) = ", norm(deepvec(x0)-x))#, " ", o) + end +end diff --git a/landmarks/generatedata.jl b/landmarks/generatedata.jl new file mode 100644 index 0000000..e7a7c5f --- /dev/null +++ b/landmarks/generatedata.jl @@ -0,0 +1,76 @@ +""" +Generate landmarks data, or read landmarks data. +- dataset: specifies type of data to be generated/loaded +- P: +- t: grid used for generating data +- σobs: standard deviation of observation noise + +Returns +- x0: initial state without noise +- xobs0, xobsT: (observed initial and final states with noise) +- Xf: forward simulated path +- P: only adjusted in case of 'real' data, like the bearskull data + +Example + x0, xobs0, xobsT, Xf, P = generatedata(dataset,P,t,σobs) + +""" +function generatedata(dataset,P,t,σobs) + + n = P.n + if P isa MarslandShardlow + dwiener = P.n + else + dwiener = length(P.nfs) + end + if dataset=="forwardsimulated" + q0 = [PointF(2.0cos(t), sin(t)) for t in (0:(2pi/n):2pi)[1:n]] #q0 = circshift(q0, (1,)) + p0 = [Point(1.0, -3.0) for i in 1:n] # #p0 = [randn(Point) for i in 1:n] + x0 = State(q0, p0) + @time Wf, Xf = landmarksforward(t, dwiener, x0, P) + xobs0 = x0.q + σobs * randn(PointF,n) + xobsT = [Xf.yy[end].q[i] for i in 1:P.n ] + σobs * randn(PointF,n) + end + if dataset in ["shifted","shiftedextreme"] # first stretch, then rotate, then shift; finally add noise + q0 = [PointF(2.0cos(t), sin(t)) for t in (0:(2pi/n):2pi)[1:n]] #q0 = circshift(q0, (1,)) + p0 = [PointF(1.0, -3.0) for i in 1:n] # #p0 = [randn(Point) for i in 1:n] + x0 = State(q0, p0) + @time Wf, Xf = landmarksforward(t, dwiener, x0, P) + xobs0 = x0.q + σobs * randn(PointF,n) + if dataset == "shifted" θ, η = π/10, 0.2 end + if dataset == "shiftedextreme" θ, η = π/5, 0.4 end + rot = SMatrix{2,2}(cos(θ), sin(θ), -sin(θ), cos(θ)) + stretch = SMatrix{2,2}(1.0 + η, 0.0, 0.0, 1.0 - η) + shift = PointF(0.1,-0.1) + xobsT = [rot * stretch * xobs0[i] + shift for i in 1:P.n ] + σobs * randn(PointF,n) + end + if dataset=="bear" + cd("/Users/Frank/github/BridgeLandmarks/landmarks/beardata") + bear0 = readdlm("bear1.csv",',') + bearT = readdlm("bear2.csv",',') + nb = size(bear0)[1] + avePoint = Point(414.0, 290.0) # average of (x,y)-coords for bear0 to center figure at origin + xobs0 = [Point(bear0[i,1], bear0[i,2]) - avePoint for i in 1:nb]/200. + xobsT = [Point(bearT[i,1], bearT[i,2]) - avePoint for i in 1:nb]/200. + # need to redefine P, because of n + if model == :ms + P = MarslandShardlow(a, γ, λ, nb) + else + P = Landmarks(a, 0.0, nb, nfs) + end + x0 = State(xobs0, rand(PointF,P.n)) + Wf, Xf = landmarksforward(t, dwiener, x0, P) + end + if dataset=="heart" + q0 = [PointF(2.0cos(t), 2.0sin(t)) for t in (0:(2pi/n):2pi)[1:n]] #q0 = circshift(q0, (1,)) + p0 = [PointF(1.0, -3.0) for i in 1:n] # #p0 = [randn(Point) for i in 1:n] + x0 = State(q0, p0) + @time Wf, Xf = landmarksforward(t, dwiener, x0, P) + xobs0 = x0.q + σobs * randn(PointF,n) + heart_xcoord(s) = 0.2*(13cos(s)-5cos(2s)-2cos(3s)-cos(4s)) + heart_ycoord(s) = 0.2*16(sin(s)^3) + qT = [PointF(heart_xcoord(t), heart_ycoord(t)) for t in (0:(2pi/n):2pi)[1:n]] #q0 = circshift(q0, (1,)) + xobsT = qT + σobs * randn(PointF,n) + end + x0, xobs0, xobsT, Xf, P +end diff --git a/landmarks/guiding.jl b/landmarks/guiding.jl new file mode 100644 index 0000000..5409e49 --- /dev/null +++ b/landmarks/guiding.jl @@ -0,0 +1,269 @@ +# compute guiding term: backward ode +# compute likelihood of guided proposal +using LinearAlgebra + +import Bridge: kernelr3!, R3!, target, auxiliary, constdiff, llikelihood, _b!, B!, σ!, b!, σ, b + + +function gpupdate(ν, P, Σ, L, v) + if all(diag(P) .== Inf) + P_ = inv(L' * inv(Σ) * L) + V_ = (L' * inv(Σ) * L)\(L' * inv(Σ) * v) + return V_, P_ + else + Z = I - P*L'*inv(Σ + L*P*L')*L + return Z*P*L'*inv(Σ)*v + Z*ν, Z*P + end +end + +function gpupdate(ν, P, C, Σ, L, v) + m, d = size(L) + @assert m == length(v) + if all(diag(P) .== Inf) + P_ = inv(L' * inv(Σ) * L) + V_ = (L' * inv(Σ) * L)\(L' * inv(Σ) * v) + C += 0.5 * dot(v, Σ\v) + C += length(v)/2*log(2π) + 0.5*logdet(Σ) + return V_, P_, C + else + Z = I - P*L'*inv(Σ + L*P*L')*L + C += 0.5 * dot(v, Σ\v) + C += length(v)/2*log(2π) + 0.5*logdet(Σ) + return Z*P*L'*inv(Σ)*v + Z*ν, Z*P, C + + end +end + + +""" +(ν, P) are the values just to the right of time T, +(Σ, L, v) are the noise covariance, observations matrix L and observation at time T +""" +function gpupdate(ν2::State, P2, C, Σ, L, v2::Vector{<:Point}) + + ν = deepvec(ν2) + P = deepmat(P2) + v = reinterpret(Float64,v2) + if all(diag(P) .== Inf) + P_ = inv(L' * inv(Σ) * L) + Σᴵv = inv(Σ) * v + V_ = (L' * inv(Σ) * L)\(L' * Σᴵv) + # P_ = inv(L' * inv(Σ) * L + 10^(-6)*I) + # V_ = P_ * L' * inv(Σ) * v + C += 0.5 * dot(v, Σᴵv) + C += length(v)/2*log(2π) + 0.5*logdet(Σ) + return deepvec2state(V_), deepmat2unc(P_), C + else + Z = I - P*L'*inv(Σ + L*P*L')*L + C += 0.5 * dot(v, Σ\v) + C += length(v)/2*log(2π) + 0.5*logdet(Σ) + + return deepvec2state(Z*P*L'*inv(Σ)*v + Z*ν), deepmat2unc(Z*P), C + # P_ = inv(L' * inv(Σ) * L + inv(P)) + # V_ = P_ * L' * inv(Σ) * v + P_ * inv(P) * ν + # return deepvec2state(V_), deepmat2unc(P_) + end +end + +struct Arg4Closure{B,T} + f::B + arg4::T +end +(f::Arg4Closure)(arg1, arg2, arg3) = f.f(arg1, arg2, arg3, f.arg4) + +""" +Replacing out with dP, which is B*arg + arg*B'- tilde_a +""" +function dP!(t, p, out, P::Union{LandmarksAux, MarslandShardlowAux}) + B!(t, p, out, P) + out .= out .+ out' - a(t, P) + out +end + +# """ +# Make a 4-tuple where each tuple contains a copy of y +# """ +# Bridge.workspace(::Bridge.R3!, y) = (copy(y), copy(y), copy(y), copy(y)) + +""" +Computes solutions to backwards filtering odes for ν and H⁺ and an interval, say (S,T]. +Initialisations for the odes on the right are given by arguments ν, H⁺, C +Writes ν and H⁺ into νt and H⁺t (for all elements in t) and returns ν, H⁺, H, C at S+ +""" +function bucybackwards!(S::R3!, t, νt, H⁺t, Ht, Paux, ν, H⁺, C) + H⁺t[end] = H⁺ + Ht[end] = InverseCholesky(lchol(H⁺)) + νt[end] = ν + wsν = Bridge.workspace(S, ν) + wsH⁺ = Bridge.workspace(S, H⁺) + b! = Arg4Closure(Bridge.b!, Paux) + dP! = Arg4Closure(Bridge.dP!, Paux) + for i in length(t)-1:-1:1 + dt = t[i] - t[i+1] + kernelr3!(b!, t[i+1], νt[i+1], wsν, νt[i], dt) + kernelr3!(dP!, t[i+1], H⁺t[i+1], wsH⁺, H⁺t[i], dt) + Ht[i] = InverseCholesky(lchol(H⁺t[i])) + F = Ht[i+1]*νt[i+1] + C += dot(Bridge.β(t, Paux),F)*dt + C += 0.5*dot(F, Bridge.a(t, Paux)*F)*dt + C += -0.5*tr(tr(Ht[i+1]*Matrix(Bridge.a(t, Paux))))*dt + # FIXME converting to full + end + νt[1], H⁺t[1], Ht[1], C +end + + +struct LRR end # identifier to call Bucy backwards for low rank riccati + +function bucybackwards!(scheme::LRR, t, νt, (St, Ut), Paux, νend, (Send, Uend)) + St[end], Ut[end] = Send, Uend + νt[end] = νend + wsν = Bridge.workspace(R3!(), νend) + ã = deepmat(Bridge.a(t[end],Paux)) + B̃ = sparse(deepmat(Bridge.B(t[end],Paux))) + for i in length(t)-1:-1:1 + dt = t[i] - t[i+1] + kernelr3!(Arg4Closure(Bridge.b!, Paux), t[i+1], νt[i+1], wsν, νt[i], dt) + LowrankRiccati.lowrankriccati!(t[i], t[i+1], -B̃, ã , (St[i+1], Ut[i+1]), (St[i], Ut[i])) + + end + νt[1], (St[1], Ut[1]) +end + + +### for lyanpunov psd still needs to be fixed +struct Lyap end + +function bucybackwards!(::Lyap, t, νt, H⁺t, Paux, νend, Hend⁺) + H⁺t[end] = Hend⁺ + νt[end] = νend + wsν = Bridge.workspace(R3!(), νend) + B̃ = Matrix(Bridge.B(0.0, Paux)) + ã = Bridge.a(0.0, Paux) + for i in length(t)-1:-1:1 + dt = t[i] - t[i+1] + kernelr3!(Arg4Closure(Bridge.b!, Paux), t[i+1], νt[i+1], wsν, νt[i], dt) + lyapunovpsdbackward_step!(t[i+1], -dt, Paux, H⁺t[i+1], H⁺t[i], B̃, ã) # maybe with -dt + end + νt[1], H⁺t[1] +end + + +""" +Construct guided proposal on a single segment with times in tt from precomputed ν and H +""" +struct GuidedProposal!{T,Ttarget,Taux,Tν,TH,Tν0, TC,F} <: ContinuousTimeProcess{T} + target::Ttarget # P + aux::Taux # Ptilde + tt::Vector{Float64} # grid of time points on single segment (S,T] + ν::Vector{Tν} + H::Vector{TH} + ν0::Tν0 + H0::TH + C0::TC + endpoint::F + + function GuidedProposal!(target, aux, tt_, ν, H, ν0, H0, C0, endpoint=Bridge.endpoint) + tt = collect(tt_) + @show typeof.(( ν0, H0, C0)) + new{Bridge.valtype(target),typeof(target),typeof(aux),eltype(ν),eltype(H),typeof(ν0),typeof(C),typeof(endpoint)}(target, aux, tt, ν, H, ν0, H0, C0, endpoint) + end +end + +Bridge.lptilde(x, Po::GuidedProposal!) = -0.5*(dot(x, Po.H0*x) - 2dot(x, Po.H0*Po.ν0)) - Po.C0 + + +function Bridge._b!((i,t), x, out, P::GuidedProposal!) + Bridge.b!(t, x, out, P.target) + out .+= amul(t,x,P.H[i]*(P.ν[i] - x),P.target) + out +end + +function Bridge._b((i,t), x, P::GuidedProposal!) + out = Bridge.b(t, x, P.target) + out .+= amul(t,x,P.H[i]*(P.ν[i] - x),P.target) + out +end + +Bridge.σ!(t, x, dw, out, P::GuidedProposal!) = σ!(t, x, dw, out, P.target) + +Bridge.σ(t, x, dw, P::GuidedProposal!) = σ(t, x, dw, P.target) + + +function _r!((i,t), x, out, P::GuidedProposal!) + out .= (P.H[i]*(P.ν[i] - x)) + out +end + + + +#H((i,t), x, P::GuidedProposal!) = P.H[i] + +target(P::GuidedProposal!) = P.target +auxiliary(P::GuidedProposal!) = P.aux + +constdiff(P::GuidedProposal!) = constdiff(target(P)) && constdiff(auxiliary(P)) + +function llikelihood(::LeftRule, Xcirc::SamplePath{<:State{Pnt}}, Q::GuidedProposal!; skip = 0) where {Pnt} + tt = Xcirc.tt + xx = Xcirc.yy + + som::deepeltype(xx[1]) = 0. + rout = copy(xx[1]) + + if !constdiff(Q) # use sizetypes? # must be target(Q).nfs + # srout = zeros(Pnt, length(P.nfs)) + # strout = zeros(Pnt, length(P.nfs)) + srout = zeros(Pnt, length(Q.target.nfs)) + strout = zeros(Pnt, length(Q.target.nfs)) + end + + + bout = copy(rout) + btout = copy(rout) + for i in 1:length(tt)-1-skip #skip last value, summing over n-1 elements + s = tt[i] + x = xx[i] + _r!((i,s), x, rout, Q) + b!(s, x, bout, target(Q)) + _b!((i,s), x, btout, auxiliary(Q)) + + dt = tt[i+1]-tt[i] + #dump(som) + som += dot(bout-btout, rout) * dt + + if !constdiff(Q) + σt!(s, x, rout, srout, target(Q)) + σt!(s, x, rout, strout, auxiliary(Q)) + + som += 0.5*Bridge.inner(srout) * dt + som -= 0.5*Bridge.inner(strout) * dt + + # inference failure + A = Bridge.a((i,s), x, target(Q)) + At = Bridge.a((i,s), x, auxiliary(Q)) + som -= 0.5*hadamtrace(A, Q.H[i]) * dt + som -= -0.5*hadamtrace(At, Q.H[i]) * dt + + + end + end + som +end + +function hadamtrace(A, H::InverseCholesky) + tr(tr(H*A)) +end + + +function hadamtrace(A, H) + @assert eachindex(A) == eachindex(H) + @unroll1 for i in eachindex(A) + if $first + som = A[i]*H[i] + else + som += A[i]*H[i] + end + end + som +end diff --git a/landmarks/inverse_uncmatrix.jl b/landmarks/inverse_uncmatrix.jl new file mode 100644 index 0000000..9adbe14 --- /dev/null +++ b/landmarks/inverse_uncmatrix.jl @@ -0,0 +1,27 @@ + + +using Test +using StaticArrays +using LinearAlgebra + +const d = 2 +const Point = SArray{Tuple{d},Float64,1,d} +const Unc = SArray{Tuple{d,d},Float64,d,d*d} + +x = [Point(rand(d)...) for i in 1:100] + +A = I + x*x' +lchol(A) = LowerTriangular((LinearAlgebra._chol!(copy(A), UpperTriangular)[1])') + +rchol(A) = LinearAlgebra._chol!(copy(A), UpperTriangular)[1] + +L = lchol(A) +R = rchol(A) + +@test norm(L*L' - A) < 1e-8 + +y = copy(x) +LinearAlgebra.naivesub!(L, y) # triangular backsolves +LinearAlgebra.naivesub!(R, y) + +@test norm(A*y - x) < 1e-9 diff --git a/landmarks/lm_main.jl b/landmarks/lm_main.jl new file mode 100755 index 0000000..87761d6 --- /dev/null +++ b/landmarks/lm_main.jl @@ -0,0 +1,340 @@ +# THIS SCRIPT REPLACES THE OLDER 'lmpar.jl' +using Bridge, StaticArrays, Distributions +using Bridge:logpdfnormal +using Test, Statistics, Random, LinearAlgebra +using Bridge.Models +using DelimitedFiles, DataFrames, CSV, RCall +using Base.Iterators, SparseArrays, LowRankApprox, Trajectories +using ForwardDiff: GradientConfig, Chunk, gradient!, gradient +using TimerOutputs + +using Plots, PyPlot #using Makie + +pyplot() + +models = [:ms, :ahs] +model = models[2] +println(model) + +TEST = false#true +partialobs = true #false +rotation = false # rotate configuration at time T +showplotσq = false + +samplers =[:sgd, :sgld, :mcmc] +sampler = samplers[3] + +ρ = 0.99 #CN par +δ = 0.8 # MALA par +ϵ = 0.01 # sgd step size +ϵstep(i) = 1/(1+i)^(0.7) + + +datasets =["forwardsimulated", "shifted","shiftedextreme", "bear", "heart","peach"] +dataset = datasets[6] + +ITER = 20 # nr of sgd iterations +subsamples = 0:20:ITER + + +const itostrat = true #false#true#false#true +const d = 2 + +n = 20#35 # nr of landmarks + +σobs = 0.01 # noise on observations + +T = 1.0#1.0#0.5 +t = 0.0:0.005:T # time grid + +#Random.seed!(5) +include("state.jl") +include("models.jl") +include("patches.jl") +include("lmguiding.jl") +include("plotlandmarks.jl") +include("automaticdiff_lm.jl") +include("generatedata.jl") + +### Specify landmarks models +a = 2.0 # Hamiltonian kernel parameter (the larger, the stronger landmarks behave similarly) + +if model == :ms + λ = 0.0; # Mean reversion par in MS-model = not the lambda of noise fields =# + γ = 5.0 # Noise level in for MS-model + dwiener = n + nfs = 0 # needs to have value for plotting purposes + P = MarslandShardlow(a, γ, λ, n) +else + db = 5.0 # domainbound + nfstd = 2.5# 1.25 # tau , width of noisefields + nfs = construct_nfs(db, nfstd, .2) # 3rd argument gives average noise of positions (with superposition) + dwiener = length(nfs) + P = Landmarks(a, 0.0, n, nfs) +end + +if (model == :ahs) & showplotσq + plotσq(db, nfs) +end + +StateW = PointF + +# set time grid for guided process +tt_ = tc(t,T)#tc(t,T)# 0:dtimp:(T) + +# generate data +x0, xobs0, xobsT, Xf, P = generatedata(dataset,P,t,σobs) + +if partialobs + L0 = LT = [(i==j)*one(UncF) for i in 1:2:2P.n, j in 1:2P.n] + Σ0 = ΣT = [(i==j)*σobs^2*one(UncF) for i in 1:P.n, j in 1:P.n] + μT = zeros(Point{Float64},P.n) + mT = zeros(PointF,P.n) # +else + LT = [(i==j)*one(UncF) for i in 1:2P.n, j in 1:2P.n] + ΣT = [(i==j)*σobs^2*one(UncF) for i in 1:2P.n, j in 1:2P.n] + μT = zeros(PointF,2P.n) + xobsT = vec(X.yy[end]) + mT = Xf.yy[end].p + L0 = [(i==j)*one(UncF) for i in 1:2:2P.n, j in 1:2P.n] + Σ0 = [(i==j)*σobs^2*one(UncF) for i in 1:P.n, j in 1:P.n] +end + + + +if model == :ms + Paux = MarslandShardlowAux(P, State(xobsT, mT)) +else + Paux = LandmarksAux(P, State(xobsT, mT)) +end + +# compute guided prposal +println("compute guiding term:") +Lt, Mt⁺ , μt = initLMμ(tt_,(LT,ΣT,μT)) +(Lt, Mt⁺ , μt), Q, (Lt0, Mt⁺0, μt0, xobst0) = construct_guidedproposal!(tt_, (Lt, Mt⁺ , μt), (LT,ΣT,μT), (L0, Σ0), (xobs0, xobsT), P, Paux) + +# initialise guided path +#xinit = State(xobs0, [Point(-6.,6.) for i in 1:P.n]) +xinit = State(xobs0, rand(PointF,n)) +#xinit = State(xobs0, zeros(PointF,n)) + +# sample guided path +println("Sample guided proposal:") +Xᵒ = initSamplePath(tt_, xinit) +Wᵒ = initSamplePath(tt_, zeros(StateW, dwiener)) +sample!(Wᵒ, Wiener{Vector{StateW}}()) +@time Bridge.solve!(EulerMaruyama!(), Xᵒ, xinit, Wᵒ, Q) +#guid = guidingterms(Xᵒ,Q) +# plot forward path and guided path +plotlandmarkpositions(Xf,Xᵒ,P.n,model,xobs0,xobsT,nfs,db=3)#2.6) + +@time llikelihood(LeftRule(), Xᵒ, Q; skip = 1) +@time lptilde(vec(xinit), Lt0, Mt⁺0, μt0, xobst0) + + +objvals = Float64[] # keep track of (sgd approximation of the) loglikelihood + + +mask = deepvec(State(0 .- 0*xinit.q, 1 .- 0*(xinit.p))) # only optimize momenta +mask_id = (mask .> 0.1) # get indices that correspond to momenta + + +sk = 1 +acc = zeros(2) # keep track of mcmc accept probs (first comp is for CN update; 2nd component for langevin update on initial momenta) + +Xsave = SamplePath{State{PointF}}[] + +# initialisation +W = copy(Wᵒ) +Wnew = copy(Wᵒ) +X = SamplePath(t, [copy(xinit) for s in tt_]) +solve!(EulerMaruyama!(), X, x0, W, Q) +ll = llikelihood(Bridge.LeftRule(), X, Q,skip=sk) +Xᵒ = copy(X) +if 0 in subsamples + push!(Xsave, copy(X)) +end + +x = deepvec(xinit) +xᵒ = copy(x) +∇x = copy(x) +∇xᵒ = copy(x) + +# for plotting +xobs0comp1 = extractcomp(xobs0,1) +xobs0comp2 = extractcomp(xobs0,2) +xobsTcomp1 = extractcomp(xobsT,1) +xobsTcomp2 = extractcomp(xobsT,2) + +showmomenta = false + +anim = @animate for i in 1:ITER +#for i in 1:ITER + # + global ll + global acc + global X + global Xᵒ + global W + global Wᵒ + global x + global xᵒ + global ∇x + global ∇xᵒ + println("iteration $i") + + δ = ϵstep(i) + + if sampler==:mcmc + δ = 0.02 # for mala in this case + end + + X,Xᵒ,W,Wᵒ,ll,x,xᵒ,∇x,∇xᵒ, obj,acc = updatepath!(X,Xᵒ,W,Wᵒ,Wnew,ll,x,xᵒ,∇x, ∇xᵒ, + sampler,(Lt0, Mt⁺0, μt0, xobst0, Q), + mask, mask_id, δ, ρ, acc) + + if i in subsamples + push!(Xsave, copy(X)) + end + push!(objvals, obj) + + + # plotting + s = deepvec2state(x).p + s0 = x0.p # true momenta + + # plot initial and final shapes + pp = Plots.plot(xobs0comp1, xobs0comp2,seriestype=:scatter, color=:black,label="q0", title="Landmark evolution") + Plots.plot!(pp, repeat(xobs0comp1,2), repeat(xobs0comp2,2),seriestype=:path, color=:black,label="") + Plots.plot!(pp, xobsTcomp1, xobsTcomp2,seriestype=:scatter , color=:orange,label="qT") # points move from black to orange + Plots.plot!(pp, repeat(xobsTcomp1,2), repeat(xobsTcomp2,2),seriestype=:path, color=:orange,label="") + + if showmomenta + Plots.plot!(pp, extractcomp(s,1), extractcomp(s,2),seriestype=:scatter , + color=:blue,label="p0 est") # points move from black to orange) + Plots.plot!(pp, extractcomp(s0,1), extractcomp(s0,2),seriestype=:scatter , + color=:red,label="p0",markersize=5) # points move from black to orange) + xlims!(-3,3) + ylims!(-4,3) + else + xlims!(-3,3) + ylims!(-2,3) + end + + + outg = [Any[X.tt[i], [X.yy[i][CartesianIndex(c, k)][l] for l in 1:d, c in 1:2]..., "point$k"] for k in 1:n, i in eachindex(X.tt) ][:] + dfg = DataFrame(time=extractcomp(outg,1),pos1=extractcomp(outg,2),pos2=extractcomp(outg,3),mom1=extractcomp(outg,4),mom2=extractcomp(outg,5),pointID=extractcomp(outg,6)) + for j in 1:n + #global pp + el1 = dfg[:pointID].=="point"*"$j" + dfg1 = dfg[el1,:] + Plots.plot!(pp,dfg1[:pos1], dfg1[:pos2],label="") + end + + pp2 = Plots.plot(collect(1:i), objvals[1:i],seriestype=:scatter ,color=:blue,markersize=1.5,label="",title="Loglikelihood approximation") + Plots.plot!(pp2, collect(1:i), objvals[1:i] ,color=:blue,label="") + xlabel!(pp2,"iteration") + ylabel!(pp2,"stoch log likelihood") + xlims!(0,ITER) + + l = @layout [a b] + Plots.plot(pp,pp2,background_color = :ivory,layout=l , size = (900, 500) ) + + plotlandmarkpositions(Xf,X,P.n,model,xobs0,xobsT,nfs,db=2.6) +end + + +cd("/Users/Frank/github/BridgeLandmarks") +fn = "me"*"_" * string(model) * "_" * string(sampler) *"_" * string(dataset) +gif(anim, fn*".gif", fps = 20) +mp4(anim, fn*".mp4", fps = 20) + +print(objvals) +#end + +sc2 = Plots.plot(collect(1:ITER), objvals,seriestype=:scatter ,color=:blue,markersize=1.2,label="",title="Loglikelihood approximation") +Plots.plot!(sc2, collect(1:ITER), objvals ,color=:blue,label="") +xlabel!(sc2,"iteration") +ylabel!(sc2,"stoch log likelihood") +xlims!(0,ITER) +display(sc2) +png(sc2,"stochlogp.png") + + +error("STOP HERE") + + + + + + + + + + + + +# also do gradient descent on parameters a (in kernel of Hamiltonian) +# first for MS model +get_targetpars(Q::GuidedProposall!) = [Q.target.a, Q.target.γ] +get_auxpars(Q::GuidedProposall!) = [Q.aux.a, Q.aux.γ] + +put_targetpars = function(pars,Q) + GuidedProposall!(MarslandShardlow(pars[1],pars[2],Q.target.λ, Q.target.n), Q.aux, Q.tt, Q.Lt, Q.Mt, Q.μt,Q.Ht, Q.xobs) +end + +put_auxpars(pars,Q) = GuidedProposall!(Q.target,MarslandShardlowAux(pars[1],pars[2],Q.aux.λ, Q.aux.xT,Q.aux.n), Q.tt, Q.Lt, Q.Mt, Q.μt,Q.Ht, Q.xobs) + +QQ = put_targetpars([3.0, 300.0],Q) +QQ.target.a +QQ.target.γ + + +function obj2(xinitv,pars) + Q = put_targetpars(pars,Q) + Q = put_auxpars(pars,Q) + xinit = deepvec2state(xinitv) + sample!(WW, Wiener{Vector{StateW}}()) + Xᵒ = Bridge.solve(EulerMaruyama!(), xinit, WW, Q) + ( + (lptilde(vec(xinit ), L0, M0⁺, μ0, V, Q) - lptilde(vec(x0), L0, M0⁺, μ0, V, Q)) + + llikelihood(LeftRule(), Xᵒ, Q; skip = 1) + ) +end + + + +if false + # write mcmc iterates to csv file + + fn = outdir*"iterates.csv" + f = open(fn,"w") + head = "iteration, time, component, value \n" + write(f, head) + iterates = [Any[s, tt[j], d, XX[i].yy[j][d]] for d in 1:1, j in 1:length(X), (i,s) in enumerate(subsamples) ][:] + writecsv(f,iterates) + close(f) + + ave_acc_perc = 100*round(acc/iterations,2) + + # write info to txt file + fn = outdir*"info.txt" + f = open(fn,"w") + write(f, "Number of iterations: ",string(iterations),"\n") + write(f, "Skip every ",string(skip_it)," iterations, when saving to csv","\n\n") + write(f, "Starting point: ",string(x0),"\n") + write(f, "End time T: ", string(T),"\n") + write(f, "Endpoint v: ",string(v),"\n") + write(f, "Noise Sigma: ",string(Σ),"\n") + write(f, "L: ",string(L),"\n\n") + write(f, "Mesh width: ",string(dt),"\n") + write(f, "rho (Crank-Nicholsen parameter: ",string(ρ),"\n") + write(f, "Average acceptance percentage: ",string(ave_acc_perc),"\n\n") + write(f, "Backward type parametrisation in terms of nu and H? ",string(νHparam),"\n") + close(f) + + + println("Average acceptance percentage: ",ave_acc_perc,"\n") + println("Parametrisation of nu and H? ", νHparam) + println("Elapsed time: ",elapsed_time) +end diff --git a/landmarks/lmguiding.jl b/landmarks/lmguiding.jl new file mode 100755 index 0000000..f64df1e --- /dev/null +++ b/landmarks/lmguiding.jl @@ -0,0 +1,183 @@ +import Bridge: kernelr3!, R3!, target, auxiliary, constdiff, llikelihood, _b!, B!, σ!, b! + +""" + Guided proposal update for newly incoming observation. + The existing tuple (Lt, Mt, μt, xobst) is updated using + Σ: covariance matrix of the incoming observation + L: specification that L x is observed (where x is the 'full' state) + newobs: new observation (obtained as L x + N(0,Σ)) +""" +function lmgpupdate(Lt, Mt::Array{Pnt,2}, μt, xobst, (L, Σ, newobs)) where Pnt + Lt = [L; Lt] + m = size(Σ)[1] + n = size(Mt)[2] + Mt = [Σ zeros(Pnt,m,n); zeros(Pnt,n,m) Mt] + μt = [0newobs; μt] + xobst = [newobs; xobst] + + Lt, Mt, μt, xobst +end + +""" + Initialise arrays for (L,M,μ) where each value is copied length(t) times +""" +function initLMμ(t,(L,M,μ)) + Lt = [copy(L) for s in t] + Mt⁺ = [copy(M) for s in t] + μt = [copy(μ) for s in t] + Lt, Mt⁺ , μt +end + +""" +Construct guided proposal on a single segment with times in tt from precomputed ν and H +""" +struct GuidedProposall!{T,Ttarget,Taux,TL,TM,Tμ,TH,Txobs,F} <: ContinuousTimeProcess{T} + target::Ttarget # P + aux::Taux # Ptilde + tt::Vector{Float64} # grid of time points on single segment (S,T] + Lt::Vector{TL} + Mt::Vector{TM} + μt::Vector{Tμ} + Ht::Vector{TH} + xobs::Txobs + endpoint::F + + function GuidedProposall!(target, aux, tt_, L, M, μ, H, xobs, endpoint=Bridge.endpoint) + tt = collect(tt_) + new{Bridge.valtype(target),typeof(target),typeof(aux),eltype(L),eltype(M),eltype(μt),eltype(H),typeof(xobs),typeof(endpoint)}(target, aux, tt, L, M, μ, H, xobs, endpoint) + end +end + + +struct Lm end + + +function guidingbackwards!(::Lm, t, (Lt, Mt⁺, μt), Paux, (L, Σ , μend)) + Mt⁺[end] .= Σ + Lt[end] .= L + BB = Matrix(Bridge.B(0, Paux)) # does not depend on time + println("computing ã and its low rank approximation:") + # various ways to compute ã (which does not depend on time); + # low rank appoximation really makes sense here +# @time aa = Matrix(Bridge.a(0, Paux)) # vanilla, no lr approx +# @time aalr = pheigfact(deepmat(Matrix(Bridge.a(0, Paux)))) # low rank approx default +# @time aalr = pheigfact(deepmat(Matrix(Bridge.a(0, Paux))),rank=400) # fix rank + @time aalr = pheigfact(deepmat(Matrix(Bridge.a(0, Paux))), rtol=1e-10) # control accuracy of lr approx + println("Rank ",size(aalr[:vectors],2), " approximation to ã") + sqrt_aalr = deepmat2unc(aalr[:vectors] * diagm(0=> sqrt.(aalr[:values]))) + + β = vec(Bridge.β(0,Paux)) # does not depend on time + for i in length(t)-1:-1:1 + dt = t[i+1]-t[i] +# Lt[i] .= Lt[i+1] * (I + BB * dt) # explicit + Lt[i] .= Lt[i+1]/(I - dt* BB) # implicit, similar computational cost +# Mt⁺[i] .= Mt⁺[i+1] + Lt[i+1]* aa * Matrix(Lt[i+1]') * dt + Mt⁺[i] .= Mt⁺[i+1] + Bridge.outer(Lt[i+1] * sqrt_aalr) * dt + μt[i] .= μt[i+1] + Lt[i+1] * β * dt + end + (Lt[1], Mt⁺[1], μt[1]) +end + +target(Q::GuidedProposall!) = Q.target +auxiliary(Q::GuidedProposall!) = Q.aux + +constdiff(Q::GuidedProposall!) = constdiff(target(Q)) && constdiff(auxiliary(Q)) + + +function _b!((i,t), x::State, out::State, Q::GuidedProposall!) + Bridge.b!(t, x, out, Q.target) + out .+= amul(t,x,Q.Lt[i]' * (Q.Mt[i] *(Q.xobs-Q.μt[i]-Q.Lt[i]*vec(x))),Q.target) + out +end + +σ!(t, x, dw, out, Q::GuidedProposall!) = σ!(t, x, dw, out, Q.target) + +# in following x is of type state +function _r!((i,t), x::State, out::State, Q::GuidedProposall!) + out .= vecofpoints2state(Q.Lt[i]' * (Q.Mt[i] *(Q.xobs-Q.μt[i]-Q.Lt[i]*vec(x)))) + out +end +# need function that multiplies square unc with state and outputs state + +function guidingterm((i,t),x::State,Q::GuidedProposall!) + #Bridge.b(t,x,Q.target) + + amul(t,x,Q.Lt[i]' * (Q.Mt[i] *(Q.xobs-Q.μt[i]-Q.Lt[i]*vec(x))),Q.target) +end +""" +Returns the guiding terms a(t,x)*r̃(t,x) along the path of a guided proposal +for each value in X.tt. +Hence, it returns an Array of type State +""" +function guidingterms(X::SamplePath{State{SArray{Tuple{2},Float64,1,2}}},Q::GuidedProposall!) + i = first(1:length(X.tt)) + out = [guidingterm((i,X.tt[i]),X.yy[i],Q)] + for i in 2:length(X.tt) + push!(out, guidingterm((i,X.tt[i]),X.yy[i],Q)) + end + out +end + +""" +v0 consists of all observation vectors stacked, so in case of two observations, it should be v0 and vT stacked +""" +function Bridge.lptilde(x, L0, M⁺0, μ0, xobs) + y = deepvec(xobs - μ0 - L0*x) + M⁺0deep = deepmat(M⁺0) + -0.5*logdet(M⁺0deep) -0.5*dot(y, M⁺0deep\y) +end + +function llikelihood(::LeftRule, Xcirc::SamplePath{State{Pnt}}, Q::GuidedProposall!; skip = 0) where {Pnt} + tt = Xcirc.tt + xx = Xcirc.yy + som::deepeltype(xx[1]) = 0. + + # initialise objects to write into + # srout and strout are vectors of Points + if !constdiff(Q) # use sizetypes? # must be target(Q).nfs + srout = zeros(Pnt, length(Q.target.nfs)) + strout = zeros(Pnt, length(Q.target.nfs)) + end + # rout, bout, btout are of type State + rout = copy(xx[1]) + bout = copy(rout) + btout = copy(rout) + + At = Bridge.a((1,0), xx[1], auxiliary(Q)) + A = zeros(Unc{deepeltype(xx[1])}, 2Q.target.n,2Q.target.n) + + for i in 1:length(tt)-1-skip #skip last value, summing over n-1 elements + s = tt[i] + x = xx[i] + _r!((i,s), x, rout, Q) + b!(s, x, bout, target(Q)) + _b!((i,s), x, btout, auxiliary(Q)) +# btitilde!((s,i), x, btout, Q) + dt = tt[i+1]-tt[i] + som += dot(bout-btout, rout) * dt + + if !constdiff(Q) + σt!(s, x, rout, srout, target(Q)) # σ(t,x)' * tilder(t,x) + σt!(s, x, rout, strout, auxiliary(Q)) # tildeσ(t,x)' * tilder(t,x) + + som += 0.5*Bridge.inner(srout) * dt # |σ(t,x)' * tilder(t,x)|^2 + som -= 0.5*Bridge.inner(strout) * dt # |tildeσ(t,x)' * tilder(t,x)|^2 + + Bridge.a!((i,s), x, A, target(Q)) #A = Bridge.a((i,s), x, target(Q)) + + # som -= 0.5*hadamtrace(A, Q.Ht[i]) * dt + # som += 0.5*hadamtrace(At, Q.Ht[i]) * dt + som += 0.5*(dot(At,Q.Ht[i]) - dot(A,Q.Ht[i])) * dt + end + end + som +end + +construct_guidedproposal! = function(tt_, (Lt, Mt⁺ , μt), (LT,ΣT,μT), (L0, Σ0), (xobs0, xobsT), P, Paux) + (Lt0₊, Mt⁺0₊, μt0₊) = guidingbackwards!(Lm(), tt_, (Lt, Mt⁺,μt), Paux, (LT, ΣT, μT)) + Lt0, Mt⁺0, μt0, xobst0 = lmgpupdate(Lt0₊, Mt⁺0₊, μt0₊, xobsT, (L0, Σ0, xobs0)) + Mt = map(X -> InverseCholesky(lchol(X)),Mt⁺) + Ht = [Lt[i]' * (Mt[i] * Lt[i] ) for i in 1:length(tt_) ] + Q = GuidedProposall!(P, Paux, tt_, Lt, Mt, μt, Ht, xobsT) + + (Lt, Mt⁺ , μt), Q, (Lt0, Mt⁺0, μt0, xobst0) +end diff --git a/landmarks/models.jl b/landmarks/models.jl new file mode 100755 index 0000000..b0d871d --- /dev/null +++ b/landmarks/models.jl @@ -0,0 +1,611 @@ +#### Landmarks specification +import Bridge: _b, _b!, B!, σ!, b!, σ, b + +struct MarslandShardlow{T} <: ContinuousTimeProcess{State{PointF}} + a::T # kernel std parameter + γ::T # noise level + λ::T # mean reversion + n::Int +end + +struct MarslandShardlowAux{S,T} <: ContinuousTimeProcess{State{PointF}} + a::T # kernel std parameter + γ::T # noise level + λ::T # mean reversion + xT::State{Point{S}} # use x = State(P.v, zero(P.v)) where v is conditioning vector + n::Int +end + +MarslandShardlowAux(P::MarslandShardlow, xT) = MarslandShardlowAux(P.a, P.γ, P.λ, xT, P.n) + +struct Noisefield{T} + δ::Point{T} # locations of noise field + λ::Point{T} # scaling at noise field + τ::T # std of Gaussian kernel noise field +end + +struct Landmarks{S,T} <: ContinuousTimeProcess{State{PointF}} + a::T # kernel std + λ::T # mean reversion + n::Int64 # numer of landmarks + nfs::Vector{Noisefield{S}} # vector containing pars of noisefields +end + +struct LandmarksAux{S,T} <: ContinuousTimeProcess{State{PointF}} + a::T # kernel std + λ::T # mean reversion + xT::State{Point{S}} # use x = State(P.v, zero(P.v)) where v is conditioning vector + n::Int64 # numer of landmarks + nfs::Vector{Noisefield{S}} # vector containing pars of noisefields +end + +LandmarksAux(P::Landmarks, xT) = LandmarksAux(P.a, P.λ, xT, P.n, P.nfs) + +const LandmarkModel = Union{Landmarks, LandmarksAux, MarslandShardlow, MarslandShardlowAux} + +Bridge.constdiff(::Union{MarslandShardlow, MarslandShardlowAux,LandmarksAux}) = true +Bridge.constdiff(::Landmarks) = false + + +""" +kernel in Hamiltonian +""" +function kernel(q, P::LandmarkModel) + (2*π*P.a^2)^(-d/2)*exp(-Bridge.inner(q)/(2*P.a^2)) +end + +""" +gradient of kernel in hamiltonian +""" +function ∇kernel(q, P::LandmarkModel) + -P.a^(-2) * kernel(q, P) * q +end + +""" +Needed for b! in case P is auxiliary process +""" +function ∇kernel(q, qT, P::LandmarkModel) + -P.a^(-2) * kernel(qT, P) * q +end + +""" +Hamiltonian for deterministic part of landmarks model +""" +function hamiltonian(x, P) + s = 0.0 + for i in axes(x, 2), j in axes(x, 2) +# s += 1/2*dot(p[i], p[j])*kernel(q[i] - q[j], P) + s += 1/2*dot(x.p[i], x.p[j])*kernel(x.q[i] - x.q[j], P) + end + s +end + +Bridge.b(t::Float64, x, P::Union{Landmarks,MarslandShardlow})= Bridge.b!(t, x, 0*x, P) + +Bridge.σ(t, x, dm, P) = Bridge.σ!(t, x, dm , 0*x, P) + +######################################################################################################################################################################################## +################ MS model ######################################################################################### + + +""" +Evaluate drift of landmarks in (t,x) and save to out +x is a state and out as well +""" +function Bridge.b!(t, x, out, P::MarslandShardlow) + zero!(out) + for i in 1:P.n + for j in 1:P.n + out.q[i] += p(x,j)*kernel(q(x,i) - q(x,j), P) + out.p[i] += -P.λ*p(x,j)*kernel(q(x,i) - q(x,j), P) - + dot(p(x,i), p(x,j)) * ∇kernel(q(x,i) - q(x,j), P) + end + end + out +end + +""" +Evaluate drift of landmarks auxiliary process in (t,x) and save to out +x is a state and out as well +""" +function Bridge.b!(t, x, out, Paux::MarslandShardlowAux) + zero!(out) + for i in 1:Paux.n + for j in 1:Paux.n + out.q[i] += p(x,j)*kernel(q(Paux.xT,i) - q(Paux.xT,j), Paux) + out.p[i] += -Paux.λ*p(x,j)*kernel(q(Paux.xT,i) - q(Paux.xT,j), Paux) + end + end + out +end + +""" +Compute tildeB(t) for landmarks auxiliary process +""" +function Bridge.B(t, Paux::MarslandShardlowAux) # not AD safe + X = zeros(UncF, 2Paux.n, 2Paux.n) + for i in 1:Paux.n + for j in 1:Paux.n + X[2i-1,2j] = kernel(q(Paux.xT,i) - q(Paux.xT,j), P) * one(UncF) + X[2i,2j] = -Paux.λ*kernel(q(Paux.xT,i) - q(Paux.xT,j), Paux)*one(UncF) + end + end + X +end + + +""" +Compute B̃(t) * X (B̃ from auxiliary process) and write to out +Both B̃(t) and X are of type UncMat +""" +function Bridge.B!(t,X,out, Paux::MarslandShardlowAux) + out .= 0.0 * out + for i in 1:Paux.n # separately loop over even and odd indices + for k in 1:2Paux.n # loop over all columns + for j in 1:Paux.n + out[2i-1,k] += kernel(q(Paux.xT,i) - q(Paux.xT,j), Paux) * X[p(j), k] + out[2i,k] += -Paux.λ*kernel(q(Paux.xT,i) - q(Paux.xT,j), Paux) * X[p(j), k] + end + end + end + out +end + +function Bridge.β(t, Paux::MarslandShardlowAux) # Not AD save + State(zeros(PointF,Paux.n), zeros(PointF,Paux.n)) +end + +function Bridge.σ!(t, x, dm, out, P::Union{MarslandShardlow, MarslandShardlowAux}) + zero!(out.q) + out.p .= dm*P.γ + out +end + + +""" +Returns matrix a(t) for Marsland-Shardlow model +""" +function Bridge.a(t, P::Union{MarslandShardlow, MarslandShardlowAux}) + error("remove error if we want to keep this") + I = Int[] + X = Unc{Float64}[] + γ2 = P.γ^2 + for i in 1:P.n + push!(I, 2i) + push!(X, γ2*one(Unc{Float64})) + end + sparse(I, I, X, 2P.n, 2P.n) +end +Bridge.a(t, x, P::Union{MarslandShardlow, MarslandShardlowAux}) = Bridge.a(t, P) + +""" +Multiply a(t,x) times xin (which is of type state) +Returns variable of type State +""" +function amul(t, x::State, xin::State, P::Union{MarslandShardlow, MarslandShardlowAux}) + out = copy(xin) + zero!(out.q) + out.p .= P.γ^2 .* xin.p + out +end +function amul(t, x::State, xin::Vector{<:Point}, P::Union{MarslandShardlow, MarslandShardlowAux}) + out = copy(x) + zero!(out.q) + out.p .= P.γ^2 .* vecofpoints2state(xin).p + out +end + +######################################################################################################################################################################################## +################ AHS model ######################################################################################### +# kernel for noisefields +function K̄(q,τ) + exp(-Bridge.inner(q)/(2*τ^2))# (2*π*τ^2)^(-d/2)*exp(-norm(x)^2/(2*τ^2)) +end +# gradient of kernel for noisefields +function ∇K̄(q,τ) + -τ^(-2) * K̄(q,τ) * q +end + +""" +Needed for b! in case P is auxiliary process +""" +function ∇K̄(q, qT, τ) + -τ^(-2) * K̄(qT,τ) * q +end + +""" + Define z(q) = < ∇K̄(q - δ,τ), λ > + Required for Stratonovich -> Ito correction in AHS-model +""" +z(q,τ,δ,λ) = Bridge.inner(∇K̄(q - δ,τ),λ) + +""" + Define ∇z(q) = ∇ < ∇K̄(q - δ,τ), λ > + Required for Stratonovich -> Ito correction in AHS-model +""" +∇z(q,τ,δ,λ) = gradient(x -> z(x,τ,δ,λ),q) + +# function for specification of diffusivity of landmarks +""" + Suppose one noise field nf + Returns diagonal matrix with noisefield for position at point location q (can be vector or Point) +""" +σq(q, nf::Noisefield) = Diagonal(nf.λ * K̄(q - nf.δ,nf.τ)) + +""" + Suppose one noise field nf + Returns diagonal matrix with noisefield for momentum at point location q (can be vector or Point) +""" +σp(q, p, nf::Noisefield) = -Diagonal(p .* nf.λ .* ∇K̄(q - nf.δ,nf.τ)) + + +""" + For AHS model compute total noise field on position experienced at a point x. + Useful for plotting purposes. + + Example usage: + σq(Point(0.0, 0.0), nfs) + σq([0.0; 0.0], nfs) +""" +function σq(x, nfs::Array{<:Noisefield,1}) + out = σq(x, nfs[1]) + for j in 2:length(nfs) + out += σq(x, nfs[j]) + end + out +end + +σq(nfs) = (x) -> σq(x,nfs) # inefficient + +""" + Construct sequence of Noisefields for AHS model + db: domainbound (sources are places on square grid specified by + (-db:2nfstd:db) x -db:2nfstd:db + nfstd: standard deviation of noise fields (the smaller: the more noise fields we use) + γ: if set to one, then the value of the noise field on the positions is approximately 1 at all locations in the domain +""" +function construct_nfs(db, nfstd, γ) + r1 = -db:2nfstd:db + r2 = -db:2nfstd:db + nfloc = Point.(collect(product(r1, r2)))[:] + nfscales = [2/pi*γ*Point(1.0, 1.0) for x in nfloc] # intensity + [Noisefield(δ, λ, nfstd) for (δ, λ) in zip(nfloc, nfscales)] +end + + +""" +Evaluate drift of landmarks in (t,x) and save to out +x is a state and out as well +""" +function Bridge.b!(t, x, out, P::Landmarks) + zero!(out) + for i in 1:P.n + for j in 1:P.n + out.q[i] += p(x,j)*kernel(q(x,i) - q(x,j), P) + out.p[i] += -dot(p(x,i), p(x,j)) * ∇kernel(q(x,i) - q(x,j), P) + end + if itostrat + for k in 1:length(P.nfs) + nf = P.nfs[k] + out.q[i] += 0.5 * z(q(x,i),nf.τ,nf.δ,nf.λ) * K̄(q(x,i)-nf.δ,nf.τ) * nf.λ + out.p[i] += 0.5 * dot(p(x,i),nf.λ) * ( z(q(x,i),nf.τ,nf.δ,nf.λ) * ∇K̄(q(x,i)-nf.δ,nf.τ) -K̄(q(x,i)-nf.δ,nf.τ) * ∇z(q(x,i),nf.τ,nf.δ,nf.λ) ) + end + end + end + out +end + +""" +Evaluate drift of landmarks auxiliary process in (t,x) and save to out +x is a state and out as well +""" +function Bridge.b!(t, x, out, Paux::LandmarksAux) + zero!(out) + for i in 1:Paux.n + for j in 1:Paux.n + out.q[i] += p(x,j)*kernel(q(Paux.xT,i) - q(Paux.xT,j), Paux) + end + if itostrat + for k in 1:length(P.nfs) + # approximate q by qT + nf = P.nfs[k] + qT = q(Paux.xT,i) + out.q[i] += 0.5 * z(qT,nf.τ,nf.δ,nf.λ) * K̄(qT-nf.δ,nf.τ) * nf.λ + out.p[i] += 0.5 * dot(p(x,i),nf.λ) * ( z(qT,nf.τ,nf.δ,nf.λ) * ∇K̄(qT - nf.δ,nf.τ) -K̄(qT - nf.δ,nf.τ) * ∇z(qT,nf.τ,nf.δ,nf.λ) ) + end + end + end + out +end + + + +""" +Compute tildeB(t) for landmarks auxiliary process +""" +function Bridge.B(t, Paux::LandmarksAux) + X = zeros(UncF, 2Paux.n, 2Paux.n) + for i in 1:Paux.n + for j in 1:Paux.n + X[2i-1,2j] = kernel(q(Paux.xT,i) - q(Paux.xT,j), P) * one(UncF) + end + if itostrat + for k in 1:length(Paux.nfs) + nf = P.nfs[k] + qT = q(Paux.xT,i) + X[2i,2i] += 0.5 * ( z(qT,nf.τ,nf.δ,nf.λ) * ∇K̄(qT - nf.δ,nf.τ) -K̄(qT - nf.δ,nf.τ) * ∇z(qT,nf.τ,nf.δ,nf.λ) ) * nf.λ' + end + end + end + X +end + +""" +Compute B̃(t) * X (B̃ from auxiliary process) and write to out +Both B̃(t) and X are of type UncMat +""" +function Bridge.B!(t,X,out, Paux::LandmarksAux) + out .= 0.0 * out + u = zero(UncF) + for i in 1:Paux.n # separately loop over even and odd indices + for k in 1:2Paux.n # loop over all columns + for j in 1:Paux.n + out[2i-1,k] += kernel(q(Paux.xT,i) - q(Paux.xT,j), Paux)*X[p(j), k] + end + if itostrat + u = 0.0*u + for k in 1:length(Paux.nfs) + nf = P.nfs[k] + qT = q(Paux.xT,i) + u += 0.5 * ( z(qT,nf.τ,nf.δ,nf.λ) * ∇K̄(qT - nf.δ,nf.τ) -K̄(qT - nf.δ,nf.τ) * ∇z(qT,nf.τ,nf.δ,nf.λ) ) * nf.λ' + end + out[2i,k] = u * X[2i,k] + end + end + end + out +end + +function Bridge.β(t, Paux::LandmarksAux) + out = 0.0 * copy(Paux.xT.q) + if itostrat + for i in 1:Paux.n + for k in 1:length(P.nfs) + nf = P.nfs[k] + qT = q(Paux.xT,i) + out[i] += 0.5 * z(qT,nf.τ,nf.δ,nf.λ) * K̄(qT-nf.δ,nf.τ) * nf.λ # simply take q at endpoint + end + end + return(State(out,zeros(PointF,Paux.n))) + else + return (State(zeros(PointF,Paux.n), zeros(PointF,Paux.n)) ) + end +end + + +""" +Compute sigma(t,x) * dm where dm is a vector and sigma is the diffusion coefficient of landmarks +write to out which is of type State +""" +function Bridge.σ!(t, x_, dm, out, P::Union{Landmarks,LandmarksAux}) + if P isa Landmarks + x = x_ + else + x = P.xT + end + zero!(out) + for i in 1:P.n + for j in 1:length(P.nfs) + out.q[i] += σq(q(x, i), P.nfs[j]) * dm[j] + out.p[i] += σp(q(x, i), p(x, i), P.nfs[j]) * dm[j] + end + end + out +end + +""" +Compute sigma(t,x)' * y where y is a state and sigma is the diffusion coefficient of landmarks +returns a vector of points of length P.nfs +""" +function σtmul(t, x_, y::State{Pnt}, P::Union{Landmarks,LandmarksAux}) where Pnt + if P isa Landmarks + x = x_ + else + x = P.xT + end + out = zeros(Pnt, length(P.nfs)) + for j in 1:length(P.nfs) + for i in 1:P.n + out[j] += σq(q(x, i), P.nfs[j])' * y.q[i] + + σp(q(x, i), p(x, i), P.nfs[j])' * y.p[i] + end + end + out +end + +function σt!(t, x_, y::State{Pnt}, out, P::Union{Landmarks,LandmarksAux}) where Pnt + zero!(out) + if P isa Landmarks + x = x_ + else + x = P.xT + end + for j in 1:length(P.nfs) + for i in 1:P.n + out[j] += σq(q(x, i), P.nfs[j])' * q(y, i) + + σp(q(x, i), p(x, i), P.nfs[j])' * p(y, i) + end + end + out +end + +function Bridge.a(t, x_, P::Union{Landmarks,LandmarksAux}) + if P isa Landmarks + x = x_ + else + x = P.xT + end + out = zeros(Unc{deepeltype(x)}, 2P.n,2P.n) + for i in 1:P.n + for k in i:P.n + for j in 1:length(P.nfs) + a11 = σq(q(x,i),P.nfs[j]) + a21 = σp(q(x,i),p(x,i),P.nfs[j]) + a12 = σq(q(x,k),P.nfs[j]) + a22 = σp(q(x,k),p(x,k),P.nfs[j]) + out[2i-1,2k-1] += a11 * a12' + out[2i-1,2k] += a11 * a22' + out[2i,2k-1] += a21 * a12' + out[2i,2k] += a21 * a22' + end + end + end + for i in 2:2P.n + for k in 1:i-1 + out[i,k] = out[k,i] + end + end + out +end + +Bridge.a(t, P::LandmarksAux) = Bridge.a(t, 0, P) + + +""" +Multiply a(t,x) times a vector of points +Returns a State +(first multiply with sigma', via function σtmul, next left-multiply this vector with σ) +""" +function amul(t, x::State, xin::Vector{<:Point}, P::Union{Landmarks,LandmarksAux}) + #vecofpoints2state(Bridge.a(t, x, P)*xin) + out = copy(x) + zero!(out) + Bridge.σ!(t, x, σtmul(t, x, vecofpoints2state(xin), P),out,P) +end + +""" +Multiply a(t,x) times a state +Returns a state +(first multiply with sigma', via function σtmul, next left-multiply this vector with σ) +""" +function amul(t, x::State, xin::State, P::Union{Landmarks,LandmarksAux}) + #vecofpoints2state(Bridge.a(t, x, P)*vec(xin)) + out = copy(x) + zero!(out) + Bridge.σ!(t, x, σtmul(t, x, xin, P),out,P) +end + +function Bridge.a!(t, x_, out, P::Union{Landmarks,LandmarksAux}) + zero!(out) + if P isa Landmarks + x = x_ + else + x = P.xT + end + for i in 1:P.n + for k in i:P.n + for j in 1:length(P.nfs) + a11 = σq(q(x,i),P.nfs[j]) + a21 = σp(q(x,i),p(x,i),P.nfs[j]) + a12 = σq(q(x,k),P.nfs[j]) + a22 = σp(q(x,k),p(x,k),P.nfs[j]) + out[2i-1,2k-1] += a11 * a12' + out[2i-1,2k] += a11 * a22' + out[2i,2k-1] += a21 * a12' + out[2i,2k] += a21 * a22' + end + end + end + for i in 2:2P.n + for k in 1:i-1 + out[i,k] = out[k,i] + end + end + out +end + + +###################################################################################################### + + +if TEST + Hend⁺ = [rand(UncF) for i in 1:2Paux.n, j in 1:2Paux.n] + t0 = 2.0 + BB = Bridge.B(t0,Paux) * Hend⁺ + out = deepcopy(Hend⁺) + Bridge.B!(t0,Hend⁺,out,Paux) + @test out==BB +end + + + + + +if false # some failed attempts to compute tr(aH) right away faster, with less memory allocation + + """ + Compute row_i{sigmaq(t,x)} * dm where dm is a vector of points and sigma is the diffusion coefficient of landmarks + write to out which is of Vector{Points} + """ + function σq!(t, x_, dm, out, P::Union{Landmarks,LandmarksAux}) + if P isa Landmarks + x = x_ + else + x = P.xT + end + zero!(out) + for j in 1:length(P.nfs) + out += σq(q(x, i), P.nfs[j]) * dm[j] + end + + out + end + + + """ + Compute row_i{sigmap(t,x)} * dm where dm is a vector of points and sigma is the diffusion coefficient of landmarks + write to out which is of Vector{Points} + """ + function σp!(t, x_, dm, out, P::Union{Landmarks,LandmarksAux}) + if P isa Landmarks + x = x_ + else + x = P.xT + end + zero!(out) + for j in 1:length(P.nfs) + out += σp(q(x, i), p(x, i), P.nfs[j]) * dm[j] + end + out + end + ####################### + function tr_aH(t, x::State,H, P::Union{Landmarks,LandmarksAux}) + som = 0.0 + outq = zeros(UncF, length(P.nfs)) + outp = zeros(UncF, length(P.nfs)) + for i in 1:P.n + σtHcol!(outq,x,H[:,2i-1],P.nfs) # compute σ' times Col_i(H) (should give J x 1 column-vector of Unc) + σtHcol!(outp,x,H[:,2i],P.nfs) # compute σ' times Col_i(H) (should give J x 1 column-vector of Unc) + for j in 1:length(P.nfs) + som += tr(σq(q(x,i),P.nfs[j]) * outq[j]) + + tr(σp(q(x,i),p(x,i),P.nfs[j]) * outp[j]) + end + + end + som + end + + function σtHcol!(out,x,Hvec,nfs) + zero!(out) + for j in 1:length(nfs) + for i in 1:P.n + out[j] += σq(q(x,i),nfs[j]) * Hvec[2i-1] + σp(q(x,i),p(x,i),nfs[j]) * Hvec[2i] + end + end + out + end + + # @time tr_aH(1.0,x0,H,P) + # @time sum(sum(Bridge.a((2,3.0,), x0, P).*H)) + # @time dot(Bridge.a((2,3.0,), x0, P),H) +end diff --git a/landmarks/nstate.jl b/landmarks/nstate.jl new file mode 100644 index 0000000..3e10473 --- /dev/null +++ b/landmarks/nstate.jl @@ -0,0 +1,133 @@ + + using Test + using StaticArrays + + const Point{T} = SArray{Tuple{d},T,1,d} # point in R2 + const Unc{T} = SArray{Tuple{d,d},T,d,d*d} # Matrix presenting uncertainty + + const PointF = Point{Float64} + const UncF = Unc{Float64} + + + struct NState{P, M<:AbstractMatrix{P}} + x::M + end + const State = NState + + NState(x::Vector) = NState(reshape(x, (2, length(x)>>1))) + + import Base: axes, #=iterate,=# eltype, copy, copyto!, zero, eachindex, getindex, setindex!, size, vec + + q(x::NState, i) = x.x[1, i] + p(x::NState, i) = x.x[2, i] + eltype(x::NState) = eltype(x.x) + deepeltype(x::NState) = eltype(eltype(x)) + q(x::NState) = @view x.x[1:2:end] + p(x::NState) = @view x.x[2:2:end] + + import LinearAlgebra: norm + norm(x::NState) = norm(vec(x)) + +# function outer(x::State, y::State) +# [outer(x[i],y[j]) for i in eachindex(x), j in eachindex(y)] + #end + + + vecofpoints2state(x::Vector) = NState(x) + + size(s::NState) = size(s.x) + axes(s::NState, i) = axes(s.x, i) + deepvec(x::NState) = vec(reinterpret(eltype(eltype(x)), x.x)) + function deepvec2state(x::Vector) + x = reinterpret(Point{eltype(x)}, x) + #dump(length(x)) + NState(reshape(x, (2, length(x)>>1))) + end + + function deepvec2state(x::Base.ReshapedArray{<:Any,1,<:Base.ReinterpretArray}) + NState(x.parent.parent) + end + + function NState(q::AbstractVector, p::AbstractVector) + length(p) != length(q) && throw(DimensionMismatch()) + NState([((q,p)[i])[j] for i in 1:2, j in 1:length(p)]) + end + + function NState(x::Vector) + NState(reshape(x, (2, length(x)>>1))) + end + vec(x::NState) = vec(x.x) + Base.broadcastable(x::NState) = x + Broadcast.BroadcastStyle(::Type{<:NState}) = Broadcast.Style{NState}() + + + function Base.getproperty(u::NState, s::Symbol) + x = getfield(u, :x) + s == :x && return x + + if s == :q + @view x[1:2:end] + elseif s == :p + @view x[2:2:end] + else + throw(ArgumentError("NState has properties `p`, `q` or `x`")) + end + end + + zero!(v::NState) = zero!(v.x) + zero!(v::AbstractArray) = v .= Ref(zero(eltype(v))) + zero(v::NState) = NState(zero(v.x)) + + copy(x::NState) = NState(copy(x.x)) + function copyto!(x::NState, y::NState) + copyto!(x.x, y.x) + x + end + + function copyto!(x::NState, y) + for i in eachindex(x) + x[i] = y[i] + end + x + end + + + + + getindex(x::NState, I) = getindex(x.x, I) + function setindex!(x::NState, val, I) + x.x[I] = val + end + + eachindex(x::NState) = CartesianIndices(axes(x.x)) + + + + import Base: *, +, /, - + import LinearAlgebra: dot + +# import Bridge: outer, inner + *(c::Number, x::NState) = NState(c*x.x) + *(x::NState, c::Number) = NState(x.x*c) + +(x::NState, y::NState) = NState(x.x + y.x) + -(x::NState, y::NState) = NState(x.x - y.x) + /(x::NState, y) = NState(x.x/y) + + dot(x::NState, y::NState) = dot(vec(x),vec(y)) + + function Base.:*(A::AbstractArray{<:Unc,2}, x::State) + vecofpoints2state(A*vec(x)) + end +if TEST + q, p = zeros(PointF, 5), rand(PointF, 5) + + x = NState(q, p) + @test x.p == p + @test x.q == q + + @test x.x[1, :] == q + @test x.x[2, :] == p + + U = rand(UncF,5,5) + U * x.p +end diff --git a/landmarks/ostate.jl b/landmarks/ostate.jl new file mode 100644 index 0000000..2d133fe --- /dev/null +++ b/landmarks/ostate.jl @@ -0,0 +1,272 @@ +import Base:iterate, eltype, copy, copyto!, zero, eachindex, getindex, setindex!, size, vec + +const Point{T} = SArray{Tuple{d},T,1,d} # point in R2 +const Unc{T} = SArray{Tuple{d,d},T,d,d*d} # Matrix presenting uncertainty + +const PointF = Point{Float64} + +abstract type UncMat end + +struct State{P} + q::Vector{P} + p::Vector{P} +end + +q(x::State) = x.q +p(x::State) = x.p +q(x::State, i) = x.q[i] +p(x::State, i) = x.p[i] + +q(x::State, i) = x.q[i] +p(x::State, i) = x.p[i] + + + +q(i::Int) = 2i - 1 +p(i::Int) = 2i + +zero!(v) = v[:] = fill!(v, zero(eltype(v))) + +iterate(x::State) = (x.q, true) +iterate(x::State, s) = s ? (x.p, false) : nothing +copy(x::State) = State(copy(x.q), copy(x.p)) +function copyto!(x::State, y::State) + copyto!(x.q, y.q) + copyto!(x.p, y.p) + x +end +function copyto!(x::State, y) + for i in eachindex(x) + x[i] = y[i] + end + x +end +deepeltype(::State{P}) where {P} = eltype(P) + + +Base.broadcastable(x::State) = x +Broadcast.BroadcastStyle(::Type{<:State}) = Broadcast.Style{State}() + + + +size(s::State) = (2, length(s.p)) + +zero(x::State) = State(zero(x.p), zero(x.q)) +zero!(v) = fill!(v, zero(eltype(v))) +zero!(v) = v[:] = fill!(v, zero(eltype(v))) + +function zero!(x::State) + zero!(x.p) + zero!(x.q) + x +end + +function getindex(x::State, I::CartesianIndex) + if I[1] == 1 + x.q[I[2]] + elseif I[1] == 2 + x.p[I[2]] + else + throw(BoundsError()) + end +end +function setindex!(x::State, val, I::CartesianIndex) + if I[1] == 1 + x.q[I[2]] = val + elseif I[1] == 2 + x.p[I[2]] = val + else + throw(BoundsError()) + end +end + +eachindex(x::State) = CartesianIndices((Base.OneTo(2), eachindex(x.p))) + +import Base: *, +, /, - +import LinearAlgebra: norm +import Bridge: outer +*(c::Number, x::State) = State(c*x.q, c*x.p) +*(x::State,c::Number) = State(x.q*c, x.p*c) ++(x::State, y::State) = State(x.q + y.q, x.p + y.p) +-(x::State, y::State) = State(x.q - y.q, x.p - y.p) +/(x::State, y) = State(x.q/y, x.p/y) + +vec(x::State) = vec([x[J] for J in eachindex(x)]) # + +deepvec(x::State{P}) where {P} = vec([x[J][K] for K in eachindex(x.p[1]), J in eachindex(x)]) + +deepvec(x::Vector) = collect(flatten(x)) + + +# matrix multiplication of mat of Uncs +function Base.:*(A::AbstractArray{Unc{T},2},B::AbstractArray{Unc{T},2}) where {T} + C = zeros(Unc{T},size(A,1), size(B,2)) + for i in 1:size(A,1) + for j in 1:size(B,2) + for k in 1:size(A,2) + C[i,j] += A[i,k] * B[k,j] + end + end + end + C +end + +# function Base.:*(A::Array{Unc,2},B::Adjoint{Unc,Array{Unc,2}}) +# C = zeros(Unc,size(A,1), size(B,2)) +# for i in 1:size(A,1) +# for j in 1:size(B,2) +# for k in 1:size(A,2) +# C[i,j] += A[i,k] * B[k,j] +# end +# end +# end +# C +# end +# +# #new +# function Base.:*(A::Adjoint{Unc,Array{Unc,2}},B::Array{Unc,2}) +# C = zeros(Unc,size(A,1), size(B,2)) +# for i in 1:size(A,1) +# for j in 1:size(B,2) +# for k in 1:size(A,2) +# C[i,j] += A[i,k] * B[k,j] +# end +# end +# end +# C +# end + + +function Base.:*(A::AbstractArray{<:Unc,2}, x::State) + vecofpoints2state(A*vec(x)) +end +function Base.:*(A::SparseMatrixCSC, x_::State) + #vecofpoints2state(Array(A)*vec(x)) + x = vec(x_) + vecofpoints2state(mul!(similar(x, A.m), A, x, true, false)) +end +#mul!(similar(x, Tx, A.m), A, x, true, false) +#Base.setindex!(A::SparseMatrixCSC{T}, _v::T, _i::Integer, _j::Integer) where {T} = SparseArrays._setindex_scalar!(A, _v, _i, _j) + +if TEST + A = reshape(rand(Unc,6),3,2) + B = reshape(rand(Unc,8),2,4) + C = B' + @test norm(deepmat(A*B) - deepmat(A) * deepmat(B))<10^(-8) + @test norm(deepmat(A*C') - deepmat(A) * deepmat(C'))<10^(-8) + BB = reshape(rand(Unc,8),4,2) + @test (A*BB')' == BB*A' + + D = reshape(rand(Unc,6),3,2) + E = reshape(rand(Unc,12),3,4) + @test D'*E == (E'*D)' + + # backsolving matrix equation A X = using lu + A1=reshape(rand(Unc,9),3,3) + B1=reshape(rand(Unc,9),3,3) + @test norm(deepmat(A1\B1) - deepmat(A1)\deepmat(B1))<10^(-10) + + +end + + +""" +Convert vector to State. it is assumed that x is ordered as follows +- position landmark 1 +- momentum landmark 1 +- position landmark 2 +- momentum landmark 2 +... +""" +function deepvec2state(x::AbstractVector{T}) where {T} + m = div(length(x),2d) + q = Vector{Point{T}}(undef,m) + p = Vector{Point{T}}(undef,m) + for i in 1:m + q[i] = Point{T}(x[(i-1)*2d .+ (1:d)]) + p[i] = Point{T}(x[((i-1)*2d+d+1):(2i*d)]) + end + State(q,p) +end + +vecofpoints2state(x::Vector{<:Point}) = State(x[1:d:end], x[d:d:end]) + + + +############################ LowRank ######################## +""" +Hdagger = U S U' with S = L L' (cholesky) +U has orthonormal columns, S is a small positive definite symmetric matrix given +in factorized form, e.g. `S <: Cholesky` + +Then H*x is defined as U inv(S) U' (that is the Moore Penrose inverse) +""" +struct LowRank{TS,TU} + S::TS + U::TU +end + +function Base.:*(H::LowRank, x::State) + #z = H.S\(H.U' * deepvec(x)) # e.g. uses two triangular backsolves if S is `Cholesky` + #deepvec2state(H.U*z) + deepvec2state(H.U * (inv(H.S) * (H.U' * deepvec(x)))) +end + +############################ InverseCholesky ######################## +struct InverseLu{TL,TU} + L::TL + U::TU +end + +""" +Compute y = H x where x is a state vector and +Hinv = LU (LU decomposition) + +From LU y =x, it follows that we +solve first z from Lz=x, and next y from Uy=z +""" +function Base.:*(H::InverseLu, x::Vector{<:Point}) + LinearAlgebra.naivesub!(LowerTriangular(H.L), x) + LinearAlgebra.naivesub!(UpperTriangular(H.U), x) + x +end + +function Base.:*(H::InverseLu, x::State) + y = vec(x) + LinearAlgebra.naivesub!(LowerTriangular(H.L), y) + LinearAlgebra.naivesub!(UpperTriangular(H.U), y) + vecofpoints2state(y) +end + + + +if TEST + A = reshape([Unc(1:4), Unc(5:8), Unc(9:12), Unc(13:16)],2,2) + B = reshape([-Unc(1:4), Unc(3:6), Unc(9:12), Unc(13:16)],2,2) + @test deepmat(A*B)==deepmat(A)*deepmat(B) + @test deepmat(A') == deepmat(A)' + @test deepmat(A*A')==deepmat(A)*deepmat(A') + Q = deepmat2unc(Matrix(qr(deepmat(A)).Q)) + C = Q * Q' + lchol(C) + @test deepmat(lchol(C))== cholesky(deepmat(C)).L + isposdef(deepmat(C)) + + # check cholinverse! + HH = InverseCholesky(lchol(C)) + x = rand(4) + xstate = deepvec2state(x) + @test norm(deepvec(HH*xstate) - inv(deepmat(C))*x)<10^(-10) + + LinearAlgebra.naivesub!(lchol(C), A) # triangular backsolves + + A = reshape(rand(Unc,16),4,4) + L, U = lu(A) + W = InverseLu(L,U) + + xs= rand(PointF,4) + xss = [xs[1]; xs[2];xs[3];xs[4]] + inv(deepmat(A)) * xss + W * xs + @test norm(deepvec2state(inv(deepmat(A)) * xss) - W * deepvec2state(xss))<10^(-10) +end diff --git a/landmarks/patches.jl b/landmarks/patches.jl new file mode 100644 index 0000000..88c4bba --- /dev/null +++ b/landmarks/patches.jl @@ -0,0 +1,313 @@ + +""" + Initialise SamplePath on time grid t by copying x into each value of the field yy +""" +initSamplePath(t,x) = Bridge.samplepath(t, x) + +function Bridge.sample!(W::SamplePath{Vector{T}}, P::Wiener{Vector{T}}, y1 = W.yy[1]) where {T} + y = copy(y1) + copyto!(W.yy[1], y) + + for i = 2:length(W.tt) + rootdt = sqrt(W.tt[i]-W.tt[i-1]) + for k in eachindex(y) + y[k] = y[k] + rootdt*randn(T) + end + copyto!(W.yy[i], y) + end + #println(W.yy[1]) + W +end + + +function Bridge.solve(solver::EulerMaruyama!, u, W::SamplePath, P::Bridge.ProcessOrCoefficients) + N = length(W) + + tt = W.tt + yy = [copy(u)] + + i = 1 + dw = W.yy[i+1] - W.yy[i] + t¯ = tt[i] + dt = tt[i+1] - t¯ + + tmp1 = Bridge._b!((i,t¯), u, 0*u, P) + tmp2 = Bridge.σ(t¯, u, dw, P) + + y = u + tmp1*dt + tmp2 + + #dump(y) + #error("here") + + + for i in 2:N-1 + t¯ = tt[i] + dt = tt[i+1] - t¯ + push!(yy, y) + if dw isa Number + dw = W.yy[i+1] - W.yy[i] + else + for k in eachindex(dw) + dw[k] = W.yy[i+1][k] - W.yy[i][k] + end + end + + Bridge._b!((i,t¯), y, tmp1, P) + Bridge.σ!(t¯, y, dw, tmp2, P) + + for k in eachindex(y) + y[k] = y[k] + tmp1[k]*dt + tmp2[k] + end + end + copyto!(yy[end], Bridge.endpoint(y, P)) + SamplePath(tt, yy) +end + + +function Bridge.solve!(solver::EulerMaruyama!, Y::SamplePath, u, W::SamplePath, P::Bridge.ProcessOrCoefficients) + N = length(W) + + tt = W.tt + yy = Y.yy + copyto!(yy[1], u) + + i = 1 + dw = W.yy[i+1] - W.yy[i] + t¯ = tt[i] + dt = tt[i+1] - t¯ + + tmp1 = Bridge._b!((i,t¯), u, 0*u, P) + tmp2 = Bridge.σ(t¯, u, dw, P) + + y = u + tmp1*dt + tmp2 + + #dump(y) + #error("here") + + + for i in 2:N-1 + t¯ = tt[i] + dt = tt[i+1] - t¯ + copyto!(yy[i], y) + if dw isa Number + dw = W.yy[i+1] - W.yy[i] + else + for k in eachindex(dw) + dw[k] = W.yy[i+1][k] - W.yy[i][k] + end + end + + Bridge._b!((i,t¯), y, tmp1, P) + Bridge.σ!(t¯, y, dw, tmp2, P) + + for k in eachindex(y) + y[k] = y[k] + tmp1[k]*dt + tmp2[k] + end + end + copyto!(yy[end], Bridge.endpoint(y, P)) + SamplePath(tt, yy) +end + + +struct StratonovichHeun! <: Bridge.SDESolver +end + +function Bridge.solve!(solver::StratonovichHeun!, Y::SamplePath, u, W::SamplePath, P::Bridge.ProcessOrCoefficients) + N = length(W) + N != length(Y) && error("Y and W differ in length.") + + tt = Y.tt + tt[:] = W.tt + yy = Y.yy + y = copy(u) + ȳ = copy(u) + + tmp1 = copy(y) + tmp2 = copy(y) + tmp3 = copy(y) + tmp4 = copy(y) + + dw = copy(W.yy[1]) + for i in 1:N-1 + t¯ = tt[i] + dt = tt[i+1] - t¯ + copyto!(yy[i], y) + if dw isa Number + dw = W.yy[i+1] - W.yy[i] + else + for k in eachindex(dw) + dw[k] = W.yy[i+1][k] - W.yy[i][k] + end + end + + Bridge._b!((i,t¯), y, tmp1, P) + Bridge.σ!(t¯, y, dw, tmp2, P) + + for k in eachindex(y) + ȳ[k] = y[k] + tmp1[k]*dt + tmp2[k] # Euler prediction + end + + Bridge._b!((i + 1,t¯ + dt), ȳ, tmp3, P) # coefficients at ȳ + #Bridge.σ!(t¯ + dt, ȳ, dw2, tmp4, P) # original implementation + Bridge.σ!(t¯ + dt, ȳ, dw, tmp4, P) + + for k in eachindex(y) + y[k] = y[k] + 0.5*((tmp1[k] + tmp3[k])*dt + tmp2[k] + tmp4[k]) + end + end + copyto!(yy[end], Bridge.endpoint(y, P)) + Y +end + +function LinearAlgebra.naivesub!(At::Adjoint{<:Any,<:LowerTriangular}, b::AbstractVector, x::AbstractVector = b) + A = At.parent + n = size(A, 2) + if !(n == length(b) == length(x)) + throw(DimensionMismatch("second dimension of left hand side A, $n, length of output x, $(length(x)), and length of right hand side b, $(length(b)), must be equal")) + end + @inbounds for j in n:-1:1 + iszero(A.data[j,j]) && throw(SingularException(j)) + xj = x[j] = A.data[j,j] \ b[j] + for i in j-1:-1:1 # counterintuitively 1:j-1 performs slightly better + b[i] -= A.data[j,i] * xj + end + end + x +end + +function LinearAlgebra.naivesub!(At::Adjoint{<:Any,<:LowerTriangular}, B::AbstractMatrix, X::AbstractMatrix = B) + A = At.parent + n = size(A, 2) + if !(n == size(B,1) == size(B,2) == size(X,1) == size(X,2)) + throw(DimensionMismatch()) + end + @inbounds for k in 1:n + for j in n:-1:1 + iszero(A.data[j,j]) && throw(SingularException(j)) + xjk = X[j,k] = A.data[j,j] \ B[j,k] + for i in j-1:-1:1 # counterintuitively 1:j-1 performs slightly better + B[i,k] -= A.data[j,i] * xjk + end + end + end + X +end + +function LinearAlgebra.naivesub!(A::LowerTriangular, B::AbstractMatrix, X::AbstractMatrix = B) + n = size(A,2) + if !(n == size(B,1) == size(X,1)) + throw(DimensionMismatch()) + end + if !(size(B,2) == size(X,2)) + throw(DimensionMismatch()) + end + + + @inbounds for k in 1:size(B,2) + for j in 1:n + iszero(A.data[j,j]) && throw(SingularException(j)) + xjk = X[j,k] = A.data[j,j] \ B[j,k] + for i in j+1:n + B[i,k] -= A.data[i,j] * xjk + end + end + end + X +end + + +function LinearAlgebra.naivesub!(A::UpperTriangular, B::AbstractMatrix, X::AbstractMatrix = B) + n = size(A, 2) + if !(n == size(B,1) == size(X,1)) + throw(DimensionMismatch()) + end + if !(size(B,2) == size(X,2)) + throw(DimensionMismatch()) + end + + @inbounds for k in 1:size(B, 2) + for j in n:-1:1 + iszero(A.data[j,j]) && throw(SingularException(j)) + xjk = X[j,k] = A.data[j,j] \ B[j,k] + for i in j-1:-1:1 # counterintuitively 1:j-1 performs slightly better + B[i,k] -= A.data[i,j] * xjk + end + end + end + X +end + +function lyapunovpsdbackward_step!(t, dt, Paux,Hend⁺,H⁺) + B = Matrix(Bridge.B(t - dt/2, Paux)) + ϕ = (I + 1/2*dt*B)\(I - 1/2*dt*B) + #Ht .= ϕ *(Hend⁺ + 1/2*dt*Bridge.a(t - dt, Paux))* ϕ' + 1/2*dt*Bridge.a(t, Paux) + H⁺ .= ϕ *(Hend⁺ + 1/2*dt*Bridge.a(t - dt, Paux))* conj!(copy(ϕ)) + 1/2*dt*Bridge.a(t, Paux) + H⁺ +end + +""" +Version where B̃ and ã do not depend on try +""" +function lyapunovpsdbackward_step!(t, dt, Paux,Hend⁺,H⁺,B̃, ã) + ϕ = (I + 1/2*dt*B̃)\(I - 1/2*dt*B̃) + #Ht .= ϕ *(Hend⁺ + 1/2*dt*Bridge.a(t - dt, Paux))* ϕ' + 1/2*dt*Bridge.a(t, Paux) + H⁺ .= ϕ *(Hend⁺ + 1/2*dt*ã)* conj!(copy(ϕ)) + 1/2*dt*ã + H⁺ +end + + +""" +Compute transpose of square matrix of Unc matrices + +A = reshape([Unc(1:4), Unc(5:8), Unc(9:12), Unc(13:16)],2,2) +B = copy(A) +A +conj!(B) +""" +function conj!(A::Array{<:Unc,2}) + for i in 1:size(A,1) + A[i,i] = A[i,i]' + for j in (i+1):size(A,2) + A[i,j], A[j, i] = A[j,i]', A[i, j]' + end + end + A +end + +function conj2(A::Array{T<:Unc,2}) where T + At = Matrix{T}(undef,size(A,2),size(A,1)) + for i in 1:size(A,2) + for j in 1:size(A,1) + At[i,j] = A[j,i]' + end + end + At +end + +if TEST + A = reshape(rand(Unc,15),5,3) + B = conj2(A) + @test norm(deepmat(A)'-deepmat(B))<10^(-10) +end + + +""" + Forward simulate landmarks process specified by P on grid t. + Returns driving motion W and landmarks process X + t: time grid + dwiener: dimension of Wiener driving process + x0: starting point + P: landmarks specification +""" +function landmarksforward(t, dwiener, x0::State{Pnt}, P) where Pnt + W = initSamplePath(t, zeros(Pnt, dwiener)) + sample!(W, Wiener{Vector{Pnt}}()) + # forward simulate landmarks + X = initSamplePath(t,x0) + println("Solve for forward provess:") + solve!(EulerMaruyama!(), X, x0, W, P) #@time solve!(StratonovichHeun!(), X, x0, W, P) + W, X +end + +tc(t,T) = t.* (2 .-t/T) +extractcomp(v,i) = map(x->x[i], v) diff --git a/landmarks/plotlandmarks.jl b/landmarks/plotlandmarks.jl new file mode 100644 index 0000000..e427637 --- /dev/null +++ b/landmarks/plotlandmarks.jl @@ -0,0 +1,116 @@ +extractcomp(v,i) = map(x->x[i], v) + + +out = [Any[X.tt[i], [X.yy[i][CartesianIndex(c, k)][l] for l in 1:d, c in 1:2]..., "point$k"] for k in 1:n, i in eachindex(X.tt) ][:] +df = DataFrame(time=extractcomp(out,1),pos1=extractcomp(out,2),pos2=extractcomp(out,3),mom1=extractcomp(out,4),mom2=extractcomp(out,5),pointID=extractcomp(out,6)) +#Any["X.tt[$i]", ["X.yy[$i][CartesianIndex($c, $k)][$l]" for l in 1:d, k in 1:n, c in 1:2]...] + +if model == :ms + titel = "Marsland-Shardlow model, " +else + titel = "Arnaudon-Holm-Sommer model, " +end +titel = titel * string(n)*" landmarks" + +dfT = DataFrame(pos1=extractcomp(vT,1), pos2=extractcomp(vT,2)) +df0= DataFrame(pos1=extractcomp(v0,1), pos2=extractcomp(v0,2)) + +@rput titel +@rput df +@rput dfT +@rput df0 + +if false + R""" + library(tidyverse) + df %>% dplyr::select(time,pos1,pos2,pointID) %>% + ggplot() + + geom_path(data=df,aes(x=pos1,pos2,group=pointID,colour=time)) + + geom_point(data=dfT, mapping=aes(x=pos1,y=pos2),colour='orange',size=0.7) + + geom_point(data=df0, mapping=aes(x=pos1,y=pos2),colour='black',size=0.7)+ + theme_minimal() + xlab('horizontal position') + + ylab('vertical position') + ggtitle(titel) + """ + + + R""" + df %>% dplyr::select(time,mom1,mom2,pointID) %>% + filter(pointID %in% c("point1","point2","point3","point40","point41","point42")) %>% + ggplot(aes(x=mom1,mom2,group=pointID,colour=time)) + geom_path() + + facet_wrap(~pointID,scales="free") + theme_minimal() + """ +end + +#### plotting +outg = [Any[XX.tt[i], [XX.yy[i][CartesianIndex(c, k)][l] for l in 1:d, c in 1:2]..., "point$k"] for k in 1:n, i in eachindex(XX.tt) ][:] +dfg = DataFrame(time=extractcomp(outg,1),pos1=extractcomp(outg,2),pos2=extractcomp(outg,3),mom1=extractcomp(outg,4),mom2=extractcomp(outg,5),pointID=extractcomp(outg,6)) +#Any["X.tt[$i]", ["X.yy[$i][CartesianIndex($c, $k)][$l]" for l in 1:d, k in 1:n, c in 1:2]...] + +dfT = DataFrame(pos1=extractcomp(vT,1), pos2=extractcomp(vT,2)) +df0= DataFrame(pos1=extractcomp(v0,1), pos2=extractcomp(v0,2)) + + +if model == :ms + titel = "Marsland-Shardlow model, " +else + titel = "Arnaudon-Holm-Sommer model, " +end +titel = titel * string(n)*" landmarks" + +@rput titel +@rput dfg +@rput dfT +@rput df0 +@rput T +R""" +library(tidyverse) +T_ <- T +dfsub <- df %>% dplyr::select(time,pos1,pos2,pointID) %>% filter(time% dplyr::select(time,pos1,pos2,pointID) %>% filter(time% dplyr::select(time,mom1,mom2,pointID) %>% + filter(pointID %in% c("point1","point2","point3","point40","point41","point42")) %>% + ggplot(aes(x=mom1,mom2,group=pointID,colour=time)) + geom_path() + + facet_wrap(~pointID,scales="free") + theme_minimal() +""" + + +if false + R""" + dfg %>% dplyr::select(time,mom1,mom2,pointID) %>% + filter(pointID %in% c("point1","point2","point3","point40","point41","point42")) %>% + ggplot(aes(x=mom1,mom2,group=pointID,colour=time)) + geom_path() + + facet_wrap(~pointID) + theme_minimal() + """ + R""" + library(tidyverse) + dfsub <- df %>% dplyr::select(time,pos1,pos2,pointID) %>% filter(time<0.99) + dfsubg <- dfg %>% dplyr::select(time,pos1,pos2,pointID) %>% filter(time<0.99) + + sub <- rbind(dfsub,dfsubg) + sub$fg <- rep(c("forward","guided"),each=nrow(dfsub)) + + ggplot() + + geom_path(data=sub, mapping=aes(x=pos1,pos2,group=pointID,colour=fg)) + + geom_point(data=dfT, mapping=aes(x=pos1,y=pos2),colour='orange',size=0.7) + + geom_point(data=df0, mapping=aes(x=pos1,y=pos2),colour='black',size=0.7)+ + #facet_wrap(~fg) + + theme_minimal() + xlab('horizontal position') + + ylab('vertical position') + ggtitle(titel) + """ +end diff --git a/landmarks/state.jl b/landmarks/state.jl new file mode 100644 index 0000000..db41c05 --- /dev/null +++ b/landmarks/state.jl @@ -0,0 +1,96 @@ + + using Test + using StaticArrays + + const Point{T} = SArray{Tuple{d},T,1,d} # point in R2 + const Unc{T} = SArray{Tuple{d,d},T,d,d*d} # Matrix presenting uncertainty + + const PointF = Point{Float64} + const UncF = Unc{Float64} + + + function deepmat(H::AbstractMatrix{S}) where {S} + d1, d2 = size(S) + reshape([H[i,j][k,l] for k in 1:d1, i in 1:size(H, 1), l in 1:d2, j in 1:size(H,2)], d1*size(H,1), d2*size(H,2)) + end + #@test outer(deepvec(x0)) == deepmat(outer(vec(x0))) + + + function deepmat2unc(A::Matrix) # d is the dimension of the square subblocks + m = div(size(A,1),d) + n = div(size(A,2),d) + [Unc(A[(i-1)*d+1:i*d,(j-1)*d+1:j*d]) for i in 1:m, j in 1:n] + end + + + function outer(x::State, y::State) + [outer(x[i],y[j]) for i in eachindex(x), j in eachindex(y)] + end + + norm(x::State) = norm(vec(x)) + + + """ + Good display of variable of type State + """ + function Base.show(io::IO, state::State) + show(io, "text/plain", hcat(q(state),p(state))) + end + + """ + Solve L L'y =x using two backsolves, + L should be lower triangular + """ + function cholinverse!(L, x) + LinearAlgebra.naivesub!(L, x) # triangular backsolves + LinearAlgebra.naivesub!(UpperTriangular(L'), x) + x + end + + lchol(A) = LowerTriangular(Matrix(LinearAlgebra._chol!(copy(A), UpperTriangular)[1])') + + ############################ InverseCholesky ######################## + struct InverseCholesky{T} + L::T + end + + """ + Compute y = H*x where Hinv = L*L' (Cholesky decomposition + + Input are L and x, output is y + + y=Hx is equivalent to LL'y=x, which can be solved + by first backsolving Lz=x for z and next + backsolving L'y=z + + L is a lower triangular matrix with element of type UncMat + x is a State or vector of points + Returns a State (Todo: split up again and return vector for vector input) + """ + function Base.:*(H::InverseCholesky, x::State) + y = copy(vec(x)) + cholinverse!(H.L, y) # triangular backsolve + vecofpoints2state(y) + end + + function Base.:*(H::InverseCholesky, x::Vector{<:Point}) + y = copy(x) + cholinverse!(H.L, y) # triangular backsolve + end + + """ + Compute y = H*X where Hinv = L*L' (Cholesky decomposition + + Input are L and X, output is Y + + y=HX is equivalent to LL'y=X, which can be solved + by first backsolving LZ=X for z and next + backsolving L'Y=Z + + L is a lower triangular matrix with element of type UncMat + X is a matrix with elements of type UncMat + Returns a matrix with elements of type UncMat + """ + function Base.:*(H::InverseCholesky, X) + cholinverse!(H.L, copy(X)) # triangular backsolves + end diff --git a/landmarks/testms.jl b/landmarks/testms.jl new file mode 100644 index 0000000..7e753cf --- /dev/null +++ b/landmarks/testms.jl @@ -0,0 +1,13 @@ + +using Test +out1 = copy(xT) +@time out1 = (Bridge.b!(t[end], xT, out1, Pmsaux)) + +@time B = Bridge.B(t[end], Pmsaux) +x = vec(xT) +out2 = copy(x) +@time out2 = mul!(out2, B, x, true, false) + +@test vec(out1) ≈ out2 + +Bridge.a(1.1,Pmsaux) diff --git a/project_partialbridge/partialbridge_bolus3.jl b/project_partialbridge/partialbridge_bolus3.jl index 5f0be78..f40788e 100644 --- a/project_partialbridge/partialbridge_bolus3.jl +++ b/project_partialbridge/partialbridge_bolus3.jl @@ -22,8 +22,8 @@ obs_scheme =["full","firstcomponent"][2] Σdiagel = 10^(-4) # settings sampler -iterations = 25000 -skip_it = 1000# 1000 +iterations = 20000 +skip_it = 400# 1000 subsamples = 0:skip_it:iterations ρ = 0.0#95 @@ -85,7 +85,7 @@ if simlongpath # Random.seed!(2) x0 = ℝ{2}(0.0, 0.0) #x0 = ℝ{2}(-8.0, 1.0) - T_long = 8.0#10.0 + T_long = 12.0#10.0 dt = 0.0001 tt_long = 0.:dt:T_long W_long = sample(tt_long, Wiener{ℝ{dp}}()) @@ -102,7 +102,7 @@ if simlongpath # else # error("provide valid number of observations ") # end - obsnum = 10 + obsnum = 15 if obsnum > 2 obsind = 1:(lt÷obsnum):lt obsnum = length(obsind) @@ -232,7 +232,7 @@ accparams = 0 mhsteps = 0 mhstepsparams = 0 -Hzero⁺ = SMatrix{d,d}(0.01*I) +Hzero⁺ = SMatrix{d,d}(0.1*I) param(P) = [P.β P.σ1] logπ(P) = logpdf(Gamma(1,100),P.β) + logpdf(Gamma(1,100),P.σ1) @@ -333,7 +333,6 @@ for iter in 1:iterations end if updateparams # this term is still not correct diffll += (V.tt[end]-V.tt[1]) *(tr(Bridge.B(0.0, Ptᵒ)) - tr(Bridge.B(0.0,Pt))) + logπ(Pᵒ) - logπ(P) - #diffll += (V.tt[2]-V.tt[1]) *(tr(Bridge.B(0.0, Ptᵒ)) - tr(Bridge.B(0.0,Pt)))+ logπ(Pᵒ) - logπ(P) end println("ind: ",ind," updateparms= ",updateparams," diff_ll: ",round(diffll, digits=3)) @@ -375,6 +374,7 @@ println("Average acceptance percentage params: ",ave_acc_percparams,"\n") trueval = param(Ptrue) est = DataFrame(iteration = 1:length(C), beta= map(x->x[1],C), sigma1=map(x->x[2],C)) + @rput est #@rput P @rput trueval @@ -422,11 +422,15 @@ if write2csv writedlm(f,obs,",") close(f) +# CSV.write("parestimates.csv",est) fn = outdir*"parestimates.csv" f = open(fn,"w") - headl = "iterate, beta\n" + headl = "iterate, beta, sigma1 \n" write(f,headl) - writedlm(f,hcat(1:length(C), C),",") + truevals = [0 param(Ptrue)] + estim = hcat(1:length(C), first.(C),last.(C)) + writedlm(f,vcat(truevals,estim),",") + # iterate 0 corresponds to the true values close(f) end diff --git a/src/Bridge.jl b/src/Bridge.jl index f863a7f..3903808 100644 --- a/src/Bridge.jl +++ b/src/Bridge.jl @@ -39,10 +39,12 @@ using Statistics include("chol.jl") + using StaticArrays using Distributions using Colors - +using Trajectories +include(joinpath(dirname(Base.find_package(string(Trajectories))), "unroll1.jl")) diff --git a/src/ode!.jl b/src/ode!.jl index d640f6b..c962ee3 100644 --- a/src/ode!.jl +++ b/src/ode!.jl @@ -7,7 +7,17 @@ to solve ``y(t + dt) - y(t) = \\int_t^{t+dt} F(s, y(s)) ds``. struct R3! <: ODESolver end -workspace(::Bridge.R3!, y) = (copy(y), copy(y), copy(y), copy(y)) +workspace(::Bridge.R3!, y) = (copy(y), copy(y), copy(y), copy(y)) + +""" +One step for inplace Ralston (1965) update (order 3 step of the Bogacki–Shampine 1989 method) +to solve ``y(t + dt) - y(t) = \\int_t^{t+dt} f(s, y(s)) ds``. +Starting point is specified by (t,y) + +f!(t,y,k) is a function that takes (t,y) and writes the result in k. +ws contains 4 copies of the type of y +the result is written into out which is of type y +""" function kernelr3!(f!, t, y, ws, out, dt) y2, k1, k2, k3 = ws f!(t, y, k1) diff --git a/src/ode.jl b/src/ode.jl index 9a19734..66af761 100644 --- a/src/ode.jl +++ b/src/ode.jl @@ -47,6 +47,13 @@ function kernelr3(f, t, y, dt, P) k3 = f(t + 3/4*dt, y + 3/4*dt*k2, P) y + dt*(2/9*k1 + 1/3*k2 + 4/9*k3) end +function kernelr3dot(f, t, y, dt, P) + k1 = f(t, y, P) + k2 = f(t + 1/2*dt, y .+ (1/2*dt).*k1, P) + k3 = f(t + 3/4*dt, y .+ (3/4*dt).*k2, P) + y .+ (dt*2/9).*k1 .+ (dt*1/3).*k2 .+ (dt*4/9).*k3 +end + function kernelr3(t, y, dt, P) k1 = F(t, y, P) diff --git a/src/partialbridge.jl b/src/partialbridge.jl index fa79d32..35121b4 100644 --- a/src/partialbridge.jl +++ b/src/partialbridge.jl @@ -1,4 +1,3 @@ - function partialbridgeode!(::R3, t, L, Σ, Lt, Mt, μt, P) m, d = size(L) Lt[end] = L @@ -25,14 +24,10 @@ function partialbridgeode!(::R3, t, L, Σ, Lt, Mt, μt, P) """ PartialBridge - Guided proposal process for diffusion bridge using backward recursion. - PartialBridge(tt, P, Pt, L, v, Σ) - Guided proposal process for a partial diffusion bridge of `P` to `v` on the time grid `tt` using guiding term derived from linear process `Pt`. - Simulate with `bridge!`. """ struct PartialBridge{T,R,R2,Tv,TL,TM} <: ContinuousTimeProcess{T} @@ -82,9 +77,10 @@ function llikelihood(::LeftRule, Xcirc::SamplePath, Po::PartialBridge; skip = 0) som += dot( _b((i,s), x, target(Po)) - _b((i,s), x, auxiliary(Po)), r ) * (tt[i+1]-tt[i]) if !constdiff(Po) - H = H((i,s), x, Po) - som -= 0.5*tr( (a((i,s), x, target(Po)) - aitilde((i,s), x, Po))*(H) ) * (tt[i+1]-tt[i]) - som += 0.5*( r'*(a((i,s), x, target(Po)) - aitilde((i,s), x, Po))*r ) * (tt[i+1]-tt[i]) + H = Bridge.H((i,s), x, Po) + A = Bridge.a((i,s), x, target(Po)) - Bridge.a((i,s), auxiliary(Po)) + som -= 0.5*tr(A *H ) * (tt[i+1]-tt[i]) + som += 0.5*( r'*A*r ) * (tt[i+1]-tt[i]) end end som diff --git a/src/partialbridgen!.jl b/src/partialbridgen!.jl index f2691f2..c52ba63 100644 --- a/src/partialbridgen!.jl +++ b/src/partialbridgen!.jl @@ -69,6 +69,10 @@ function rti!((t,i), x, out, P::PartialBridge!) out end +function Hi((t,i), P::PartialBridge!) + P.H[i] +end + btitilde!((t,i), x, out, P) = _b!((t,i), x, out, P.Pt) constdiff(P::PartialBridge!) = constdiff(P.Target) && constdiff(P.Pt) @@ -92,9 +96,26 @@ function llikelihood(::LeftRule, Xcirc::SamplePath, Po::PartialBridge!; skip = 0 som += dot(bout, rout) * (tt[i+1]-tt[i]) som -= dot(btout, rout) * (tt[i+1]-tt[i]) - if !constdiff(Po) - error("not implemented") + if !constdiff(Po) # todo: write test + H = Bridge.Hi((i,s), Po) + A = Bridge.a((i,s), x, target(Po)) + At = Bridge.a((i,s), x, auxiliary(Po)) + som -= 0.5*hadamtrace(A, H) * (tt[i+1]-tt[i]) + som += 0.5*hadamtrace(At, H) * (tt[i+1]-tt[i]) + som += 0.5*( r'*(A-At)*r ) * (tt[i+1]-tt[i]) end end som end + +function hadamtrace(A, H) + @assert eachindex(A) == eachindex(H) + @unroll1 for i in eachindex(A) + if $first + som = A[i]*H[i] + else + som += A[i]*H[i] + end + end + som +end diff --git a/src/partialbridgenuH.jl b/src/partialbridgenuH.jl index 9ece707..71ae157 100644 --- a/src/partialbridgenuH.jl +++ b/src/partialbridgenuH.jl @@ -1,42 +1,105 @@ -function partialbridgeodeνH!(::R3, t, L, Σ, v, νt, Ht, P, ϵ) +function updateνH⁺(L, Σ, v, ϵ) m, d = size(L) - # print(typeof(H⁺t)) - # print(typeof(νt)) - # print(typeof(inv(L' * inv(Σ) * L + ϵ * I) )) - Ht[end] = (L' * inv(Σ) * L + ϵ * I) - H⁺ = inv(Ht[end]) - νt[end] = H⁺ * L' * inv(Σ) * v - ν = νt[end] - F(t, y, P) = B(t, P)*y + β(t,P) - Ri(t, y, P) = B(t, P)*y + y * B(t,P)' - a(t, P) + @assert m == length(v) + H = (L' * inv(Σ) * L + ϵ * I) + H⁺ = inv(H) + ν = H⁺ * L' * inv(Σ) * v + ν, H⁺ +end +function updateC(L, Σ, v, C = 0.0) + m, d = size(L) + C += 0.5 * dot(v, Σ\v) + C += m/2*log(2pi) + 0.5*logdet(Σ) + C +end +function updateνH⁺C(L, Σ, v, ϵ) + updateνH⁺(L, Σ, v, ϵ)..., updateC(L, Σ, v) +end + + + +function partialbridgeodeνH!(::R3, t, νt, Ht, P, (ν, H⁺, C)) + #m, d = size(L) + #@assert m == length(v) + Ht[end] = H = inv(H⁺) + νt[end] = ν + + b̃(t, y, P) = B(t, P)*y + β(t, P) + dH⁺(t, y, P) = B(t, P)*y + (B(t, P)*y)' - a(t, P) +# dH(t, y, P) = - B(t, P)'*y - y * B(t,P) + y*a(t, P)*y' +# dF(t, y, (H,P)) = -B(t, P)'*y + H*a(t, P)*y + H*β(t, P) + dC(t, (F, H), P) = dot(β(t, P), F) + 0.5*dot(F, a(t, P)*F) - 0.5*tr(H*a(t, P)) +# function dS(t, (ν, H⁺, C), P) +# H = inv(H⁺) +# b̃(t, ν, P), dH⁺(t, H⁺, P), dC(t, (H*ν, H), P) +# end + + for i in length(t)-1:-1:1 dt = t[i] - t[i+1] - ν = kernelr3(F, t[i+1], ν, dt, P) - H⁺ = kernelr3(Ri, t[i+1], H⁺, dt, P) + H⁺ = kernelr3(dH⁺, t[i+1], H⁺, dt, P) + #H = kernelr3(dH, t[i+1], H, dt, P) + #F = kernelr3(dF, t[i+1], F, dt, (H,P)) + #ν, H⁺, C = kernelr3dot(dS, t[i+1], (ν, H⁺, C), dt, P) + + F = H*ν + C += dC(t[i+1], (F, H), P)*dt + + + νt[i] = ν = kernelr3(b̃, t[i+1], ν, dt, P) + Ht[i] = H = inv(H⁺) - νt[i] = ν - Ht[i] = inv(H⁺) end - νt, Ht - end + + νt, Ht, C +end + +function updateFHC(L, Σ, v, F, H, ϵ = 0.0, C = 0.0) + m, d = size(L) + H += (L' * inv(Σ) * L + ϵ * I) + F += L' * inv(Σ) * v + F, H, updateC(L, Σ, v, C) +end + +function partialbridgeodeHνH!(::R3, t, Ft, Ht, P, (F, H, C)) + Ht[end] = H + Ft[end] = F + + dH(t, y, P) = - B(t, P)'*y - y * B(t,P) + y*a(t, P)*y' + dF(t, y, (H,P)) = -B(t, P)'*y + H*a(t, P)*y + H*β(t, P) + + for i in length(t)-1:-1:1 + dt = t[i] - t[i+1] + C += β(t[i+1], P)'*F*dt + 0.5*F'*a(t[i+1], P)*F*dt - 0.5*tr(H*a(t[i+1], P))*dt + H = kernelr3(dH, t[i+1], H, dt, P) + F = kernelr3(dF, t[i+1], F, dt, (H, P)) + Ft[i] = F + Ht[i] = H + end + + Ft, Ht, C +end + struct Lyap end -function partialbridgeodeνH!(::Lyap, t, νt, Ht, P, νend, Hend⁺) - Ht[end] = inv(Hend⁺) +function partialbridgeodeνH!(::Lyap, t, νt, Ht, P, (νend, Hend⁺, C)) + Ht[end] = H = inv(Hend⁺) νt[end] = νend H⁺ = Hend⁺ ν = νend - F(t, y, P) = B(t, P)*y + β(t,P) + b̃(t, y, P) = B(t, P)*y + β(t,P) for i in length(t)-1:-1:1 dt = t[i] - t[i+1] - ν = kernelr3(F, t[i+1], ν, dt, P) + ν = kernelr3(b̃, t[i+1], ν, dt, P) H⁺ = lyapunovpsdbackward_step(t[i+1], H⁺, -dt, P) + F = Ht[i+1]*νt[i+1] + C += β(t[i+1], P)'*F*dt + 0.5*F'*a(t[i+1], P)*F*dt - 0.5*tr(H*a(t[i+1], P))*dt νt[i] = ν - Ht[i] = inv(H⁺) + Ht[i] = H = inv(H⁺) end - ν, H⁺ + ν, H⁺, C end @@ -56,14 +119,15 @@ Guided proposal process for diffusion bridge using backward recursion. linear process `Pt` with backwards equation initialized at `ν, Hend⁺`. """ -struct PartialBridgeνH{T,TP,TPt,Tν,TH} <: ContinuousTimeProcess{T} +struct PartialBridgeνH{T,TP,TPt,Tν,TH,TC} <: ContinuousTimeProcess{T} Target::TP Pt::TPt tt::Vector{Float64} ν::Vector{Tν} H::Vector{TH} - PartialBridgeνH(P::TP, Pt::TPt, tt, νt::Vector{Tν}, Ht::Vector{TH}) where {TP,TPt,Tν,TH} = - new{Bridge.valtype(P),TP,TPt,Tν,TH}(P, Pt, tt, νt, Ht) + C::TC + PartialBridgeνH(P::TP, Pt::TPt, tt, νt::Vector{Tν}, Ht::Vector{TH}, C::TC=0.0) where {TP,TPt,Tν,TH,TC} = + new{Bridge.valtype(P),TP,TPt,Tν,TH,TC}(P, Pt, tt, νt, Ht, C) # 6-7 arg @@ -75,8 +139,9 @@ struct PartialBridgeνH{T,TP,TPt,Tν,TH} <: ContinuousTimeProcess{T} Ht = zeros(TH, N) Tν = typeof(@SVector zeros(d)) νt = zeros(Tν, N) - partialbridgeodeνH!(R3(), tt, L, Σ, v, νt, Ht, Pt, ϵ) - PartialBridgeνH(P, Pt, tt, νt, Ht) + ν, H⁺, C = updateνH⁺C(L, Σ, v, ϵ) + _, _, C = partialbridgeodeνH!(R3(), tt, νt, Ht, Pt, (ν, H⁺, C)) + PartialBridgeνH(P, Pt, tt, νt, Ht, C) end end # 5 arg @@ -85,8 +150,8 @@ function partialbridgeνH(tt_, P, Pt, νend::Tv, Hend⁺::TH) where {Tv,TH} N = length(tt) Ht = zeros(TH, N) νt = zeros(Tv, N) - ν, H⁺ = partialbridgeodeνH!(Lyap(), tt, νt, Ht, Pt, νend, Hend⁺) - PartialBridgeνH(P, Pt, tt, νt, Ht), ν, H⁺ + ν, H⁺, C = partialbridgeodeνH!(Lyap(), tt, νt, Ht, Pt, (νend, Hend⁺, C)) + PartialBridgeνH(P, Pt, tt, νt, Ht, C), ν, H⁺, C end function _b((i,t)::IndexedTime, x, P::PartialBridgeνH) @@ -101,7 +166,7 @@ a(t, x, P::PartialBridgeνH) = a(t, x, P.Target) Γ(t, x, P::PartialBridgeνH) = Γ(t, x, P.Target) constdiff(P::PartialBridgeνH) = constdiff(P.Target) && constdiff(P.Pt) - +lptilde(x, P::PartialBridgeνH) = -0.5*((P.ν[1] - x)'*P.H[1] * (P.ν - x)) - P.C function llikelihood(::LeftRule, Xcirc::SamplePath, Po::PartialBridgeνH; skip = 0) tt = Xcirc.tt diff --git a/src/sde!.jl b/src/sde!.jl index 350c5b4..18aaa79 100644 --- a/src/sde!.jl +++ b/src/sde!.jl @@ -1,5 +1,23 @@ +""" +Inplace sde solver. +Typical call is like +solve!(EulerMaruyama!(), X, x0, W, P) +or +solve!(StratonovichHeun!(), X, x0, W, P) + +Here +- x0 is the starting point, +- W the driving Wiener process, +- P the ContinuousTimeProcess + +Requires +- drift function _b!((i,t¯), y, tmp1, P) +- diffusion function σ!(t¯, y, dw, tmp2, P) + +The result is written into X +""" function solve!(solver::EulerMaruyama!, Y::VSamplePath, u::T, W, P::ProcessOrCoefficients) where {T} N = length(W) N != length(Y) && error("Y and W differ in length.") diff --git a/src/types.jl b/src/types.jl index ede49bf..8a1f0f7 100644 --- a/src/types.jl +++ b/src/types.jl @@ -136,9 +136,14 @@ function stack(args::SamplePath...) VSamplePath(args[1].tt, vcat((X.yy' for X in args)...)) end -# separate a Zip2 +# separate a zip of two Vectors +if VERSION < v"1.1-" sep(Z::Base.Iterators.Zip2{Vector{T1},Vector{T2}}) where {T1,T2} = T1[z[1] for z in Z], T2[z[2] for z in Z] # takes into account the minimum of length +else +sep(Z::Base.Iterators.Zip{Tuple{Vector{T1},Vector{T2}}}) where {T1,T2} = + T1[z[1] for z in Z], T2[z[2] for z in Z] # takes into account the minimum of length +end copy(X::VSamplePath) = VSamplePath(copy(X.tt), copy(X.yy)) diff --git a/test/bessel.jl b/test/bessel.jl index 8fbb4a0..a8f98da 100644 --- a/test/bessel.jl +++ b/test/bessel.jl @@ -25,10 +25,10 @@ function hit(u, v, dt, tmax, P::ContinuousTimeProcess{T}) where T t = 0. rdt = sqrt(dt) - + y::T = u - - + + while sign(v-y) == sign(v-u) && t < tmax t += dt y += Bridge.b(t, y, P)*dt + Bridge.σ(t, y, P)*rdt*randn(T) @@ -40,13 +40,13 @@ struct Target <: ContinuousTimeProcess{Float64} mu end - + Bridge.b(t, x, P::Target) = -P.mu*x Bridge.σ(t, x, P::Target) = sqrt(2.) Bridge.a(t, x, P::Target) = 2. Bridge.Γ(t, x, P::Target) = 0.5 - + Bridge.constdiff(P::Target) = true x0 = 0.5 @@ -106,14 +106,14 @@ VERBOSE && println("X") X = solve(Euler(), x0, Bridge.sample(tt,Bridge.Wiener{Float64}()), P) while any(X.yy .< 0) || X.yy[end] > 0.1 global X = solve(Euler(), x0, Bridge.sample(tt,Bridge.Wiener{Float64}()), P) -end +end VERBOSE && println("Xo") @test abs(p - 0.1788) < 0.015 @test abs(pt - pt_) < 0.015 -@test abs(phat - 0.1788)<0.015 -@test abs(phat3 - 0.1788)<0.015 -@test abs(ahat(B3) - 2) < 0.15 -@test abs(ahat(Xo) - 2) < 0.15; \ No newline at end of file +@test abs(phat - 0.1788) < 0.015 +@test abs(phat3 - 0.1788) < 0.015 +@test abs(ahat(B3) - 2) < 0.17 +@test abs(ahat(Xo) - 2) < 0.15; diff --git a/test/partialbridge!.jl b/test/partialbridge!.jl index 2e2b443..c383c58 100644 --- a/test/partialbridge!.jl +++ b/test/partialbridge!.jl @@ -2,83 +2,35 @@ using Bridge, Distributions using Test, Statistics, Random, LinearAlgebra using Bridge.Models -T = 2.0 -dt = 1/1000 +include("partialparam.jl") +x0 = Vector(x0) +L = Matrix(L) +Σ = Matrix(Σ) +v = Vector(v) -tt = 0.:dt:T -struct PBIntegratedDiffusion <: ContinuousTimeProcess{Float64} - γ::Float64 -end -struct PBIntegratedDiffusionAux <: ContinuousTimeProcess{Float64} - γ::Float64 -end - -PorPtilde = Union{PBIntegratedDiffusion, PBIntegratedDiffusionAux} - - -βu(t, x::Float64, P::PBIntegratedDiffusion) = - (x+sin(x)) + 1/2 -βu(t, x::Float64, P::PBIntegratedDiffusionAux) = -x + 1/2 -# not really a 'beta' - -Bridge.b(t::Float64, x, P::PorPtilde) = Bridge.b!(t, x, copy(x), P) -function Bridge.b!(t, x, out, P::PorPtilde) - out[1] = x[2] - out[2] = βu(t, x[2], P) - out -end - -function Bridge.σ!(t, x, dm, out, P::PorPtilde) - out[1] = 0.0 - out[2] = dm*P.γ - out -end -function Bridge.a(t, P::PorPtilde) - [0.0 0.0; 0.0 P.γ^2] -end -Bridge.a(t, x, P::PorPtilde) = Bridge.a(t, P::PorPtilde) - -Bridge.constdiff(::PorPtilde) = true - -function Bridge.B!(t, arg, out, P::PBIntegratedDiffusionAux) - B = [0.0 1.0; 0.0 -1.0] - out .= (B*arg) - out -end -function BBt!(t, arg, out, P::PBIntegratedDiffusionAux) - B = [0.0 1.0; 0.0 -1.0] - out .= (B*arg + arg*B') - out -end - -function Bridge.dP!(t, p, out, P) - BBt!(t, p, out, P) - out[2,2] -= P.γ^2 - out -end # Generate Data Random.seed!(1) -P = PBIntegratedDiffusion(0.7) -Pt = PBIntegratedDiffusionAux(0.7) +P = PBIntegratedDiffusion(γ) +Pt = PBIntegratedDiffusionAux(γ) W1 = sample(tt, Wiener()) -x0 = [2.0, 1.0] + X1 = solve(EulerMaruyama!(), x0, W1, P) -L = [1. 0.] -Σnoise = fill(0.01, 1, 1) -v = [2.5] + + # Solve Backward Recursion N = length(tt) νt = [zero(x0) for i in 1:N] Σt = [zero(Bridge.outer(x0)) for i in 1:N] -ϵ = 0.02 -Bridge.partialbridgeode!(Bridge.R3!(), tt, L, Σnoise, v, νt, Σt, Pt, ϵ) + +Bridge.partialbridgeode!(Bridge.R3!(), tt, L, Σ, v, νt, Σt, Pt, ϵ) j = length(tt)÷2 @@ -87,14 +39,13 @@ A = Bridge.a(NaN, Pt) @test norm((νt[j+1] - νt[j])/dt - (Bridge.b(tt[j+1], νt[j], Pt))) < 0.01 j = length(tt) - 3 @test norm((B*Σt[j+1] + Σt[j+1]*B' - Bridge.a(tt[j+1], Pt)) - Bridge.dP!(tt[j+1], Σt[j+1], copy(Σt[j+1]), Pt)) < 0.01 -@test_broken norm((Σt[j+1] - Σt[j])/dt - (B*Σt[j+1] + Σt[j+1]*B' - Bridge.a(tt[j+1], Pt))) < 0.01 +#@test_broken norm((Σt[j+1] - Σt[j])/dt - (B*Σt[j+1] + Σt[j+1]*B' - Bridge.a(tt[j+1], Pt))) < 0.01 -Po1 = Bridge.PartialBridge!(tt, P, Pt, L, v, ϵ, Σnoise) +Po1 = Bridge.PartialBridge!(tt, P, Pt, L, v, ϵ, Σ) @test Po1.H == inv.(Σt) -x0 = [2.0, 1.0] Xo1 = deepcopy(X1) solve!(Bridge.EulerMaruyama!(), Xo1, x0, W1, Po1) @@ -159,6 +110,6 @@ function mcmc1(x0, tt, Po2) end @time @testset "MCMC1" begin - mcmc1(x0, tt, Po2) + mcmc1(x0, tt, Po1) end diff --git a/test/partialbridge.jl b/test/partialbridge.jl index 1c5fe62..8687f2e 100644 --- a/test/partialbridge.jl +++ b/test/partialbridge.jl @@ -3,10 +3,7 @@ using Test, Statistics, Random, LinearAlgebra using Bridge.Models -T = 2.0 -dt = 1/100 - -tt = 0.:dt:T +include("partialparam.jl") struct IntegratedDiffusion <: ContinuousTimeProcess{ℝ{2}} γ::Float64 end @@ -35,16 +32,14 @@ Bridge.constdiff(::IntegratedDiffusionAux) = true # Generate Data Random.seed!(1) -P = IntegratedDiffusion(0.7) -Pt = IntegratedDiffusionAux(0.7) +P = IntegratedDiffusion(γ) +Pt = IntegratedDiffusionAux(γ) W = sample(tt, Wiener()) -x0 = ℝ{2}(2.0, 1.0) + X = solve(Euler(), x0, W, P) -L = @SMatrix [1. 0.] -Σ = @SMatrix [0.0] -v = ℝ{1}(2.5) + # Solve Backward Recursion @@ -64,15 +59,18 @@ j = 10 @test norm((μt[j+1] - μt[j])/dt - (-Lt[j+1]*Bridge.β(tt[j+1], Pt))) < 0.01 @test norm((inv(Mt[j+1]) - inv(Mt[j]))/dt - (-Lt[j+1]*Bridge.a(tt[j+1], Pt)*Lt[j+1]')) < 0.01 + + Po = Bridge.PartialBridge(tt, P, Pt, L, v, Σ) @test Po.L == Lt W = sample(tt, Wiener()) -x0 = ℝ{2}(2.0, 1.0) + Xo = copy(X) solve!(Euler(), Xo, x0, W, Po) +@show LP = log(pdf(Normal(μt[1][1] + (Lt[1]*x0)[1], (Mt[1][1])^(-0.5)), v[1])) # Likelihood diff --git a/test/partialbridgenuH.jl b/test/partialbridgenuH.jl index 611aab9..f3303e7 100644 --- a/test/partialbridgenuH.jl +++ b/test/partialbridgenuH.jl @@ -1,34 +1,62 @@ using StaticArrays -include("partialbridge!.jl") -T = 2.0 -dt = 1/1000 -tt = 0.:dt:T -#struct PBIntegratedDiffusion <: ContinuousTimeProcess{ℝ{2}} -# γ::Float64 -#end +struct PBIntegratedDiffusion <: ContinuousTimeProcess{Float64} + γ::Float64 +end +struct PBIntegratedDiffusionAux <: ContinuousTimeProcess{Float64} + γ::Float64 +end -Bridge.b(t::Float64, x, P::PBIntegratedDiffusion) = ℝ{2}(x[2], βu(t, x[2], P)) +PorPtilde = Union{PBIntegratedDiffusion, PBIntegratedDiffusionAux} -@test Bridge.b!(0.0, X1.yy[1], copy( X1.yy[1]), P) == Bridge.b(0.0, X1.yy[1], P) -Bridge.σ(t, x, P::PBIntegratedDiffusion) = ℝ{2}(0.0, P.γ) +βu(t, x::Float64, P::PBIntegratedDiffusion) = - (x+sin(x)) + 1/2 +βu(t, x::Float64, P::PBIntegratedDiffusionAux) = -x + 1/2 +# not really a 'beta' -@test Bridge.σ!(0.0, X1.yy[1], 1.0, copy( X1.yy[1]), P) == Bridge.σ(0.0, X1.yy[1], P) +Bridge.b(t::Float64, x, P::PorPtilde) = Bridge.b!(t, x, copy(x), P) +function Bridge.b!(t, x, out, P::PorPtilde) + out[1] = x[2] + out[2] = βu(t, x[2], P) + out +end +function Bridge.σ!(t, x, dm, out, P::PorPtilde) + out[1] = 0.0 + out[2] = dm*P.γ + out +end +function Bridge.a(t, P::PorPtilde) + [0.0 0.0; 0.0 P.γ^2] +end +Bridge.a(t, x, P::PorPtilde) = Bridge.a(t, P::PorPtilde) -Bridge.constdiff(::PBIntegratedDiffusion) = true +Bridge.constdiff(::PorPtilde) = true -#struct PBIntegratedDiffusionAux <: ContinuousTimeProcess{ℝ{2}} -# γ::Float64 -#end +function Bridge.B!(t, arg, out, P::PBIntegratedDiffusionAux) + B = [0.0 1.0; 0.0 -1.0] + out .= (B*arg) + out +end +function BBt!(t, arg, out, P::PBIntegratedDiffusionAux) + B = [0.0 1.0; 0.0 -1.0] + out .= (B*arg + arg*B') + out +end -Bridge.b(t::Float64, x, P::PBIntegratedDiffusionAux) = ℝ{2}(x[2], βu(t, x[2], P)) +function Bridge.dP!(t, p, out, P) + BBt!(t, p, out, P) + out[2,2] -= P.γ^2 + out +end -@test Bridge.b!(0.0, X1.yy[1], copy( X1.yy[1]), Pt) == Bridge.b(0.0, X1.yy[1], Pt) +Bridge.constdiff(::PBIntegratedDiffusion) = true +Bridge.b(t::Float64, x, P::PBIntegratedDiffusion) = ℝ{2}(x[2], βu(t, x[2], P)) +Bridge.σ(t, x, P::PBIntegratedDiffusion) = ℝ{2}(0.0, P.γ) +Bridge.b(t::Float64, x, P::PBIntegratedDiffusionAux) = ℝ{2}(x[2], βu(t, x[2], P)) Bridge.σ(t, P::PBIntegratedDiffusionAux) = ℝ{2}(0.0, P.γ) Bridge.σ(t, x, P::PBIntegratedDiffusionAux) = Bridge.σ(t, P) @@ -37,48 +65,89 @@ Bridge.B(t, P::PBIntegratedDiffusionAux) = @SMatrix [0.0 1.0; 0.0 -1.0] Bridge.β(t, P::PBIntegratedDiffusionAux) = ℝ{2}(0, 1/2) Bridge.a(t, P::PBIntegratedDiffusionAux) = @SMatrix [0.0 0.0; 0.0 P.γ^2] +SKIP! = true +if !SKIP! + +include("partialbridge!.jl") + + +@test Bridge.b!(0.0, X1.yy[1], copy( X1.yy[1]), P) == Bridge.b(0.0, X1.yy[1], P) + + +@test Bridge.σ!(0.0, X1.yy[1], 1.0, copy( X1.yy[1]), P) == Bridge.σ(0.0, X1.yy[1], P) + +@test Bridge.b!(0.0, X1.yy[1], copy( X1.yy[1]), Pt) == Bridge.b(0.0, X1.yy[1], Pt) + +end + +include("partialparam.jl") + # Generate Data Random.seed!(1) W2 = sample(tt, Wiener()) -@test W2.yy == W1.yy -X2 = solve(Euler(), ℝ{2}(x0), W2, P) +X2 = solve(Euler(), x0, W2, P) + -@test X2.yy == X1.yy +if !SKIP! + @test W2.yy == W1.yy + @test X2.yy == X1.yy -L = @SMatrix [1. 0.] -Σ = @SMatrix [0.01] -v = ℝ{1}(2.5) +end -ϵ = 0.02 -# Solve Backward Recursion -S2 = typeof(L) -S = typeof(L*L') -T = typeof(diag(L*L')) +Po2 = Bridge.PartialBridgeνH(tt, P, Pt, L, v, ϵ, Σ) -N = length(tt) -Lt = zeros(S2, N) -Ht = zeros(S, N) -νt = zeros(T, N) +Ft = copy(Po2.ν) +Ht = copy(Po2.H) +ν, H⁺, C_ = Bridge.updateνH⁺C(L, Σ, v, ϵ) -Po2 = Bridge.PartialBridgeνH(tt, P, Pt, L, v, ϵ, Σ) +F, H, C = Bridge.updateFHC(L, Σ, v, zero(Ft[end]), zero(Ht[end]), ϵ) +@test C ≈ C_ +@test F ≈ H*ν +@test H⁺ ≈ inv(H) -Xo2 = copy(X2) -solve!(Euler(), Xo2, ℝ{2}(x0), W2, Po2) -@test norm(Xo1.yy - Xo2.yy) < 500*eps() +Ft, Ht, C = Bridge.partialbridgeodeHνH!(Bridge.R3(), tt, Ft, Ht, Pt, (F, H, C)) +@test abs(C - Po2.C) < 0.03 +@test maximum(norm.(Ht .- Po2.H)./norm(Ht)) < 1e-5 +@test maximum(norm.(Po2.H .* Po2.ν .- Ft)) < 0.015 #not very precise, update if cond number becomes better +@test_broken cond(Ht[1]) < 1.e7 # + +#LP2 = -0.5*(x0'*Po2.H[1]*x0 - 2*x0'*Po2.F)[] - Po2.C +LP2 = -0.5*(x0'*Po2.H[1]*x0 - 2*x0'*Po2.H[1]*Po2.ν[1])[] - Po2.C + +@show LP, LP2 +@test abs(LP - LP2) < 0.01 + +# no epsilon +F, H, C = Bridge.updateFHC(L, Σ, v, zero(Ft[end]), zero(Ht[end]), 0.0) +Ft, Ht, C = Bridge.partialbridgeodeHνH!(Bridge.R3(), tt, Ft, Ht, Pt, (F, H, C)) +LP3 = -0.5*(x0'*Ht[1]*x0 - 2*x0'*Ft[1])[] - C +@test abs(LP - LP2) < 0.01 + + + +Xo2 = copy(X2) +solve!(Euler(), Xo2, x0, W2, Po2) + +if !SKIP! + @test norm(Xo1.yy - Xo2.yy)