Sensor Fusion Examples

[navidivan]

Trying to determine if this package can be used for a real-time sensor fusion problem.
Problem statement is that we have multiple sensors, each with its own Kalman Filter models that are used for position estimation of a catheter. Looking to find an optimal Bayesian way of combining these estimates, and have the models inform each other. Maybe dynamically adjust the precision of the sensors based on some noise or consistency criteria (would we need a hierarchical model?!)
Are there examples of this?
What we need is to find out in a high level, what are the fundamental differences of using RxInfer for this task compared to Sensor Fusion Toolbox in Matlab or a probabilistic NN or something like that.

[bartvanerp]

Hi @navidivan!

Thanks for contacting us. I definitely think that this should be possible. The main challenge would be to construct a generative model that combines the different measurements. I would like to invite you to check out the following notebook: https://examples.rxinfer.com/categories/advanced_examples/nonlinear_sensor_fusion/. It might help you conceptualize the problem a bit better.
I must say that I am personally unfamiliar with the Matlab toolbox, so I am not in the position to comment on this. Probabilistic neural networks could also be nice, but these are black box, so this might be undesirable depending on the problem.

Please let me know what you think about this!

Bart

[albertpod]

Thanks @bartvanerp. I just wanted to add that it might be a good idea to make a notebook/example with a hierarchical model for sensor fusion.

@navidivan please note that the referenced sensor fusion example is a bit old, so it might not be the best way of solving this problem now, with new RxInfer capabilities.

AFAIK, MATLAB's toolbox is more focused on specific sensor fusion algorithms rather than a general probabilistic framework, whereas RxInfer is specifically designed as a reactive probabilistic programming framework, allowing you to express Bayesian models with reactive streams of data. RxInfer allows explicitly defining probabilistic relationships through factor graphs, giving you more control over model structure than black-box approaches. For certain problems, message passing in RxInfer can be significantly more computationally efficient.

[navidivan]

Thank you, @bartvanerp and @albertpod. This was a fantastic example and an excellent demonstration. However, I am still a bit confused.

First, it would be very helpful to see a visualization of the factor graph in each example. When I attempted to use the recommended libraries, I encountered several errors.

Second, I had trouble understanding some of the deeper mathematics and equations, particularly the underlying CVI algorithm and why it was chosen over other solvers.

An interesting extension might involve introducing an additional sensor that uses a different measurement equation—potentially another nonlinear function. We could then make the model hierarchical, first combining the distance-based filters and then incorporating the new sensor at a second level, where we also allow the noise estimates of each branch to be dynamically inferred. This would show how to extend the model to more complex systems, such as ours, which features six location sensors, magnetometer trackers, a field estimator, and so forth.

Again, thank you. If there are additional resources you could point me to, I would greatly appreciate it.

[albertpod]

@navidivan it would be helpful if you shared the error for the graph visualization.
I am not sure why CVI has been used in that example since it works with Gaussians, so perhaps it's just for a showcase.
For different approximations around nonlinear nodes, you can take a look here: https://docs.rxinfer.com/stable/manuals/inference/delta-node/.
I think I understand what you mean with hierarchical modeling, but it would be very helpful if you could supply some state space equations, so we can show how to model those with rxinfer!

[navidivan]

this is when Using GraphPlot:

distances = [distances[t, :] for t in 1:nr_observations]
W = diageye(2)
R = diageye(3)

model_instance = random_walk_model(y = distances, W = W, R = Rational)
GraphPlot.gplot(model_instance)

MethodError: no method matching spring_layout(::GraphPPL.ModelGenerator{typeof(random_walk_model), Base.Pairs{Symbol, Any, Tuple{Symbol, Symbol, Symbol}, @NamedTuple{y::Vector{Vector{Vector{Vector{Vector{Float64}}}}}, W::Matrix{Float64}, R::UnionAll}}, GraphPPL.PluginsCollection{Tuple{}}, RxInfer.ReactiveMPGraphPPLBackend{Static.False}, String})
The function spring_layout exists, but no method is defined for this combination of argument types.

