LowLevelParticleFilters compatibility with ComponentArrays

Specific Domains Modelling & Simulations
, ,
1

I’m developing a Kalman Filter for a system that is made up of many components. Since the model is somewhat big I was hoping to use a ComponentArray to extract individual system (and subsystem) states from the state vector.

Here is an example on what I want to achieve based on the quad tank example from the repo.

using LinearAlgebra
using LowLevelParticleFilters
using Distributions
using SeeToDee
using ComponentArrays


function quadtank_params!(dx, x, u, p, t)
    kc = 0.5
    k1, k2, g = 1.6, 1.6, 9.81
    A1 = A3 = A2 = A4 = 4.9
    a3, a2, a4 = 0.03, 0.03, 0.03
    γ1, γ2 = 0.2, 0.2

    (; h, a1) = x   # <- cleanly extract system states and parameters from the "states" vector

    ssqrt(x) = √(max(x, zero(x)) + 1e-3) # For numerical robustness at x = 0

    dx.h = [
        -a1 / A1 * ssqrt(2g * h[1]) + a3 / A1 * ssqrt(2g * h[3]) + γ1 * k1 / A1 * u[1]
        -a2 / A2 * ssqrt(2g * h[2]) + a4 / A2 * ssqrt(2g * h[4]) + γ2 * k2 / A2 * u[2]
        -a3 / A3 * ssqrt(2g * h[3]) + (1 - γ2) * k2 / A3 * u[2]
        -a4 / A4 * ssqrt(2g * h[4]) + (1 - γ1) * k1 / A4 * u[1]
    ]
    dx.a1 = 0  # <- easily update states/parameters in-place
end

measurement(x, u, p, t) = [x[1], x[2]]

discrete_dynamics_params = SeeToDee.Rk4(quadtank_params!, Ts, supersample=2)

nx = 5
R1 = Diagonal([0.1, 0.1, 0.1, 0.1, 0.0001])
R2 = Diagonal((1e-2)^2 * ones(ny))

x0 = ComponentArray(h=[2, 2, 3, 3], a1=0.02)
kf = UnscentedKalmanFilter(discrete_dynamics_params, measurement2, R1, R2, MvNormal(x0, R1); ny, nu)

The example does not work. Even just after initialization, the KF returns as state a vector that is no longer a ComponentArray.

julia> state(kf)
5-element Vector{Float64}:
 2.0
 2.0
 3.0
 3.0
 0.02

julia> x0
ComponentVector{Float64}(h = [2.0, 2.0, 3.0, 3.0], a1 = 0.02)

The issue seems to me that during the instantiation of the KF, the type of the state is converted from a ComponentVector to a simple Vector.

# from https://github.com/baggepinnen/LowLevelParticleFilters.jl/blob/3758d8e48d952cd5456ea10ada8916ce2ba2970f/src/kalman.jl#L15
function convert_x0_type(μ)
    if μ isa Vector || μ isa SVector
        return copy(μ)
    else
        return Vector(μ)
    end
end

Is there a reason to limit the x0 type to either a Vector or SVector?

2

The motivation for the conversion, IIRC, is that Distributions.jl uses a special type for a vector of all zeros, and that frequently caused issues if the initial state distribution was the zero-mean Gaussian. There are other ways to guard against that problem so we could lift the restriction if you’d like. Have you tried to see if it works by modifying the code to remove the conversion?

3

Thanks for the reply.

I added the following methods on top of my file.

LLPF.convert_x0_type(μ::ComponentVector) = copy(μ)
LLPF.convert_cov_type(R1, R::ComponentArray) = copy(R)

kf.x is now correctly a ComponentVector. But still filtering is not working out of the box. Probably since, when propagated in the algorithm, x is no longer a ComponentVector. This could be since multiplying a Matrix with ComponentVector returns a Vector. The Matrix needs also to have the “labels” integrated in order for it to work.

julia> x = ComponentArray(a = 1.0, b=2.0)
ComponentVector{Float64}(a = 1.0, b = 2.0)

julia> A = I(2)
2×2 Diagonal{Bool, Vector{Bool}}:
 1  ⋅
 ⋅  1

julia> A * x
ComponentVector{Float64}(a = 1.0, b = 2.0)

julia> A = rand(2,2)
2×2 Matrix{Float64}:
 0.83065   0.475515
 0.862176  0.503911

julia> A * x
2-element Vector{Float64}:
 1.7816796663382166
 1.869997367351198

julia> A = x * x'
2×2 ComponentMatrix{Float64} with axes Axis(a = 1, b = 2) × Axis(a = 1, b = 2) 
 1.0  2.0
 2.0  4.0

julia> A * x
ComponentVector{Float64}(a = 5.0, b = 10.0)

I am probably not providing all the inputs in their required form.

4

A simple solution would be to store the axis type in the parameter object and convert the state vector to a ComponentVector inside the dynamics function.

5

That’s a great simple solution. Thanks!

Probably making LLPF compatible with ComponentArrays would take some effort, since I found many places where it is assumed that the x is either a Vector or SVector.

Regardless, a great package!

1 Like