Measurement models

The Kalman-type filters

each come with their own built-in measurement model, e.g., the standard KalmanFilter uses the linear measurement model $y = Cx + Du + e$, while the ExtendedKalmanFilter and UnscentedKalmanFilter use the nonlinear measurement model $y = h(x,u,p,t) + e$ or $y = h(x,u,p,t,e)$. For covariance propagation, the ExtendedKalmanFilter uses linearization to approximate the nonlinear measurement model, while the UnscentedKalmanFilter uses the unscented transform.

It is sometimes useful to mix and match dynamics and measurement models. For example, using the unscented transform from the UKF for the dynamics update (predict!), but the linear measurement model from the standard KalmanFilter for the measurement update (correct!) if the measurement model is linear.

This is possible by constructing a filter with an explicitly created measurement model. The available measurement models are

LinearMeasurementModel performs linear propagation of covariance (as is done in KalmanFilter).
EKFMeasurementModel uses linearization to propagate covariance (as is done in ExtendedKalmanFilter).
UKFMeasurementModel uses the unscented transform to propagate covariance (as is done in UnscentedKalmanFilter).
CompositeMeasurementModel combines multiple measurement models.

Constructing a filter with a custom measurement model

Constructing a Kalman-type filter automatically creates a measurement model of the corresponding type, given the functions/matrices passed to the filter constructor. To construct a filter with a non-standard measurement model, e.g., and UKF with a KF measurement model, manually create the desired measurement model and pass it as the second argument to the constructor. For example, to construct an UKF with a linear measurement model, we do

using LowLevelParticleFilters, LinearAlgebra
nx = 100    # Dimension of state
nu = 2      # Dimension of input
ny = 90     # Dimension of measurements

# Define linear state-space system
const __A = 0.1*randn(nx, nx)
const __B = randn(nx, nu)
const __C = randn(ny,nx)
function dynamics_ip(dx,x,u,p,t)
    # __A*x .+ __B*u
    mul!(dx, __A, x)
    mul!(dx, __B, u, 1.0, 1.0)
    nothing
end
function measurement_ip(y,x,u,p,t)
    # __C*x
    mul!(y, __C, x)
    nothing
end

R1 = I(nx)
R2 = I(ny)

mm_kf = LinearMeasurementModel(__C, 0, R2; nx, ny)
ukf = UnscentedKalmanFilter(dynamics_ip, mm_kf, R1; ny, nu)

