diff --git a/Project.toml b/Project.toml index 7f82ec5..224c00f 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "LowLevelParticleFilters" uuid = "d9d29d28-c116-5dba-9239-57a5fe23875b" authors = ["baggepinnen "] -version = "3.10.1" +version = "3.11.0" [deps] DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab" diff --git a/src/ekf.jl b/src/ekf.jl index 49cb2ac..ba7acc9 100644 --- a/src/ekf.jl +++ b/src/ekf.jl @@ -28,7 +28,7 @@ See also [`UnscentedKalmanFilter`](@ref) which is typically more accurate than ` """ ExtendedKalmanFilter -function ExtendedKalmanFilter(dynamics, measurement, R1,R2,d0=SimpleMvNormal(Matrix(R1)); nu::Int, Ts = 1.0, p = NullParameters(), α = 1.0, check = true, Ajac = nothing, Cjac = nothing) +function ExtendedKalmanFilter(dynamics, measurement, R1,R2,d0=SimpleMvNormal(Matrix(R1)); nu::Int, ny=nothing, Ts = 1.0, p = NullParameters(), α = 1.0, check = true, Ajac = nothing, Cjac = nothing) nx = size(R1,1) ny = size(R2,1) T = eltype(R1) diff --git a/src/solutions.jl b/src/solutions.jl index 95d541d..b7213df 100644 --- a/src/solutions.jl +++ b/src/solutions.jl @@ -4,19 +4,29 @@ abstract type AbstractFilteringSolution end KalmanFilteringSolution{Tx,Txt,TR,TRt,Tll} <: AbstractFilteringSolution # Fields -- `x`: predictions ``x(t+1|t)`` -- `xt`: filtered estimates ``x(t|t)`` -- `R`: predicted covariance matrices ``R(t+1|t)`` -- `Rt`: filter covariances ``R(t|t)`` +- `x`: predictions ``x(t+1|t)`` (plotted if `plotx=true`) +- `xt`: filtered estimates ``x(t|t)`` (plotted if `plotxt=true`) +- `R`: predicted covariance matrices ``R(t+1|t)`` (plotted if `plotR=true`) +- `Rt`: filter covariances ``R(t|t)`` (plotted if `plotRt=true`) - `ll`: loglikelihood -- `e`: prediction errors +- `e`: prediction errors ``e(t|t-1) = y - ŷ(t|t-1)`` (plotted if `plote=true`) # Plot The solution object can be plotted ``` -plot(sol, plotx=true, plotxt=true, plotu=true, ploty=true, plotyh=true, plotyht=false, name="") +plot(sol, plotx=true, plotxt=true, plotR=true, plotRt=true, plote=true, plotu=true, ploty=true, plotyh=true, plotyht=true, name="") ``` -where `plotx`, `plotxt`, `plotu`, `ploty`, `plotyh`, `plotyht` are booleans that control which plots are shown. `name` is a string that is prepended to the labels of the plots, which is useful when plotting multiple solutions in the same plot. +where +- `plotx`: Plot the predictions `x(t|t-1)` +- `plotxt`: Plot the filtered estimates `x(t|t)` +- `plotR`: Plot the predicted covariances `R(t|t-1)` as ribbons at ±2σ (1.96 σ to be precise) +- `plotRt`: Plot the filter covariances `R(t|t)` as ribbons at ±2σ (1.96 σ to be precise) +- `plote`: Plot the prediction errors `e(t|t-1) = y - ŷ(t|t-1)` +- `plotu`: Plot the input +- `ploty`: Plot the measurements +- `plotyh`: Plot the predicted measurements `ŷ(t|t-1)` +- `plotyht`: Plot the filtered measurements `ŷ(t|t)` +- `name`: a string that is prepended to the labels of the plots, which is useful when plotting multiple solutions in the same plot. """ struct KalmanFilteringSolution{F,Tu,Ty,Tx,Txt,TR,TRt,Tll,Te} <: AbstractFilteringSolution f::F @@ -30,20 +40,38 @@ struct KalmanFilteringSolution{F,Tu,Ty,Tx,Txt,TR,TRt,Tll,Te} <: AbstractFilterin e::Te end -@recipe function plot(timevec::AbstractVector{<:Real}, sol::KalmanFilteringSolution; plotx = true, plotxt=true, plotu=true, ploty=true, plotyh=true, plotyht=false, plote=false, name = "") +@recipe function plot(timevec::AbstractVector{<:Real}, sol::KalmanFilteringSolution; plotx = true, plotxt=true, plotu=true, ploty=true, plotyh=true, plotyht=false, plote=false, plotR=false, plotRt=false, name = "") isempty(name) || (name = name*" ") kf = sol.f nx, nu, ny = length(sol.x[1]), length(sol.u[1]), length(sol.y[1]) layout --> nx*(plotx || plotxt) + plotu*nu + (ploty || plotyh || plotyht || plote)*ny - plotx && @series begin - label --> ["$(name)x$(i)(t|t-1)" for i in 1:nx] |> permutedims - subplot --> (1:nx)' - timevec, reduce(hcat, sol.x)' + if plotx + m = reduce(hcat, sol.x)' + twoσ = 1.96 .* sqrt.(reduce(hcat, diag.(sol.R))') + for i = 1:nx + @series begin + label --> "$(name)x$(i)(t|t-1)" + subplot --> i + if plotR + ribbon := twoσ[:,i] + end + timevec, m[:,i] + end + end end - plotxt && @series begin - label --> ["$(name)x$(i)(t|t)" for i in 1:nx] |> permutedims - subplot --> (1:nx)' - timevec, reduce(hcat, sol.xt)' + if plotxt + m = reduce(hcat, sol.xt)' + twoσ = 1.96 .* sqrt.(reduce(hcat, diag.(sol.Rt))') + for i = 1:nx + @series begin + label --> "$(name)x$(i)(t|t)" + subplot --> i + if plotRt + ribbon := twoσ[:,i] + end + timevec, m[:,i] + end + end end plotu && nu > 0 && @series begin label --> ["u$(i)" for i in 1:nu] |> permutedims diff --git a/src/ukf.jl b/src/ukf.jl index e91517c..5045445 100644 --- a/src/ukf.jl +++ b/src/ukf.jl @@ -91,13 +91,13 @@ The one exception where this will not work is when calling `simulate`, which ass # Augmented UKF If the noise is not additive, one may use the augmented form of the UKF. In this form, the dynamics functions take additional input arguments that correspond to the noise terms. To enable this form, the typed constructor ``` -UnscentedKalmanFilter{inplace_dynamics,inplace_measurement,augmented_dynamics,augmnented_measurement}(...) +UnscentedKalmanFilter{inplace_dynamics,inplace_measurement,augmented_dynamics,augmented_measurement}(...) ``` is used, where the Boolean type parameters have the following meaning - `inplace_dynamics`: If `true`, the dynamics function operates in-place, i.e., it modifies the first argument in `dynamics(dx, x, u, p, t)`. Default is `false`. - `inplace_measurement`: If `true`, the measurement function operates in-place, i.e., it modifies the first argument in `measurement(y, x, u, p, t)`. Default is `false`. - `augmented_dynamics`: If `true` the dynamics function is augmented with an additional noise input `w`, i.e., `dynamics(x, u, p, t, w)`. Default is `false`. -- `augmnented_measurement`: If `true` the measurement function is agumented with an additional noise input `e`, i.e., `measurement(x, u, p, t, e)`. Default is `false`. +- `augmented_measurement`: If `true` the measurement function is agumented with an additional noise input `e`, i.e., `measurement(x, u, p, t, e)`. Default is `false`. Use of augmented dynamics incurs extra computational cost. The number of sigma points used is `2L+1` where `L` is the length of the augmented state vector. Without augmentation, `L = nx`, with augmentation `L = nx + nw` and `L = nx + ne` for dynamics and measurement, respectively. diff --git a/test/test_large.jl b/test/test_large.jl index a97d709..989876b 100644 --- a/test/test_large.jl +++ b/test/test_large.jl @@ -98,6 +98,6 @@ a = @allocated forward_trajectory(ekf, u, y) ## Plotting ==================================================================== using Plots plot(sol_kf, plothy = true, plote = true) -plot(sol_ukf, plothy = true, plote = true) -plot(sol_ekf, plothy = true, plote = true) +plot(sol_ukf, plothy = true, plote = true, plotR=true) +plot(sol_ekf, plothy = true, plote = true, plotRt=true) plot(sol_sqkf, plothy = true, plote = true) \ No newline at end of file