Refine tests

.github/workflows/ci.yml

@@ -15,7 +15,7 @@ jobs:
       fail-fast: false
       matrix:
         version:
-          - '1.4' # Replace this with the minimum Julia version that your package supports. E.g. if your package requires Julia 1.5 or higher, change this to '1.5'.
+          - '1.6' # Replace this with the minimum Julia version that your package supports. E.g. if your package requires Julia 1.5 or higher, change this to '1.5'.
           - '1' # Leave this line unchanged. '1' will automatically expand to the latest stable 1.x release of Julia.
           - 'nightly'
         os:

Project.toml

@@ -6,6 +6,7 @@ version = "0.1.1"
 [deps]
 Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
+FFTW = "7a1cc6ca-52ef-59f5-83cd-3a7055c09341"
 LazyArrays = "5078a376-72f3-5289-bfd5-ec5146d43c02"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
@@ -14,7 +15,7 @@ Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
 Distributions = "0.20, 0.21, 0.22, 0.23, 0.24, 0.25"
 DocStringExtensions = "0.6, 0.7, 0.8"
 LazyArrays = "0.13, 0.14, 0.15, 0.16, 0.17, 0.18, 0.19, 0.20, 0.21"
-julia = "1.4"
+julia = "1.6"
 
 [extras]
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"

src/KalmanFilters.jl

@@ -5,7 +5,8 @@ module KalmanFilters
         Distributions,
         LinearAlgebra,
         LazyArrays,
-        Statistics
+        Statistics,
+        FFTW
 
     const liblapack = Base.liblapack_name
 
@@ -46,9 +47,10 @@ module KalmanFilters
         measurement_update,
         measurement_update!,
         calc_nis,
-        nis_test,
-        sigma_bound_test,
-        two_sigma_bound_test,
+        calc_nis_test,
+        calc_sigma_bound_test,
+        calc_two_sigma_bound_test,
+        innovation_correlation_test,
         ConsideredState,
         ConsideredCovariance,
         ConsideredMeasurementModel,

src/kf.jl

@@ -3,6 +3,40 @@ struct KFTimeUpdate{X,P} <: AbstractTimeUpdate{X,P}
     covariance::P
 end
 
+struct KFMeasurementUpdate{X,P,R,S,K} <: AbstractMeasurementUpdate{X,P}
+    state::X
+    covariance::P
+    innovation::R
+    innovation_covariance::S
+    kalman_gain::K
+end
+
+function time_update(x, P, F::Union{Number, AbstractMatrix}, Q)
+    x_apri = calc_apriori_state(x, F)
+    P_apri = calc_apriori_covariance(P, F, Q)
+    KFTimeUpdate(x_apri, P_apri)
+end
+
+function measurement_update(x, P, y, H::Union{Number, AbstractVector, AbstractMatrix}, R; consider = nothing)
+    ỹ = calc_innovation(H, x, y)
+    PHᵀ = calc_P_xy(P, H)
+    S = calc_innovation_covariance(H, P, R)
+    K = calc_kalman_gain(PHᵀ, S, consider)
+    x_post = calc_posterior_state(x, K, ỹ, consider)
+    P_post = calc_posterior_covariance(P, PHᵀ, K, consider)
+    KFMeasurementUpdate(x_post, P_post, ỹ, S, K)
+end
+
+calc_apriori_state(x, F) = F * x
+calc_apriori_covariance(P, F, Q) = F * P * F' + Q
+
+calc_P_xy(P, H) = P * H'
+calc_innovation(H, x, y) = y - H * x
+calc_innovation_covariance(H, P, R) = H * P * H' + R
+calc_kalman_gain(PHᵀ, S, consider::Nothing) = PHᵀ / S
+calc_posterior_state(x, K, ỹ, consider::Nothing) = x + K * ỹ
+calc_posterior_covariance(P, PHᵀ, K, consider::Nothing) = P - PHᵀ * K' # (I - K * H) * P ?
+
 struct KFTUIntermediate{T}
     x_apri::Vector{T}
     p_apri::Matrix{T}
@@ -18,14 +52,6 @@ KFTUIntermediate(T::Type, num_x::Number) =
 
 KFTUIntermediate(num_x::Number) = KFTUIntermediate(Float64, num_x)
 
-struct KFMeasurementUpdate{X,P,R,S,K} <: AbstractMeasurementUpdate{X,P}
-    state::X
-    covariance::P
-    innovation::R
-    innovation_covariance::S
-    kalman_gain::K
-end
-
 struct KFMUIntermediate{T,K<:Union{<:AbstractVector{T},<:AbstractMatrix{T}}}
     innovation::Vector{T}
     innovation_covariance::Matrix{T}
@@ -50,38 +76,6 @@ end
 
 KFMUIntermediate(num_x::Number, num_y::Number) = KFMUIntermediate(Float64, num_x, num_y)
 