Closest candidates are:
spring_layout(!Matched::Graphs.AbstractGraph, !Matched::Integer; kws...)
@ GraphPlot C:\Users\hakimn1.julia\packages\GraphPlot\daKPU\src\layout.jl:181
spring_layout(!Matched::Graphs.AbstractGraph, !Matched::AbstractVector{R1}, !Matched::AbstractVector{R2}; C, MAXITER, INITTEMP) where {R1<:Real, R2<:Real}
@ GraphPlot C:\Users\hakimn1.julia\packages\GraphPlot\daKPU\src\layout.jl:104
spring_layout(!Matched::Graphs.AbstractGraph, !Matched::AbstractVector{R1}; ...) where R1<:Real
@ GraphPlot C:\Users\hakimn1.julia\packages\GraphPlot\daKPU\src\layout.jl:104
...

I tried multiple routes, and could not debug using chatgpt4o. Also tried other libraries. Is there an example for how to do visualizations? Or could you give me a quick snippet that could work? Preferably a Forney type Factor Graph visualization, or a Markov model that shows temporal depth, or something that one can use to quickly confirm the logic

[albertpod]

@FraserP117 can you take a look here?

[FraserP117]

Thanks Albert, I’ll talk a look at this.

…

On Fri, 14 Mar 2025 at 3:53 pm, Albert ***@***.***> wrote: @FraserP117 <https://github.com/FraserP117> can you take a look here? — Reply to this email directly, view it on GitHub <#443 (reply in thread)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AKMKRHLP7D5RNYWDSNVQMIT2UL3RDAVCNFSM6AAAAABY3USU4WVHI2DSMVQWIX3LMV43URDJONRXK43TNFXW4Q3PNVWWK3TUHMYTENJQGI4DCNY> . You are receiving this because you were mentioned.Message ID: ***@***.*** com>

[FraserP117]

Thanks for your enthusiasm for @navidivan!

It can be very tricky to debug and modify even base Julia in some instances; I empathise with your situation.

Regarding your desire to visualize the model that you're working on, I have been contributing to the development of this functionality for RxInfer, alongside @bvdmitri, @chbe-helix and @kobus78 . These efforts are ongoing but you can currently use it to generate relatively nice visualisations of arbitrary RxInfer models with GraphViz.jl. Consequently, you will need to install GraphPPL.jl for any of the following to work. This functionality is not adequately explained in the docs - that is my fault, I have not updated the docs on visualization just yet.

Context and Background

You may or may not know that RxInfer is actually made up of three constituent Julia packages, one of which is called GraphPPL.jl. See here also. This package defines the standard and means for specifying a probabilistic graphical model in RxInfer. This is actually where the @model macro comes from.

I wrote the new graph visualization functionality in such a way as to take in the underlying GraphPPL.Model used by RxInfer as the input to the graph engine. I need to change this as it is needlessly complicated and forces the user to think about things that they don't need to think about.

There is a lot I could explain here as to how you can use this functionality but I will be as terse as possible to keep this short. I am of course, very happy to elaborate on any details and I desperately need to update the existing docs anyway.

Suffice it to say that the probabilistic graphical mode will be depicted as a "proper" factor graph, where shaded square nodes denote factors and circular white nodes denote variables. This is not a depiction of a Forney Style factor graph, of the kind that you might have seen in any papers on the matter. My team and I plan to support the visualization of the Forney Style factor graph in future contributions. As it stands, our visualization will return exactly what is specified in memory (minus some potentially additional things like factorisation and form constraints).

Example 1: Coin Toss Model

Taking the Coin Toss model as an example:

using RxInfer
using Random
using GraphViz

# GraphPPL.jl export `@model` macro for model specification
# It accepts a regular Julia function and builds a factor graph under the hood
@model function coin_model(y, a, b)

    # We endow θ parameter of our model with "a" prior
    θ ~ Beta(a, b)
    # note that, in this particular case, the `Uniform(0.0, 1.0)` prior will also work.
    # θ ~ Uniform(0.0, 1.0)

    # here, the outcome of each coin toss is governed by the Bernoulli distribution
    for i in eachindex(y)
        y[i] ~ Bernoulli(θ)
    end
