diff --git a/docs/src/measurement_models.md b/docs/src/measurement_models.md index 7cbfd85..f0e0524 100644 --- a/docs/src/measurement_models.md +++ b/docs/src/measurement_models.md @@ -4,15 +4,15 @@ The Kalman-type filters - [`ExtendedKalmanFilter`](@ref) - [`UnscentedKalmanFilter`](@ref) -each come with their own built-in measurement model, e.g., the standard [`KalmanFilter`](@ref) uses the linear measurement model ``y = Cx + Du + e``, while the [`ExtendedKalmanFilter`](@ref) and [`UnscentedKalmanFilter`](@ref) use the nonlinear measurement model ``y = h(x,u,p,t) + e`` or ``y = h(x,u,p,t,e)``. The [`ExtendedKalmanFilter`](@ref) uses linearization to approximate the nonlinear measurement model, while the [`UnscentedKalmanFilter`](@ref) uses the unscented transform to approximate the nonlinear measurement model. +each come with their own built-in measurement model, e.g., the standard [`KalmanFilter`](@ref) uses the linear measurement model ``y = Cx + Du + e``, while the [`ExtendedKalmanFilter`](@ref) and [`UnscentedKalmanFilter`](@ref) 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`](@ref) uses linearization to approximate the nonlinear measurement model, while the [`UnscentedKalmanFilter`](@ref) 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, but the linear measurement model from the standard [`KalmanFilter`](@ref) for the measurement update ([`correct!`](@ref)) if the measurement model is linear. +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!`](@ref)), but the linear measurement model from the standard [`KalmanFilter`](@ref) for the measurement update ([`correct!`](@ref)) 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`](@ref) performs linear propagation of covariance (as is done in [`KalmanFilter`](@ref)) -- [`EKFMeasurementModel`](@ref) uses linearization to propagate covariance (as is done in [`ExtendedKalmanFilter`](@ref)) -- [`UKFMeasurementModel`](@ref) uses the unscented transform to propagate covariance (as is done in [`UnscentedKalmanFilter`](@ref)) - +- [`LinearMeasurementModel`](@ref) performs linear propagation of covariance (as is done in [`KalmanFilter`](@ref)). +- [`EKFMeasurementModel`](@ref) uses linearization to propagate covariance (as is done in [`ExtendedKalmanFilter`](@ref)). +- [`UKFMeasurementModel`](@ref) uses the unscented transform to propagate covariance (as is done in [`UnscentedKalmanFilter`](@ref)). +- [`CompositeMeasurementModel`](@ref) combines multiple measurement models. ## Constructing a filter with a custom measurement model @@ -27,13 +27,13 @@ ny = 90 # Dimension of measurements const __A = 0.1*randn(nx, nx) const __B = randn(nx, nu) const __C = randn(ny,nx) -function dynamics_large_ip(dx,x,u,p,t) +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_large_ip(y,x,u,p,t) +function measurement_ip(y,x,u,p,t) # __C*x mul!(y, __C, x) nothing @@ -43,46 +43,46 @@ R1 = I(nx) R2 = I(ny) mm_kf = LinearMeasurementModel(__C, 0, R2; nx, ny) -ukf = UnscentedKalmanFilter(dynamics_large_ip, mm_kf, R1; ny, nu) +ukf = UnscentedKalmanFilter(dynamics_ip, mm_kf, R1; ny, nu) ``` 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 one or several measurement models -Above we constructed a filter with a custom measurement model, we can also pass a custom measurement model when we call `correct!`. Tis may be useful when performing sensor fusion with sensors operating at different rates, or when parts of the measurement model are linear, and other parts are nonlinear. +## 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. ```@example MEASUREMENT_MODELS using LowLevelParticleFilters, LinearAlgebra -nx = 100 # Dimension of state -nu = 2 # Dimension of input -ny = 90 # Dimension of measurements +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_large_ip(dx,x,u,p,t) +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_large_ip(y,x,u,p,t) +function measurement_ip(y,x,u,p,t) # __C*x mul!(y, __C, x) nothing end -R1 = I(nx) +R1 = I(nx) # Covariance matrices R2 = I(ny) # Construct three different filters -kf = KalmanFilter(__A, __B, __C, 0, R1, R2) -ukf = UnscentedKalmanFilter(dynamics_large_ip, measurement_large_ip, R1, R2; ny, nu) -ekf = ExtendedKalmanFilter(dynamics_large_ip, measurement_large_ip, R1, R2; nu) +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 @@ -91,8 +91,8 @@ x,u,y = LowLevelParticleFilters.simulate(kf, U) # Simulate trajectory using the # Construct three different measurement models mm_kf = LinearMeasurementModel(__C, 0, R2; nx, ny) -mm_ekf = EKFMeasurementModel{Float64, true}(measurement_large_ip, R2; nx, ny) -mm_ukf = UKFMeasurementModel{Float64, true, false}(measurement_large_ip, 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] @@ -111,5 +111,3 @@ using Test @test kf.x ≈ ekf.x ≈ ukf.x @test kf.R ≈ ekf.R ≈ ukf.R ``` - -### Sensor-fusion example \ No newline at end of file diff --git a/src/ukf.jl b/src/ukf.jl index 15c49e4..fe5579a 100644 --- a/src/ukf.jl +++ b/src/ukf.jl @@ -316,9 +316,9 @@ function safe_cov(xs::Vector{<:SVector}, m = safe_mean(xs)) end """ - correct!(ukf::UnscentedKalmanFilter{IPD, IPM, AUGD, AUGM}, u, y, p = parameters(ukf), t::Real = index(ukf) * ukf.Ts; R2 = get_mat(ukf.R2, ukf.x, u, p, t), mean, cross_cov, innovation, measurement) + correct!(ukf::UnscentedKalmanFilter{IPD, IPM, AUGD, AUGM}, u, y, p = parameters(ukf), t::Real = index(ukf) * ukf.Ts; R2 = get_mat(ukf.R2, ukf.x, u, p, t), mean, cross_cov, innovation) -The correction step for an [`UnscentedKalmanFilter`](@ref) allows the user to override, `R2`, `mean`, `cross_cov`, `innovation`, `measurement`. +The correction step for an [`UnscentedKalmanFilter`](@ref) allows the user to override, `R2`, `mean`, `cross_cov`, `innovation`. # Arguments: - `u`: The input @@ -329,7 +329,6 @@ The correction step for an [`UnscentedKalmanFilter`](@ref) allows the user to ov - `mean`: The function that computes the mean of the output sigma points. - `cross_cov`: The function that computes the cross-covariance of the state and output sigma points. - `innovation`: The function that computes the innovation between the measured output and the predicted output. -- `measurement`: The measurement function. # Extended help To perform separate measurement updates for different sensors, see the ["Measurement models" in the documentation](@ref measurement_models)