UnscentedKalmanFilter{true, false, false, false, typeof(Main.dynamics_ip), LinearMeasurementModel{Matrix{Float64}, Int64, LinearAlgebra.Diagonal{Bool, Vector{Bool}}, Nothing}, LinearAlgebra.Diagonal{Bool, Vector{Bool}}, LowLevelParticleFilters.SimpleMvNormal{Vector{Float64}, LinearAlgebra.Diagonal{Bool, Vector{Bool}}}, LowLevelParticleFilters.SigmaPointCache{Vector{Vector{Float64}}, Vector{Vector{Float64}}}, Vector{Float64}, Matrix{Float64}, LowLevelParticleFilters.NullParameters, Nothing, typeof(LowLevelParticleFilters.safe_mean), typeof(LowLevelParticleFilters.safe_cov), typeof(LinearAlgebra.cholesky!)}(Main.dynamics_ip, LinearMeasurementModel{Matrix{Float64}, Int64, LinearAlgebra.Diagonal{Bool, Vector{Bool}}, Nothing}([0.45563868255969786 1.2767221563269395 … 0.5664561128334656 0.633487302084747; -1.0309093438610097 -0.8365799692760448 … -1.2792629123847703 -0.2654701159186893; … ; -0.7528181076204443 0.7769090438716332 … 0.4549191393234413 -0.6703097540313754; 0.8326613494926483 -1.4716719378245975 … 1.1337257013525612 1.0103431316009601], 0, Bool[1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], 90, nothing), Bool[1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], LowLevelParticleFilters.SimpleMvNormal{Vector{Float64}, LinearAlgebra.Diagonal{Bool, Vector{Bool}}}([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Bool[1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1]), LowLevelParticleFilters.SigmaPointCache{Vector{Vector{Float64}}, Vector{Vector{Float64}}}([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  …  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  …  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; … ; 0.0 0.0 … 1.0 0.0; 0.0 0.0 … 0.0 1.0], 0, 1.0, 90, 2, LowLevelParticleFilters.NullParameters(), nothing, LowLevelParticleFilters.safe_mean, LowLevelParticleFilters.safe_cov, LinearAlgebra.cholesky!)

When we create the filter with the custom measurement model, we do not pass the arguments that are associated with the measurement model to the filter constructor, i.e., we do not pass any measurement function, and not the measurement covariance matrix $R_2$.

Sensor fusion: Using several different measurement models

Above we constructed a filter with a custom measurement model, we can also pass a custom measurement model when we call correct!. This may be useful when, e.g., performing sensor fusion with sensors operating at different sample rates, or when parts of the measurement model are linear, and other parts are nonlinear.

The following example instantiates three different filters and three different measurement models. Each filter is updated with each measurement model, demonstrating that any combination of filter and measurement model can be used together.

using LowLevelParticleFilters, LinearAlgebra
nx = 10    # Dimension of state
nu = 2     # Dimension of input
ny = 9     # Dimension of measurements

# Define linear state-space system
const __A = 0.1*randn(nx, nx)
const __B = randn(nx, nu)
const __C = randn(ny,nx)
function dynamics_ip(dx,x,u,p,t)
    # __A*x .+ __B*u
    mul!(dx, __A, x)
    mul!(dx, __B, u, 1.0, 1.0)
    nothing
end
function measurement_ip(y,x,u,p,t)
    # __C*x
    mul!(y, __C, x)
    nothing
end

R1 = I(nx) # Covariance matrices
R2 = I(ny)

# Construct three different filters
kf  = KalmanFilter(__A, __B, __C, 0, R1, R2)
ukf = UnscentedKalmanFilter(dynamics_ip, measurement_ip, R1, R2; ny, nu)
ekf = ExtendedKalmanFilter(dynamics_ip, measurement_ip, R1, R2; nu)

# Simulate some data
T    = 200 # Number of time steps
U = [randn(nu) for _ in 1:T]
x,u,y = LowLevelParticleFilters.simulate(kf, U) # Simulate trajectory using the model in the filter

# Construct three different measurement models
mm_kf = LinearMeasurementModel(__C, 0, R2; nx, ny)
mm_ekf = EKFMeasurementModel{Float64, true}(measurement_ip, R2; nx, ny)
mm_ukf = UKFMeasurementModel{Float64, true, false}(measurement_ip, R2; nx, ny)


mms = [mm_kf, mm_ekf, mm_ukf]
filters = [kf, ekf, ukf]

for mm in mms, filter in filters
    @info "Updating $(nameof(typeof(filter))) with measurement model $(nameof(typeof(mm)))"
    correct!(filter, mm, u[1], y[1]) # Pass the measurement model as the second argument to the correct! function if not using the measurement model built into the filter
end

WARNING: redefinition of constant Main.__A. This may fail, cause incorrect answers, or produce other errors.
WARNING: redefinition of constant Main.__B. This may fail, cause incorrect answers, or produce other errors.
WARNING: redefinition of constant Main.__C. This may fail, cause incorrect answers, or produce other errors.
[ Info: Updating KalmanFilter with measurement model LinearMeasurementModel
[ Info: Updating ExtendedKalmanFilter with measurement model LinearMeasurementModel
[ Info: Updating UnscentedKalmanFilter with measurement model LinearMeasurementModel
[ Info: Updating KalmanFilter with measurement model EKFMeasurementModel
[ Info: Updating ExtendedKalmanFilter with measurement model EKFMeasurementModel
[ Info: Updating UnscentedKalmanFilter with measurement model EKFMeasurementModel
[ Info: Updating KalmanFilter with measurement model UKFMeasurementModel
[ Info: Updating ExtendedKalmanFilter with measurement model UKFMeasurementModel
[ Info: Updating UnscentedKalmanFilter with measurement model UKFMeasurementModel

Since the dynamics in this particular example is in fact linear, we should get identical results for all three filters.

using Test
@test kf.x ≈ ekf.x ≈ ukf.x
@test kf.R ≈ ekf.R ≈ ukf.R

Test Passed

Video tutorial

A video demonstrating the use of multiple measurement models in a sensor-fusion context is available on YouTube: