Merge pull request #164 from baggepinnen/plotR

add ability to plot covariance in Kalman filter solution plot

Project.toml

@@ -1,7 +1,7 @@
 name = "LowLevelParticleFilters"
 uuid = "d9d29d28-c116-5dba-9239-57a5fe23875b"
 authors = ["baggepinnen <[email protected]>"]
-version = "3.10.1"
+version = "3.11.0"
 
 [deps]
 DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab"

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)

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

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.
 

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)