-calc_apriori_state(x, F) = F * x
-calc_apriori_covariance(P, F, Q) = F * P * F' .+ Q
-
-function time_update(x, P, F::Union{Number, AbstractMatrix}, Q)
-    x_apri = calc_apriori_state(x, F)
-    P_apri = calc_apriori_covariance(P, F, Q)
-    KFTimeUpdate(x_apri, P_apri)
-end
-
-function time_update!(tu::KFTUIntermediate, x, P, F::AbstractMatrix, Q)
-    x_apri = calc_apriori_state!(tu.x_apri, x, F)
-    P_apri = calc_apriori_covariance!(tu.p_apri, tu.fp, P, F, Q)
-    KFTimeUpdate(x_apri, P_apri)
-end
-
-function measurement_update(x, P, y, H::Union{Number, AbstractVector, AbstractMatrix}, R; consider = nothing)
-    ỹ = calc_innovation(H, x, y)
-    PHᵀ = calc_P_xy(P, H)
-    S = calc_innovation_covariance(H, P, R)
-    K = calc_kalman_gain(PHᵀ, S, consider)
-    x_post = calc_posterior_state(x, K, ỹ, consider)
-    P_post = calc_posterior_covariance(P, PHᵀ, K, consider)
-    KFMeasurementUpdate(x_post, P_post, ỹ, S, K)
-end
-
-@inline calc_P_xy(P, H) = P * H'
-@inline calc_innovation(H, x, y) = y .- H * x
-@inline calc_innovation_covariance(H, P, R) = H * P * H' .+ R
-@inline calc_kalman_gain(PHᵀ, S, consider::Nothing) = PHᵀ / S
-@inline calc_posterior_state(x, K, ỹ, consider::Nothing) = x .+ K * ỹ
-@inline calc_posterior_covariance(P, PHᵀ, K, consider::Nothing) = P .- PHᵀ * K' # (I - K * H) * P ?
-
 function measurement_update!(mu::KFMUIntermediate, x, P, y, H::AbstractMatrix, R)
     ỹ = calc_innovation!(mu.innovation, H, x, y)
     PHᵀ = calc_P_xy!(mu.pht, P, H)
