diff --git a/GeneralisedFilters/CHANGELOG.md b/GeneralisedFilters/CHANGELOG.md index 660ef143..2ded0701 100644 --- a/GeneralisedFilters/CHANGELOG.md +++ b/GeneralisedFilters/CHANGELOG.md @@ -25,6 +25,9 @@ filters and CSMC ancestor sampling and backward simulation use the setting of their particle filter. Results depend on the generator state and `blocksize` but not on the number of threads. `SerialExecution()` remains the default and is unchanged. +- Compute only the triangular factor in square-root backward messages for larger static + Float32/Float64 matrices. This avoids forming Q and contended small-matrix BLAS calls, + speeding up CSMC ancestor sampling and backward simulation, especially with threads. ## 0.5.0 diff --git a/GeneralisedFilters/src/kernels/kalman.jl b/GeneralisedFilters/src/kernels/kalman.jl index 4225d5f9..3f741f43 100644 --- a/GeneralisedFilters/src/kernels/kalman.jl +++ b/GeneralisedFilters/src/kernels/kalman.jl @@ -268,18 +268,33 @@ not differentiated by the HMC parameter update. """ struct SqrtBackwardInformationPredictor <: AbstractBackwardPredictor end +# Backward messages need only R. For larger static matrices, qr(A) falls back to +# LAPACK and explicitly constructs Q; concurrent small factorizations also contend +# for BLAS workspace. Generic in-place QR on MMatrix keeps Householder reflectors +# implicit and uses Julia's unblocked implementation. Keep the unrolled static QR +# for tiny matrices, where it is faster. Other scalar/storage types retain qr's +# own dispatch, including for AD numbers. +_backward_qr_upper(A) = qr(A).R +function _backward_qr_upper(A::StaticMatrix{M,N,T}) where {M,N,T<:Union{Float32,Float64}} + if M <= 16 && N <= 8 + return qr(A).R + end + factors = qr!(MMatrix{M,N,T}(A)).factors + return triu(SMatrix{min(M, N),N}(factors[SOneTo(min(M, N)), SOneTo(N)])) +end + # Numerical primitive: compress one or two residual blocks without constructing # any filtering objects. Backends may avoid materialising the stacked matrix. function _qr_compress_residual(B, r) n = size(B, 2) # Padding also handles fewer residual rows than state dimensions. T = promote_type(eltype(B), eltype(r)) - R = qr(vcat(hcat(B, r), zeros(T, n + 1, n + 1))).R + R = _backward_qr_upper(vcat(hcat(B, r), zeros(T, n + 1, n + 1))) return R[1:n, 1:n], R[1:n, n + 1], abs2(R[n + 1, n + 1]) end function _qr_compress_residual(B::StaticMatrix{M,N}, r::StaticVector) where {M,N} T = promote_type(eltype(B), eltype(r)) - R = qr(vcat(hcat(B, r), zero(SMatrix{N + 1,N + 1,T}))).R + R = _backward_qr_upper(vcat(hcat(B, r), zero(SMatrix{N + 1,N + 1,T}))) return R[SOneTo(N), SOneTo(N)], R[SOneTo(N), N + 1], abs2(R[N + 1, N + 1]) end function _qr_compress_residual(B, r, C, q) @@ -295,11 +310,11 @@ end # Upper root of I + C*C', obtained without forming a potentially ill-conditioned Gram matrix. function _identity_plus_root(C) return _correct_cholesky_sign( - qr(vcat(Matrix{eltype(C)}(I, size(C, 1), size(C, 1)), C')).R + _backward_qr_upper(vcat(Matrix{eltype(C)}(I, size(C, 1), size(C, 1)), C')) ) end function _identity_plus_root(C::StaticMatrix{M,N,T}) where {M,N,T} - return _correct_cholesky_sign(qr(vcat(one(SMatrix{M,M,T}), C')).R) + return _correct_cholesky_sign(_backward_qr_upper(vcat(one(SMatrix{M,M,T}), C'))) end # Log determinant of F*F', retaining support for non-triangular covariance factors. diff --git a/GeneralisedFilters/test/algorithms/sqrt_backward.jl b/GeneralisedFilters/test/algorithms/sqrt_backward.jl index fb526584..27719092 100644 --- a/GeneralisedFilters/test/algorithms/sqrt_backward.jl +++ b/GeneralisedFilters/test/algorithms/sqrt_backward.jl @@ -46,6 +46,59 @@ end end +@testitem "Backward R-only QR preserves static roots and normalized likelihoods" begin + using GeneralisedFilters, StaticArrays, LinearAlgebra, Random, ForwardDiff + G = GeneralisedFilters + rng = Xoshiro(512) + for T in (Float32, Float64), (m, n) in ((4, 2), (32, 16), (17, 17), (7, 13)) + A = SMatrix{m,n,T}(randn(rng, T, m, n)) + # Full rank, rank deficient, and differently scaled columns all use QR; + # no Gram-matrix factorisation or positive-definiteness repair is needed. + for B in ( + A, + A * Diagonal(SVector{n,T}(ntuple(i -> i == 1 ? zero(T) : one(T), n))), + A * Diagonal(SVector{n,T}(10 .^ range(-4, 4; length=n))), + ) + saved = copy(B) + R = @inferred G._backward_qr_upper(B) + @test R isa SMatrix{min(m, n),n,T} + @test istriu(R) + @test R' * R ≈ B' * B rtol = 100eps(T) + @test B == saved + end + end + + # Exercise both the R-only compression and overlap paths at the dimension + # that exposed the threaded regression, against an independent forward KF. + for T in (Float32, Float64) + n, m = 16, 4 + p = GaussianPrior(zero(SVector{n,T}), one(SMatrix{n,n,T})) + d = LinearGaussianDynamics( + T(0.9) * one(SMatrix{n,n,T}), zero(SVector{n,T}), T(0.1) * one(SMatrix{n,n,T}) + ) + o = LinearGaussianObservation( + SMatrix{m,n,T}(randn(rng, T, m, n)), + zero(SVector{m,T}), + T(0.2) * one(SMatrix{m,m,T}), + ) + ys = [SVector{m,T}(randn(rng, T, m)) for _ in 1:5] + bp = SqrtBackwardInformationPredictor() + back = G.backward_initialise(bp, o, ys[end]) + for t in 4:-1:1 + back = G.backward_update(bp, G.backward_predict(bp, back, d), o, ys[t]) + end + pred = G.kalman_predict(GaussianState(p.μ0, p.Σ0), d) + @test G.compute_marginal_predictive_likelihood(pred, back; include_constant=true) ≈ + marginal_loglikelihood(StateSpaceModel(p, d, o), KF(), ys) rtol = 100eps(T) + @test back.B isa SMatrix{n,n,T} + end + + # Non-BLAS scalar types retain generic QR dispatch and differentiability. + C = SMatrix{9,9}(Matrix{Float64}(I, 9, 9)) + f(x) = sum(abs2, G._identity_plus_root(x * C)) + @test ForwardDiff.derivative(f, 0.3) ≈ 2 * 9 * 0.3 +end + @testitem "Explicit singular covariance factors preserve the model" begin using GeneralisedFilters, StaticArrays, LinearAlgebra, Statistics G = GeneralisedFilters