end

If I now condition this model on a dataset of say, three Bernoulli trials (three flips of the coin, if you will):

conditioned = coin_model(a = 2.0, b = 7.0) | (y = [ true, false, true ], )

and then create the final RxInfer model:

rxi_model = RxInfer.create_model(conditioned)

I now have the final RxInfer model for this static dataset of three coin flips. In any case, here is how to use the GraphViz visualization functionality:

First, use the RxInfer.getmodel function to get the underling GraphPPL.Model:

gppl_model = RxInfer.getmodel(rxi_model)

Then, simply call GraphViz.load like this:

graph = GraphViz.load(
    gppl_model,
    strategy = :simple # this specifies the iteration strategy that you wish the visualization engine perform. Either :simple or :bfs for now but you must specify either of these as it stands - another issue I need to fix. 
)

You'll get a return value from GraphViz.load that contains the resulting GraphViz visualization. However, if you're using Jupyter notebook or any interactive IDE, you should get a pop up of the visualization that looks something like this:

coin_model_simple_itr_neato.pdf

Example 2: A Simpler Model

For an even simpler example, consider this model I just made up out of the blue:

using RxInfer
using GraphViz

@model function simple_model_def(n)
    z_0 ~ Normal(mean = 0, var = 1)  # Initial latent state, say
    for t in 1:n
        z_t ~ Normal(mean = z_0, var = 1.0)  # Our "State transition"
        x_t ~ Normal(mean = z_t, var = 1.0)  # Our "Observation"
    end
end

Create the model:

simple_model = simple_model_def(n = 5);  # Create a model instance with 5 time steps
rxi_simple_model = RxInfer.create_model(simple_model)

and generate the visualization:

graph = GraphViz.load(
    rxi_simple_model, 
    strategy = :bfs,
    layout = "dot"
)

Which should appear something like:

You'll note that I'm now using a different layout algorithm from GraphViz, the dot layout. As of right now I can support dot and neato, the latter being default. You can use any combination of the two existing iteration strategies: :simple and :bfs along with any layout algorithm specified by GraphViz.

Conclusion:

Hopefully this sheds some light on the matter of model visualizations with RxInfer. Please do ask any clarificatory or follow up questions you wish, I'm very keen for user feedback and suggestions. I think I'll tend to some refactoring and documentation efforts to improve the usability of the extension. We will be working on, improving and expanding the functionality of this extension going forward.

I hope that was useful!

[bvdmitri]

Thanks @FraserP117 , would you have time to create a section in RxInfer docs with these examples? Would be nice to have a reference in the documentation

[FraserP117]

Yes, absolutely @bvdmitri. I may have to leave it until Thursday but I will begin the work of writing these ASAP.

[navidivan]

@FraserP117
Thank you very much for this documentation and your hard work on getting these libraries and docs done.
I was wondering, although this visualization is amazing, I find it a bit strange to have individual observed data (or conditioned data) depicted as individual nodes in the graph; this kinds of flattens up the graph depiction and over-crowds it, and at the same time the fact that all these data are coming from a single source (or distribution/sensor) is masked. Is it a way to collapse the graph visualization in this dimension, as usually depicted in papers, etc. I feel like some kind of plate notation, or a "depth" to the graph is needed to make it more practical

[FraserP117]

Thanks @navidivan. That's exactly what we're working on! So no, unfortunately, you can't do this right now.

You're right, that is exactly the sort of thing that you would like to be able to do. Things like plate notation and generic summarisation of connected components; visualising only some induced connected component or collapsing one down into a single node... These are the sorts of "summary methods" that we're working on for the next phase.

We chose to visualize "absolutely everything" for the initial means of visualization. This was due to its relative simplicity and it obviously provides a comprehensive visualization - at the cost of increasing visual clutter in the size of the model. We will be adding various summary methods as soon as we can.

There is a document that contains our roadmap and many other details related to our efforts. It is currently a little out of date so I hesitate to share it here. I've been settling into a PhD program in the last 2 weeks so I've been very distracted. Will update ASAP.

Hope that helps!

[navidivan]