@@ -92,6 +86,12 @@ function measurement_update!(mu::KFMUIntermediate, x, P, y, H::AbstractMatrix, R
     KFMeasurementUpdate(x_post, P_post, ỹ, S, K)
 end
 
+function time_update!(tu::KFTUIntermediate, x, P, F::AbstractMatrix, Q)
+    x_apri = calc_apriori_state!(tu.x_apri, x, F)
+    P_apri = calc_apriori_covariance!(tu.p_apri, tu.fp, P, F, Q)
+    KFTimeUpdate(x_apri, P_apri)
+end
+
 function calc_P_xy!(PHᵀ, P, H)
     mul!(PHᵀ, P, H')
     PHᵀ

src/tests.jl

@@ -1,38 +1,94 @@
 """
 $(SIGNATURES)
 
-Average number of sigma bound exceedings
-
-Returns the average number of unbiased states / innovation values that exceed the ⨦σ bound of the given covariance
+Tests if the absolute value of the states exceed the standard deviation.
 """
-function mean_num_sigma_bound_exceedings(state_over_time::Vector{Vector{T}}, covariance_over_time::Vector{Matrix{T}}) where T
-    mean(map((x, P) -> abs.(x) .> sqrt.(diag(P)), state_over_time, covariance_over_time))
+function do_state_errors_exceed_stds(
+    state_errors::T,
+    variances::T
+) where T <: AbstractArray
+    does_state_error_exceed_std.(state_errors, variances)
 end
 
-function mean_num_sigma_bound_exceedings(state_over_time::Vector{T}, covariance_over_time::Vector{T}) where T
-    mean(map((x, P) ->  abs(x) > sqrt(P), state_over_time, covariance_over_time))
+"""
+$(SIGNATURES)
+
+Tests if the absolute value of the state exceed the variance
+"""
+function does_state_error_exceed_std(state_error, variance)
+    abs(state_error) > sqrt(variance)
 end
 
 """
 $(SIGNATURES)
 
 Innovation magnitude bound test (σ-bound-test)
 
-Tests if approximately 68% of state values lie within the ⨦σ bound
+Tests if approximately 68% of state values lie within the ⨦σ bound. The parameter
+dims must be the dimenstion of Monte Carlo runs or the time dimension.
 """
-function sigma_bound_test(state_over_time, covariance_over_time)
-    isapprox.(mean_num_sigma_bound_exceedings(state_over_time, covariance_over_time), .32, atol = .015)
+function calc_sigma_bound_test(
+    state_errors::AbstractMatrix{T},
+    variances::AbstractMatrix{T};
+    dims = 1,
+    atol = 0.06
+) where T
+    mean_state_errors_exceed_variance = vec(mean(
+        do_state_errors_exceed_stds(state_errors, variances),
+        dims = dims
+    ))
+    exceed_probability = 1 - (cdf(Normal(), 1) - cdf(Normal(), -1))
+    isapprox.(mean_state_errors_exceed_variance, exceed_probability; atol)
+end
+
+function calc_sigma_bound_test(
+    state_errors::AbstractVector{T},
+    variances::AbstractVector{T};
+    dims = 1,
+    atol = 0.06
+) where T
+    only(calc_sigma_bound_test(
+        reshape(state_errors, length(state_errors), 1),
+        reshape(variances, length(state_errors), 1);
+        dims,
+        atol
+    ))
 end
 
 """
 $(SIGNATURES)
 
 Innovation magnitude bound test (2σ-bound-test)
 
-Tests if approximately 95% of state values lie within the ⨦2σ bound
+Tests if approximately 95% of state values lie within the ⨦2σ bound. The parameter
+    dims must be the dimenstion of Monte Carlo runs or the time dimension.
 """
-function two_sigma_bound_test(state_over_time, covariance_over_time)
-    isapprox.(mean_num_sigma_bound_exceedings(state_over_time, 4 .* covariance_over_time), .05, atol = .008)
+function calc_two_sigma_bound_test(
+    state_errors::AbstractMatrix{T},
+    variances::AbstractMatrix{T};
+    dims = 1,
+    atol = 0.05
+) where T
+    mean_state_errors_exceed_4x_variance = vec(mean(
+        do_state_errors_exceed_stds(state_errors, 4 * variances),
+        dims = dims
+    ))
+    exceed_probability = 1 - (cdf(Normal(), 2) - cdf(Normal(), -2))
+    isapprox.(mean_state_errors_exceed_4x_variance, exceed_probability; atol)
+end
+
+function calc_two_sigma_bound_test(
+    state_errors::AbstractVector{T},
+    variances::AbstractVector{T};
+    dims = 1,
+    atol = 0.05
+) where T
+    only(calc_two_sigma_bound_test(
+        reshape(state_errors, length(state_errors), 1),
+        reshape(variances, length(state_errors), 1);
+        dims,
+        atol
+    ))
 end
 
 """
@@ -45,27 +101,45 @@ Double-tailed siginicance test with false alarm probability α = 0.05
 Calculates confidence interval [r1 r2] and tests Prob{ ∑ NIS values)} ∈ [r1 r2] ∣ H_0 ) = 1 - α
 with Hypothesis H_0: N * ∑ NIS values ∼ χ^2_{dof}
      dof (degree of freedom): N * m (N: window length, m: dimension of state vector)
+see https://cs.adelaide.edu.au/~ianr/Teaching/Estimation/LectureNotes2.pdf
 """
-function nis_test(nis_over_time, dof)
-    sum_of_nis = sum(nis_over_time)
+function calc_nis_test(nis_values::AbstractVector; num_measurements = 1)
+    degrees_of_freedom = num_measurements * length(nis_values)
+    sum_of_nis_vals = sum(nis_values)
 
-    r1 = cquantile(Chisq(dof), .975)
-    r2 = cquantile(Chisq(dof), .025)
+    r1 = cquantile(Chisq(degrees_of_freedom), .975)
+    r2 = cquantile(Chisq(degrees_of_freedom), .025)
 
-    (sum_of_nis >= r1) && (sum_of_nis <= r2)
+    r1 <= sum_of_nis_vals <= r2
 end
 
 """
 $(SIGNATURES)
 
 Normalized innovation squared (NIS)
 
-Returns NIS-value for a single innovation sequence `seq` and its variance `var`
+Returns NIS-value 
 """
-function calc_nis(seq, var)
-    dot(seq, var \ seq)
+function calc_nis(innovation, innovation_covariance)
+    real(dot(innovation, innovation_covariance \ innovation))
 end
 
 function calc_nis(mu::AbstractMeasurementUpdate)
-    calc_nis(get_innovation(mu), mu.innovation_covariance)
+    calc_nis(get_innovation(mu), get_innovation_covariance(mu))
+end
+
+"""
+$(SIGNATURES)
+
+Auto correlation test
+
+"""
+function innovation_correlation_test(innovations::AbstractMatrix)
+    correlations = abs.(ifft(fft(innovations) .* conj(fft(innovations))))
+    scaled_correlations = correlations ./ correlations[1]
+    maximum.(eachcol(scaled_correlations[2:end,:])) .< 0.1 # Less than 10% correlation
+end
+
+function innovation_correlation_test(innovations::AbstractVector)
+    only(innovation_correlation_test(reshape(innovations, length(innovations), 1)))
 end

test/runtests.jl

@@ -1,4 +1,4 @@
-using Test, KalmanFilters, Random, LinearAlgebra, LazyArrays
+using Test, KalmanFilters, Random, LinearAlgebra, LazyArrays, Statistics, FFTW
 
 Random.seed!(1234)