Merge pull request #43 from sisl/innovation_covariance

Adding measure_with_innov_cov.
sisl · Nov 27, 2024 · 64983be · 64983be
2 parents d264bd4 + bf76709
commit 64983be
Show file tree

Hide file tree

Showing 10 changed files with 169 additions and 65 deletions.
diff --git a/Project.toml b/Project.toml
@@ -11,14 +11,16 @@ Logging = "56ddb016-857b-54e1-b83d-db4d58db5568"
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 
 [compat]
-julia = "1"
 Distributions = "0.16, 0.17, 0.18, 0.19, 0.20, 0.24, 0.25"
 ForwardDiff = "0.9, 0.10"
+StableRNGs = "1.0.2"
+julia = "1"
 
 [extras]
 NBInclude = "0db19996-df87-5ea3-a455-e3a50d440464"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
+StableRNGs = "860ef19b-820b-49d6-a774-d7a799459cd3"
 
 [targets]
-test = ["NBInclude", "Plots", "Test"]
+test = ["NBInclude", "Plots", "Test", "StableRNGs"]
diff --git a/src/GaussianFilters.jl b/src/GaussianFilters.jl
@@ -37,7 +37,10 @@ export
 
 include("simulate.jl")
 
-export	update
+export	
+	update,
+	update_with_info
+
 include("kf.jl")
 include("ekf.jl")
 