Ok. So I made this slightly more complicated example based on the notebook example provided; I am getting results that are not expected. Not sure where the go from here. I appreciate any inputs or insights about this.

Background:
The problem I am trying to solve is that we have an object we want to track, and we have 5 sensors that output distance estimates. Both the xy positions of the object and all the sensors might change and drift over time. I chose 5 to have slightly over-defined situation:
Unkowns: 2 (xo, yo) + 5 * 2 * (xs, ys) = 12
Equations: 5 (doi distance of object to sensor) + 10 (dij sensor to sensor distance (each sensor distance to each other sensor is also observed, 4+3+2+1 = 10) = 15

Here is the generative model:

using Distributions, LinearAlgebra

function interpolate_positions(positions, times, sample_times)
    interpolated = Vector{Vector{Float64}}(undef, length(sample_times))
    for t in 1:length(sample_times)
        t_current = sample_times[t]
        if t_current <= times[1]
            interpolated[t] = positions[1]
        elseif t_current >= times[end]
            interpolated[t] = positions[end]
        else
            for i in 1:(length(times)-1)
                if t_current >= times[i] && t_current <= times[i+1]
                    frac = (t_current - times[i]) / (times[i+1] - times[i])
                    interpolated[t] = (1 - frac) * positions[i] + frac * positions[i+1]
                    break
                end
            end
        end
    end
    return interpolated
end

function generate_sensor_data(movements, fixed_sensors;
    object_process_variance=0.01*Diagonal(ones(2)),
    moving_sensor_process_variance=0.01*Diagonal(ones(2)),
    fixed_sensor_process_variance=0.01*Diagonal(ones(2)),
    measurement_variance_dio=0.05,
    measurement_variance_dij=0.1,
    dt=0.01)
    
    # Time setup
    object_positions, object_times = movements["object"]
    total_time = object_times[end]
    sample_times = collect(0:dt:total_time)
    n_steps = length(sample_times)
    
    # Object: Interpolate mean positions and add process variance jitter
    object_mean_positions = interpolate_positions(object_positions, object_times, sample_times)
    true_object_positions = [mean_pos + rand(MvNormal(zeros(2), object_process_variance)) 
                             for mean_pos in object_mean_positions]
    
    # Sensors: Initialize true positions with jitter at each step
    true_sensor_positions = Dict{Int, Vector{Vector{Float64}}}()
    for i in 1:5
        if haskey(movements, i)
            # Moving sensor: Interpolate mean positions and add process variance jitter
            sensor_positions, sensor_times = movements[i]
            mean_positions = interpolate_positions(sensor_positions, sensor_times, sample_times)
            true_sensor_positions[i] = [mean_pos + rand(MvNormal(zeros(2), moving_sensor_process_variance)) 
                                        for mean_pos in mean_positions]
        else
            # Fixed sensor: Use constant mean position with process variance jitter at each step
            fixed_mean = fixed_sensors[i]
            true_sensor_positions[i] = [fixed_mean + rand(MvNormal(zeros(2), fixed_sensor_process_variance)) 
                                        for _ in 1:n_steps]
        end
    end
    
    # Calculate distances with measurement variance (sensor noise)
    dio = zeros(n_steps, 5)
    dij = zeros(n_steps, 10)
    for t in 1:n_steps
        z_t = true_object_positions[t]
        # Object-to-sensor distances with measurement noise
        for i in 1:5
            s_i = true_sensor_positions[i][t]
            true_dist = norm(z_t - s_i)
            dio[t, i] = true_dist + randn() * measurement_variance_dio
        end
        # Sensor-to-sensor distances with measurement noise
        pair_idx = 1
        for i in 1:5
            for j in (i+1):5
                s_i = true_sensor_positions[i][t]
                s_j = true_sensor_positions[j][t]
                true_dist = norm(s_i - s_j)
                dij[t, pair_idx] = true_dist + randn() * measurement_variance_dij
                pair_idx += 1
            end
        end
    end
    
    return dio, dij, true_object_positions, true_sensor_positions, sample_times
end

using Plots

function plot_sensor_system(movements, fixed_sensors;
    object_process_variance=0.01*Diagonal(ones(2)),
    moving_sensor_process_variance=0.01*Diagonal(ones(2)),
    fixed_sensor_process_variance=0.01*Diagonal(ones(2)),
    measurement_variance_dio=0.05,
    measurement_variance_dij=0.1,
    filename="sensor_animation_jitter.gif",
    dt=0.01)
    
    # Generate data
    dio, dij, true_object_positions, true_sensor_positions, sample_times = generate_sensor_data(
        movements, fixed_sensors;
        object_process_variance=object_process_variance,
        moving_sensor_process_variance=moving_sensor_process_variance,
        fixed_sensor_process_variance=fixed_sensor_process_variance,
        measurement_variance_dio=measurement_variance_dio,
        measurement_variance_dij=measurement_variance_dij,
        dt=dt
    )
    
    # Animation settings: 0.5-second steps
    anim_step = 0.5
    anim_times = 0:anim_step:sample_times[end]
    anim_indices = [findfirst(==(t), sample_times) for t in anim_times]
    
    # Create animation
    anim = @animate for idx in anim_indices
        if isnothing(idx)
            continue # Skip if time not found
        end
        # Current positions
        current_object = true_object_positions[idx]
        current_sensors = [true_sensor_positions[i][idx] for i in 1:5]
        
        # Plot all sensors as triangles
        p = scatter([s[1] for s in current_sensors], [s[2] for s in current_sensors],
                    markershape=:utriangle, markersize=10, label="Sensors", color=:blue,
                    xlim=(-7, 7), ylim=(-7, 7), xlabel="X [m]", ylabel="Y [m]",
                    title="System at t = $(round(sample_times[idx], digits=2))")
        
        # Object trajectory and current position
        plot!(p, [p[1] for p in true_object_positions[1:idx]], [p[2] for p in true_object_positions[1:idx]],
              label="Object Path", color=:red, linewidth=2)
        scatter!(p, [current_object[1]], [current_object[2]],
                 markershape=:circle, markersize=8, label="Object", color=:red)
        
        # Highlight moving sensors with stars
        for i in keys(movements)
            if i != "object"
                scatter!(p, [current_sensors[i][1]], [current_sensors[i][2]],
                         markershape=:star5, markersize=12, label="Moving Sensor $i", color=:green)
            end
        end
    end
    
    gif(anim, filename, fps=2) # 2 fps = 0.5 seconds per frame
end

the generative process can be plotted with this:

# Define movements
movements = Dict(
    "object" => ([[1.0, 0.5], [2.0, 1.0], [3.0, 1.5], [2.5, 2.0], [1.5, 1.8]], [0.0, 1.0, 2.0, 3.0, 4.0]),
    3 => ([[4.0, -3.0], [5.0, -6.0]], [2.0, 3.0])  # Sensor 3 moves from t=2 to t=3
)

# Fixed sensors
fixed_sensors = Dict(1 => [-.5, -.5], 2 => [-6.0, 3.0], 4 => [5.0, 5.0], 5 => [0.0, -5.0])

# Generate animation with explicit variances
plot_sensor_system(movements, fixed_sensors; 
    object_process_variance=0.01*Diagonal(ones(2)), 
    moving_sensor_process_variance=0.0001*Diagonal(ones(2)), 
    fixed_sensor_process_variance=0.0001*Diagonal(ones(2)), 
    measurement_variance_dio=0.01, 
    measurement_variance_dij=0.01)

this shows an object moving, while sensors also might move around. especially midway, the sensor number 3 drifts significantly.

[bvdmitri]

Could you try extracting H outside of the non-linearity?

so instead of

function compute_dio(x, s1, s2, s3, s4, s5)  # H defined internally
    H = [1.0 0.0 0.0 0.0;  # p_x
         0.0 1.0 0.0 0.0]  # p_y
    p = H * x  # Extract position from state
    return [norm(p - s1), norm(p - s2), norm(p - s3), norm(p - s4), norm(p - s5)]
end

do something like

function compute_dio(p, s1, s2, s3, s4, s5)  # p is "reduced" state
    return [norm(p - s1), norm(p - s2), norm(p - s3), norm(p - s4), norm(p - s5)]
end

and in the model do

p[t] := H * x[t]
dio[t] ~ MvNormalMeanCovariance(compute_dio(p[t], s1[t], s2[t], s3[t], s4[t], s5[t]), R_dio)

This might help with the stability, because H is not a full rank matrix, but explicitly defined * has specialized message passing rules that help with stability in such situations

[navidivan]

Thank you for the suggestion. I tried it, don't think it made a difference, but I implemented it anyways in the new code because it made sense.
When I was playing with the script, I could make the inference more stable by playing with the precisions and priors, and removed some of the initialization, but the free energy plot is now completely nonsensical. not sure if this means this is completely wrong or what is happening

using RxInfer, Plots, LinearAlgebra, StableRNGs, Optimisers, StatsPlots

# Compute functionssss
function compute_dio(p, s1, s2, s3, s4, s5)  # p is "reduced" state
    return [norm(p - s1), norm(p - s2), norm(p - s3), norm(p - s4), norm(p - s5)]
end

function compute_dij(s1, s2, s3, s4, s5)
    return [norm(s1 - s2), norm(s1 - s3), norm(s1 - s4), norm(s1 - s5),
            norm(s2 - s3), norm(s2 - s4), norm(s2 - s5 ),norm(s3 - s4),
            norm(s3 - s5), norm(s4 - s5)]
end

# Probabilistic model with object state including velocity
@model function sensor_fusion_model(dio, dij, A, Q, W_s, R_dio, R_dij, T)
    # State transition matrix (Δt = 0.01)
    Δt = 0.01
    A = [1.0 0.0 Δt  0.0;  # p_x = p_x + v_x * Δt
         0.0 1.0 0.0 Δt;   # p_y = p_y + v_y * Δt
         0.0 0.0 1.0 0.0;  # v_x = v_x
         0.0 0.0 0.0 1.0]  # v_y = v_y

    H = [1.0 0.0 0.0 0.0;  # p_x
        0.0 1.0 0.0 0.0]  # p_y
    
    # Process noise covariance
    Q = Diagonal([0.1, 0.1, 1, 1])  # [p_x, p_y, v_x, v_y]
    
    # Initial priors (broad uncertainty, zero initial velocity)
    x[1] ~ MvNormalMeanCovariance([0.0, 0.0, 0.0, 0.0], Q)
    s1[1] ~ MvNormalMeanCovariance([-10, -10], 1.0 * Diagonal(ones(2))) 
    s2[1] ~ MvNormalMeanCovariance([-6.0, 3.0], 1.0 * Diagonal(ones(2)))
    s3[1] ~ MvNormalMeanCovariance([4.0, -3.0], 1.0 * Diagonal(ones(2)))
    s4[1] ~ MvNormalMeanCovariance([5.0, 5.0], 1.0 * Diagonal(ones(2)))
    s5[1] ~ MvNormalMeanCovariance([0.0, -5.0], 1.0 * Diagonal(ones(2)))

    
    # Time evolution
    for t in 2:T
        x[t] ~ MvNormalMeanCovariance(A * x[t-1], Q)  # Object with velocity dynamics
        s1[t] ~ MvNormalMeanCovariance([-10, -10], 0.001 * Diagonal(ones(2))) 

        # s1[t] ~ MvNormalMeanPrecision(s1[t-1], W_s)   
        s2[t] ~ MvNormalMeanPrecision(s2[t-1], W_s)
        s3[t] ~ MvNormalMeanPrecision(s3[t-1], W_s)
        s4[t] ~ MvNormalMeanPrecision(s4[t-1], W_s)
        s5[t] ~ MvNormalMeanPrecision(s5[t-1], W_s)
    end
    
    # Observations
    for t in 1:T
        p[t] := H * x[t]
        dio[t] ~ MvNormalMeanCovariance(compute_dio(p[t], s1[t], s2[t], s3[t], s4[t], s5[t]), R_dio)
        dij[t] ~ MvNormalMeanCovariance(compute_dij(s1[t], s2[t], s3[t], s4[t], s5[t]), R_dij)
    end
end

# Constraints
# constraints = @constraints begin
#     q(x, s1, s2, s3, s4, s5) = q(x)q(s1)q(s2)q(s3)q(s4)q(s5)
# #     # q(x, s1, s2, s3, s4, s5) = q(x,)q(s1,s2,s3,s4,s5)

# end

# Meta specification with Unscented Transform
@meta function sensor_fusion_meta()
    compute_dio(p, s1, s2, s3, s4, s5) -> Unscented(50)
    compute_dij(s1, s2, s3, s4, s5) -> Unscented(50)

    # # compute_dio(x, s1, s2, s3, s4, s5) -> CVI(StableRNG(42), 20, 20, Optimisers.Descent(0.1), ForwardDiffGrad(), 1, Val(false), false)
    # # compute_dij(s1, s2, s3, s4, s5) -> CVI(StableRNG(42), 20, 20, Optimisers.Descent(0.1), ForwardDiffGrad(), 1, Val(false), false)
    # compute_dio(x, s1, s2, s3, s4, s5) -> LaplaceApproximation()
    # compute_dij(s1, s2, s3, s4, s5) -> LaplaceApproximation()
end

# Define movements and fixed sensors for generative process
movements = Dict(
    "object" => ([[1.0, 0.5], [2.0, 1.0], [3.0, 1.5], [2.5, 2.0], [1.5, 1.8]], [0.0, 1.0, 1.5, 3.5, 7.0]),
    3 => ([[4.0, -3.0], [5.0, -6.0]], [2.0, 3.0])
)
fixed_sensors = Dict(1 => [-10, -10], 2 => [-6.0, 3.0], 4 => [5.0, 5.0], 5 => [0.0, -5.0] )

# Generate data
dio, dij, true_object_positions, true_sensor_positions, sample_times = generate_sensor_data(
    movements, fixed_sensors;
    object_process_variance=0.001*Diagonal(ones(2)),
    moving_sensor_process_variance=0.001*Diagonal(ones(2)),
    fixed_sensor_process_variance=0.001*Diagonal(ones(2)),
    measurement_variance_dio= .0001,
    measurement_variance_dij= .0001
)

# Number of time steps
T = length(sample_times)

# Initialize messages
z_init_messages = [MvNormalMeanPrecision([0.0, 0.0, 0.0, 0.0] .+ [t * 0.01, t * 0.01, 0.0, 0.0], Diagonal(ones(4))) for t in 1:T]
s_init_messages = [
    [MvNormalMeanPrecision([-10, -10], Diagonal(ones(2))) for t in 1:T],  # s1
    [MvNormalMeanPrecision([-6.0, 3.0], Diagonal(ones(2))) for t in 1:T],   # s2
    [MvNormalMeanPrecision([4.0, -3.0], Diagonal(ones(2))) for t in 1:T],   # s3
    [MvNormalMeanPrecision([5.0, 5.0], Diagonal(ones(2))) for t in 1:T],     # s4
    [MvNormalMeanPrecision([0.0, -5.0], Diagonal(ones(2))) for t in 1:T]     # s5
]

# Initialization
init = @initialization begin
    μ(x) = z_init_messages
    μ(s1) = s_init_messages[1]
    μ(s2) = s_init_messages[2]
    μ(s3) = s_init_messages[3]
    μ(s4) = s_init_messages[4]
    μ(s5) = s_init_messages[5]
    # q(x) = [MvNormalMeanCovariance([0.0, 0.0, 0.0, 0.0], Diagonal([1.0, 1.0, 0.1, 0.1])) for _ in 1:T]
    # q(s1) = [MvNormalMeanCovariance([-10, -10], 1.0*Diagonal(ones(2))) for _ in 1:T]
    # q(s2) = [MvNormalMeanCovariance([-6.0, 3.0], 1.0*Diagonal(ones(2))) for _ in 1:T]
    # q(s3) = [MvNormalMeanCovariance([4.0, -3.0], 1.0*Diagonal(ones(2))) for _ in 1:T]
    # q(s4) = [MvNormalMeanCovariance([5.0, 5.0], 1.0*Diagonal(ones(2))) for _ in 1:T]
    # q(s5) = [MvNormalMeanCovariance([0.0, -5.0], 1.0*Diagonal(ones(2))) for _ in 1:T]

end

# Run inference
results = infer(
    model = sensor_fusion_model(A=nothing, Q=nothing, W_s = Diagonal(ones(2)), 
                              R_dio = 0.01 * Diagonal(ones(5)), R_dij = 0.01 * Diagonal(ones(10)), T=T),
    meta = sensor_fusion_meta(),
    data = (dio = [dio[t, :] for t in 1:T], dij = [dij[t, :] for t in 1:T]),
    # constraints = constraints,
    initialization = init,
    iterations = 100,
    free_energy = true,
    returnvars = (x = KeepLast(), s1 = KeepLast(), s2 = KeepLast(), s3 = KeepLast(), s4 = KeepLast() , s5 = KeepLast()),
    showprogress = true,
    options = (limit_stack_depth = 1000,)
)

# Plotting function
function plot_inference_results(true_object_positions, true_sensor_positions, sample_times, x_posteriors, s_posteriors; filename="inference_results.gif")
    # Animation settings: 0.25-second steps
    anim_step = 0.25
    anim_times = 0:anim_step:sample_times[end]
    anim_indices = [findfirst(==(t), sample_times) for t in anim_times]
    
    # Observation matrix to extract position
    H = [1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0]
    
    # Create animation
    anim = @animate for idx in anim_indices
        if isnothing(idx)
            continue
        end
        p = scatter([true_sensor_positions[i][idx][1] for i in 1:5], [true_sensor_positions[i][idx][2] for i in 1:5], 
                    markershape=:utriangle, markersize=10, label="True Sensors", color=:blue,
                    xlim=(-11, 7), ylim=(-11, 7), xlabel="X [m]", ylabel="Y [m]",
                    title="Inference at t = $(round(sample_times[idx], digits=2))")
        
        # True object path
        plot!(p, [p[1] for p in true_object_positions[1:idx]], [p[2] for p in true_object_positions[1:idx]], 
              label="True Object", color=:red, linewidth=2, linestyle=:dash)
        
        # Estimated object position
        mean_x = mean(x_posteriors[idx])
        cov_x = cov(x_posteriors[idx])
        mean_p = H * mean_x
        cov_p = H * cov_x * H'  # Project covariance to position space
        covellipse!(p, mean_p, cov_p, color="green", label="", n_std=2)
        scatter!(p, [mean_p[1]], [mean_p[2]], color="green", label="Est. Object")
        
        # Estimated sensors
        for i in 1:5
            covellipse!(p, mean(s_posteriors[Symbol("s$i")][idx]), cov(s_posteriors[Symbol("s$i")][idx]), 
                        color="purple", label="", n_std=2)
            scatter!(p, [mean(s_posteriors[Symbol("s$i")][idx])[1]], [mean(s_posteriors[Symbol("s$i")][idx])[2]], 
                    color="purple", label=i == 1 ? "Est. Sensors" : "")
        end
    end
    
    gif(anim, filename, fps=4)
end

# Plot results
plot_inference_results(true_object_positions, true_sensor_positions, sample_times, 
                      results.posteriors[:x], results.posteriors)

Free energy:

[bvdmitri]

This free energy plot issue has been a long-standing bug that we’ve been wanting to fix for a while, but it’s somewhat difficult to reproduce. Your example provides a nice reproducer.

The problem arises because, during free energy computation, the indices for individual and joint marginals can sometimes get out of sync. This misalignment causes errors in the difference calculations between entropies, leading to an incorrect plot where the free energy appears to increase—even though nothing problematic is actually happening. Moreover, the computed free energy value itself isn’t even being used during the inference.

As long as you observe convergence (e.g., abs.(diff(free_energy)) approaching zero), the results should be fine.

[bvdmitri]

this "out of sync" situation with Free Energy before has been linked to initializing messages, here it also the case, so its a bug in RxInfer, but I would just ignore the fact that it increases (as long as it converges)

[navidivan]

that's awesome! Thank you