diff --git a/src/ekf.jl b/src/ekf.jl
@@ -40,3 +40,22 @@ function measure(filter::ExtendedKalmanFilter, bp::GaussianBelief, y::AbstractVe
     Σn = (I - K * H) * bp.Σ
     return GaussianBelief(μn, Σn)
 end
+
+function measure_info(filter::ExtendedKalmanFilter, bp::GaussianBelief, y::AbstractVector{a};
+    u::AbstractVector{b} = [false]) where {a<:Number, b<:Number}
+
+    # Measurement update
+    yp = measure(filter.o, bp.μ, u)
+    H = jacobian(filter.o, bp.μ, u)
+
+    # Kalman Gain
+    Σ_Y = H * bp.Σ * H' + filter.o.V
+    K = bp.Σ * H' * inv(Σ_Y)
+
+    # Measurement update
+    μn = bp.μ + K * (y - yp)
+    Σn = (I - K * H) * bp.Σ
+
+    info = (innovation_cov = ΣY, kalman_gain = K, predicted_measurement = yp)
+    return GaussianBelief(μn, Σn), info
+end
diff --git a/src/kf.jl b/src/kf.jl
@@ -19,6 +19,18 @@ function update(filter::AbstractFilter, b0::GaussianBelief,
     return bn
 end
 
+function update_with_info(filter::AbstractFilter, b0::GaussianBelief,
+    u::AbstractVector{<:Number}, y::AbstractVector{<:Number})
+
+    # predict
+    bp = predict(filter, b0, u)
+
+    # measure
+    bn, info = measure_info(filter, bp, y; u = u)
+
+    return bn, info
+end
+
 # Kalman filter functions
 
 """
@@ -46,16 +58,24 @@ then matrix D will be factored into the y predictions
 """
 function measure(filter::KalmanFilter, bp::GaussianBelief, y::AbstractVector{<:Number};
                 u::AbstractVector{<:Number} = [false])
+    return first(measure_info(filter, bp, y; u = u))
+end
+
+function measure_info(filter::KalmanFilter, bp::GaussianBelief, y::AbstractVector{<:Number};
+    u::AbstractVector{<:Number} = [false])
 
     # Kalman Gain
-    K = bp.Σ * filter.o.C' *
-        inv(filter.o.C * bp.Σ * filter.o.C' + filter.o.V)
+    ΣY = filter.o.C * bp.Σ * filter.o.C' + filter.o.V
+    K = bp.Σ * filter.o.C' * inv(ΣY)
 
     # Predicted measurement
     yp = measure(filter.o, bp.μ, u)
 
     # Measurement update
     μn = bp.μ + K * (y-yp)
     Σn = (I - K * filter.o.C) * bp.Σ
-    return GaussianBelief(μn, Σn)
+
+    info = (innovation_cov = ΣY, kalman_gain = K, predicted_measurement = yp)
+
+    return GaussianBelief(μn, Σn), info
 end
diff --git a/src/ukf.jl b/src/ukf.jl
@@ -140,3 +140,38 @@ function measure(filter::UnscentedKalmanFilter, bp::GaussianBelief, y::AbstractV
     Σn = bp.Σ - K * Σ_XY'
     return GaussianBelief(μn, Σn)
 end
+
+
+function measure_info(filter::UnscentedKalmanFilter, bp::GaussianBelief, y::AbstractVector{<:Number};
+    u::AbstractVector{<:Number} = [false])
+
+    # Measurement update
+
+    # approximate Gaussian belief with sigma points
+    points, w_μ, w_Σ = unscented_transform(bp, filter.λ, filter.α, filter.β)
+
+    # iterate over sigma points and computed expected measurement
+    ysp = [measure(filter.o, point, u) for point in points]
+
+
+    catpoints = reduce(hcat, ysp)
+    # compute expected measurement
+    yp = catpoints * w_μ
+
+    # compute marginal covariance components
+    ydiff = catpoints .- yp
+    scaled_ydiff = ydiff .* sqrt.(w_Σ)'
+    Σ_Y = scaled_ydiff * scaled_ydiff' + filter.o.V
+
+    xdiff = reduce(hcat, points) .- bp.μ
+    scaled_xdiff = xdiff .* sqrt.(w_Σ)'
+    Σ_XY = scaled_xdiff * scaled_ydiff'
+
+    # measurement update
+    K = Σ_XY / Σ_Y
+    μn = bp.μ + K * (y - yp)
+    Σn = bp.Σ - K * Σ_XY'
+
+    info = (innovation_cov = Σ_Y, kalman_gain = K, predicted_measurement = yp)
+    return GaussianBelief(μn, Σn), info
+end
diff --git a/test/runtests.jl b/test/runtests.jl
@@ -5,6 +5,7 @@ using Logging
 using LinearAlgebra
 using Distributions
 using NBInclude
+using StableRNGs
 
 # Package Under Test
 using GaussianFilters
@@ -16,7 +17,7 @@ global_logger(SimpleLogger(stderr, Logging.Debug))
 Random.seed!(0)
 
 # Check equality of two arrays
-@inline function array_isapprox(x::AbstractArray{F},
+function array_isapprox(x::AbstractArray{F},
                   y::AbstractArray{F};
                   rtol::F=sqrt(eps(F)),
                   atol::F=zero(F)) where {F<:AbstractFloat}
@@ -32,7 +33,7 @@ Random.seed!(0)
 end
 
 # Check if array equals a single value
-@inline function array_isapprox(x::AbstractArray{F},
+function array_isapprox(x::AbstractArray{F},
                   y::F;
                   rtol::F=sqrt(eps(F)),
                   atol::F=zero(F)) where {F<:AbstractFloat}
@@ -51,21 +52,20 @@ end
         include(joinpath(testdir, "test_models.jl"))
     end
 
-    @time @testset "GaussianFilter GM-PHD Testing" begin
-        include(joinpath(testdir, "test_gmphd.jl"))
-    end
-
     @time @testset "GaussianFilter KF Testing" begin
         include(joinpath(testdir, "test_kf.jl"))
     end
 
     @time @testset "GaussianFilter EKF Testing" begin
         include(joinpath(testdir, "test_ekf.jl"))
     end
-
+    
     @time @testset "GaussianFilter UKF Testing" begin
         include(joinpath(testdir, "test_ukf.jl"))
     end
+    @time @testset "GaussianFilter GM-PHD Testing" begin
+        include(joinpath(testdir, "test_gmphd.jl"))
+    end
 
     @testset "Notebooks testing" begin 
         nbdir = joinpath(dirname(@__DIR__), "notebooks")
@@ -78,5 +78,4 @@ end
             end
         end
     end
-
 end
diff --git a/test/test_ekf.jl b/test/test_ekf.jl
@@ -1,4 +1,4 @@
-Random.seed!(0)
+rng = StableRNG(0)
 
 # Dynamics
 dt = 0.001
@@ -37,7 +37,7 @@ action_sequence = [[0.0,0.0,0.0] for t in times]
 
 b0 = GaussianBelief([10.0,0.0,0.0], Matrix{Float64}(I,3,3))
 
-sim_states, sim_measurements = simulation(ekf,b0,action_sequence)
+sim_states, sim_measurements = simulation(ekf,b0,action_sequence, rng)
 
 # Filter
 filtered_beliefs = run_filter(ekf, b0, action_sequence, sim_measurements)
@@ -61,12 +61,21 @@ array_isapprox(first_update.Σ, first_predict_measure.Σ)
 @test size(μ) == (length(filtered_beliefs), length(filtered_beliefs[1].μ))
 @test size(Σ) == (length(filtered_beliefs), size(filtered_beliefs[1].Σ)...)
 
-# change with algorithm / rng changes
-
-array_isapprox(sim_states[end], [10.7664, 0.213008, -0.463593];atol=0.001)
-array_isapprox(sim_measurements[end], [10.9978, 0.491584, -0.33985];atol=0.001)
-array_isapprox(filtered_beliefs[end].μ, [10.051, -0.0285158, -0.575077];
+array_isapprox(sim_states[end], [10.392777087830043, 1.881278839651635, 1.3840241662693935]; atol=0.001)
+array_isapprox(sim_measurements[end], [10.247812301812102, 2.3133675336888575, 1.1394960504116656]; atol=0.001)
+array_isapprox(filtered_beliefs[end].μ, [10.11546527880089, 1.9759609213772842, 1.2802257669046886];
                 atol=0.001)
-array_isapprox(filtered_beliefs[end].Σ, [0.329097 -0.00281178 0.000218104;
-                                        -0.00281178 0.0168616 -1.87166e-6;
-                                        0.000218104 -1.87166e-6 0.0168371]; atol=0.001)
+array_isapprox(filtered_beliefs[end].Σ, [0.22360753709112413 0.003704458778907191 -0.0053481184628957346;
+                                        0.003704458778907191 0.01690085173611888 -9.213406017856073e-5;
+                                        -0.0053481184628957346 -9.213406017856073e-5 0.016969928487616054]; atol=0.001)
+
+@test issymmetric(filtered_beliefs[end].Σ)  
+@test isposdef(filtered_beliefs[end].Σ) 
+
+_, info = update_with_info(ekf, b0, action_sequence[1], sim_measurements[1])
+@test haskey(info, :innovation_cov)
+@test haskey(info, :kalman_gain)
+@test haskey(info, :predicted_measurement)
+@test size(info.innovation_cov) == (3,3)
+@test size(info.kalman_gain) == (3,3) 
+@test length(info.predicted_measurement) == 3
diff --git a/test/test_gmphd.jl b/test/test_gmphd.jl
@@ -1,15 +1,15 @@
 # Arbitrary GM-PHD example
 let
-    Random.seed!(0) # reset seed
+    rng = StableRNG(0)
 
-    x0 = GaussianMixture([1.0],[randn(2)],[Matrix(1.0I,2,2)])
-    Z = [randn(1)]
+    x0 = GaussianMixture([1.0],[randn(rng, 2)],[Matrix(1.0I,2,2)])
+    Z = [randn(rng, 1)]
     k(z) = 0
-    gamma = GaussianMixture([1.0],[randn(2)],[Matrix(1.0I,2,2)])
+    gamma = GaussianMixture([1.0],[randn(rng, 2)],[Matrix(1.0I,2,2)])
     sp_dyn = [Dynamics(Matrix(1.0I,2,2),Matrix(1.0I,2,2),[0.0, 0.0])]
-    sp = Spawn(GaussianMixture([1.0],[randn(2)],[Matrix(1.0I,2,2)]), sp_dyn)
+    sp = Spawn(GaussianMixture([1.0],[randn(rng, 2)],[Matrix(1.0I,2,2)]), sp_dyn)
     dyn = Dynamics(Matrix(1.0I,2,2),Matrix(1.0I,2,2))
-    mea = Measurement(randn(1,2),ones(1,1))
+    mea = Measurement(randn(rng, 1,2),ones(1,1))
 
     # Should construct identically
     phd1 = PHDFilter(gamma, sp, dyn, mea, 0.5, 0.5, k)
@@ -22,7 +22,7 @@ let
     xp = predict(phd1, x0)
     s = measure(phd1, xp, Z)
     @test s.N == 6
-    array_isapprox(s.w, [0.5, 0.5, 0.25, 0.425012, 0.383326, 0.191663], atol=0.002)
+    array_isapprox(s.w, [0.5, 0.5, 0.25, 0.19792404040719125, 0.5347173063952059, 0.26735865319760294], atol=0.002)
     @test length(s.Σ) == s.N
     @test length(s.μ) == s.N
 
@@ -31,8 +31,8 @@ let
     U = 0.1 # Merging threshold
     J_max = 10; # Max number of Gaussian terms
     p = prune(s, T, U, J_max)
-    @test p.N == 2
-    array_isapprox(p.w, [0.925012, 1.32499], atol=0.002)
+    @test p.N == 4
+    array_isapprox(p.w, [0.8020759595928089, 0.5, 0.75, 0.19792404040719125], atol=0.002)
     @test length(p.μ) == p.N
 
     # Test single step updating
@@ -41,18 +41,17 @@ let
     @test sp.μ == p.μ
 
     # Test state extraction at different thresholds
-    states1 = multiple_target_state_extraction(sp, 0.8)
+    states1 = multiple_target_state_extraction(sp, 0.5)
     @test length(states1) == 2
-    array_isapprox(states1[1], [-0.132261, 0.598847], atol=0.002)
-
+    array_isapprox(states1[1], [-0.16045023924680923, 0.4344660539101717], atol=0.002)
     states2 = multiple_target_state_extraction(sp, 2.0)
     @test length(states2) == 0
+    array_isapprox(states1[2], [-0.3811118617867437, -0.020452508120591215], atol=0.002)
 end
 
 # Surveillance example
 let
-    Random.seed!(0)
-
+    rng = StableRNG(0)
     # Dynamics
     Δ = 1 # Sampling Period
     σ_p = 5
@@ -109,11 +108,11 @@ let
     Tf = 20
     for t = 1 : Δ : Tf
         MVD = MvNormal(Dyns.Q)
-        x_new = [  Dyns.A*xsim[t][i] + rand(MVD,1)[:] for i=1:length(xsim[t]) ]
+        x_new = [  Dyns.A*xsim[t][i] + rand(rng, MVD,1)[:] for i=1:length(xsim[t]) ]
         if t == 13
             MVDspawn = MvNormal(Q_β)
             x1 = xsim[t][1]
-            x_spawn = Dyns.A*x1 + rand(MVDspawn,1)[:]
+            x_spawn = Dyns.A*x1 + rand(rng, MVDspawn,1)[:]
             push!(x_new,x_spawn)
         end
         if t == 8
@@ -128,19 +127,21 @@ let
     x =  [γ]
     for t = 1:Δ:Tf
         MVD = MvNormal(Meas.R)
-        z = [ Meas.C*xsim[t][i] + rand(MVD,1)[:] for i=1:length(xsim[t])]
+        z = [ Meas.C*xsim[t][i] + rand(rng, MVD,1)[:] for i=1:length(xsim[t])]
         x_new_pruned = update(phd,x[t],z,T,U,J_max)
         push!(x, x_new_pruned)
     end
 
     sf = x[end]
     @test sf.N == 6
-    array_isapprox(sf.w, [0.999298, 0.983239, 0.95002,
-                        1.03055, 0.0204876, 0.000549536], atol=0.002)
+    array_isapprox(sf.w, [0.9859331369043042, 0.974981461116766, 0.9417729637426844,
+                          0.9135806098534265, 0.020487604998975638, 0.020487604998975638], atol=0.002)
     @test length(sf.Σ) == 6
 
     esf = multiple_target_state_extraction(sf,0.5)
     @test length(esf) == 4
-    array_isapprox(esf[1], [-282.13, -724.875, -1.9644, -46.7133], atol=0.002)
-    array_isapprox(esf[4], [-313.933, -301.799, -3.09298, -1.9735], atol=0.002)
+    array_isapprox(esf[1], [-252.02649932803052, -359.93362714587613, 4.6283940976004185, 8.39795059665333], atol=0.002)
+    array_isapprox(esf[2], [-259.95843400206553, -222.24691047655602, -3.2305683889212893, 1.5813642133068648], atol=0.002)
+    array_isapprox(esf[3], [37.31131019093644, 308.138415366749, -25.376170638745595, -0.49215530411660485], atol=0.002)
+    array_isapprox(esf[4], [-287.2383000244064, -489.18789830748494, -14.57194769862216, -10.073401647546325], atol=0.002)
 end
diff --git a/test/test_kf.jl b/test/test_kf.jl
@@ -1,4 +1,4 @@
-Random.seed!(0)
+rng = StableRNG(0)
 
 # Dynamics
 dt = 0.1
@@ -25,7 +25,7 @@ times = 0:dt:10
 Fmag = 1000
 action_sequence = [[Fmag*cos(t), Fmag*sin(t)] for t in times]
 
-sim_states, sim_measurements = simulation(kf, b0,action_sequence);
+sim_states, sim_measurements = simulation(kf, b0,action_sequence, rng);
 
 # Filter
 filtered_beliefs = run_filter(kf, b0, action_sequence, sim_measurements)
@@ -47,12 +47,20 @@ array_isapprox(first_update.Σ, first_predict_measure.Σ)
 @test size(μ) == (length(filtered_beliefs), length(filtered_beliefs[1].μ))
 @test size(Σ) == (length(filtered_beliefs), size(filtered_beliefs[1].Σ)...)
 
-# change with algorithm / rng changes
-array_isapprox(sim_states[end], [59.9223, -7.17527, 178.149, 30.4653];atol=0.001)
-array_isapprox(sim_measurements[end], [-7.01552, 30.4259];atol=0.001)
-array_isapprox(filtered_beliefs[end].μ, [60.1627, -7.13376, 183.227, 30.3895];
+array_isapprox(sim_states[end], [98.92569494648434, -3.644139152469229, 247.30163635010385, 44.08105949503985]; atol=0.001)
+array_isapprox(sim_measurements[end], [-3.9598235926013814, 43.571078921455715]; atol=0.001)
+array_isapprox(filtered_beliefs[end].μ, [95.45866414957369, -4.222755472734632, 240.6036938459223, 44.10233946193025];
                 atol=0.001)
-array_isapprox(filtered_beliefs[end].Σ,   [12.6069 0.0320871 0.0 0.0;
-                                            0.0320871 0.179129 0.0 0.0;
-                                            0.0 0.0 12.6069 0.0320871;
-                                            0.0 0.0 0.0320871 0.179129]; atol=0.001)
+array_isapprox(filtered_beliefs[end].Σ,   
+[12.60691909451178 0.03208712152522081 0.0 0.0; 0.03208712152522081 0.17912878474779198 0.0 0.0; 0.0 0.0 12.60691909451178 0.03208712152522081; 0.0 0.0 0.03208712152522081 0.17912878474779198]; atol=0.001)
+
+@test issymmetric(filtered_beliefs[end].Σ)  
+@test isposdef(filtered_beliefs[end].Σ) 
+
+_, info = update_with_info(kf, b0, action_sequence[1], sim_measurements[1])
+@test haskey(info, :innovation_cov)
+@test haskey(info, :kalman_gain) 
+@test haskey(info, :predicted_measurement)
+@test size(info.innovation_cov) == (2,2)
+@test size(info.kalman_gain) == (4,2)
+@test length(info.predicted_measurement) == 2