LowLevelParticleFiltersMTK

A helper package for state-estimation workflows using LowLevelParticleFilters.jl with ModelingToolkit models.

Installation

The package is registered, you can install it using: julia import Pkg; Pkg.add("LowLevelParticleFiltersMTK")

Challenges with performing state estimation with ModelingToolkit models

Consider a discrete-time dynamical system for which we want to perform state estimation: math \begin{aligned} x(t+1) &= f(x(t), u(t), p, t, w(t))\\ y(t) &= g(x(t), u(t), p, t, e(t)) \end{aligned}

Getting a ModelingToolkit model into this form requires several steps that are non trivial, such as generating the dynamics and measurement functions, $f$ and $g$, on the form required by the filter and discretizing a continuous-time model.

Workflows involving ModelingToolkit also demand symbolic indexing rather than indexing with integers, this need arises due to the fact that the state realization for the system is chosen by MTK rather than by the user, and this realization may change between different versions of MTK. One cannot normally specify the required initial state distribution and the dynamics noise distribution without having knowledge of the state realization. To work around this issue, this package requires the user to explicitly model how the disturbance inputs $w$ are affecting the dynamics, such that the realization of the dynamics noise becomes independent on the chosen state realization. This results in a dynamical model where the dynamics disturbance $w$ is an input to the model math \dot{x} = f(x, u, p, t, w) Some state estimators handle this kind of dynamics natively, like the UnscentedKalmanFilter with AUGD = true, while others, like the ExtendedKalmanFilter require manipulation of this model to work. This package handles such manipulation automatically, e.g., by continuously linearizing $f$ w.r.t. $w$ to obtain $Bw(t)$ and providing the ExtendedKalmanFilter with the time-varying dynamics covariance matrix $R1(x, u, p, t) = Bw(t) Rw B_w(t)^T$.

Finally, this package provides symbolic indexing of the solution object, such that one can easily obtain the estimated posterior distribution over any arbitrary variable in the model, including "observed" variables that are not part of the state vector being estimated by the estimator.

Workflow

[!TIP] It is assumed that the reader is familiar with the basics of LowLevelParticleFilters.jl. Consult the documentation and the video lectures liked therein to obtain such familiarity.

The workflow can be summarized as follows 1. Define a model using ModelingToolkit 2. Create an instance of prob = StateEstimationProblem(...). This problem contains the model as well as specifications of inputs, outputs, disturbance inputs, noise probability distributions and discretization method. 3. Instantiate a state estimator using filt = get_filter(prob, FilterConstructor). This calls the filter constructor with the appropriate dynamics functions depending on what type of filter is used. 4. Perform state estimation using the filter object as you would normally do with LowLevelParticleFilters.jl. Obtain a fsol::KalmanFilteringSolution object, either from calling LowLevelParticleFilters.forward_trajectory or by creating one manually after having performed custom filtering. 5. Wrap the fsol object in a sol = StateEstimationSolution(fsol, prob) object. This will provide symbolic indexing capabilities similar to how solution objects work in ModelingToolkit. 6. Analyze the solution object using, e.g., sol[var], plot(sol), plot(sol, idxs=[var1, var2]) etc. 7. Profit from your newly derived insight.

As you can see, the workflow is similar to the standard MTK workflow, but contains a few more manual steps, notably the instantiation of the filter in step 3. and the manual wrapping of the solution object in step 5. The design is made this way since state estimation does not fit neatly into a problem->solve framework, in particular, one may have measurements arriving at irregular intervals, partial measurements, custom modifications of the covariance of the estimator etc. For simple cases where batch filtering (offline) is applicable, the function LowLevelParticleFilters.forward_trajectory produces the required KalmanFilteringSolution object that can be wrapped in a StateEstimationSolution object. Situations that demand more flexibility instead require the user to manually construct this solution object, in which case inspecting the implementation of LowLevelParticleFilters.forward_trajectory and modifying it to suit your needs is a good starting point. An example of this is demonstrated in the tutorial fault detection.

Example

The example below demonstrates a complete workflow, annotating the code with comments to point out things that are perhaps non-obvious. ```julia using LowLevelParticleFiltersMTK using LowLevelParticleFilters using LowLevelParticleFilters: SimpleMvNormal using ModelingToolkit using SeeToDee # used to discretize the dynamics using Plots using StaticArrays using LinearAlgebra

t = ModelingToolkit.tnounits D = ModelingToolkit.Dnounits

@mtkmodel SimpleSys begin @variables begin x(t) = 2.0 u(t) = 0 y(t) w(t), [disturbance = true, input = true] end @equations begin D(x) ~ -x + u + w # Explicitly encode where dynamics noise enters the system with w y ~ x end end

@named model = SimpleSys() # Do not use @mtkbuild here cmodel = complete(model) # complete is required for variable indexing since we did not use @mtkbuild above inputs = [cmodel.u] # The (unbound) inputs to the system outputs = [cmodel.y] # The outputs for which we obtain measurements disturbance_inputs = [cmodel.w] # The dynamics disturbance inputs to the system

nu = length(inputs) # Number of inputs nw = length(disturbance_inputs) # Number of disturbance inputs ny = length(outputs) # Number of measured outputs R1 = SMatrix{nw,nw}(0.01I(nw)) # Dynamics noise covariance R2 = SMatrix{ny,ny}(0.1I(ny)) # Measurement noise covariance

df = SimpleMvNormal(R1) # Dynamics noise distribution. This has to be a Gaussian if using a Kalman-type filter dg = SimpleMvNormal(R2) # Measurement noise distribution. This has to be a Gaussian if using a Kalman-type filter

Ts = 0.1 # Sampling interval discretization = function (f,Ts,xinds,alginds,nu) isempty(alg_inds) || error("Rk4 only handles differential equations, consider Trapezoidal instead") SeeToDee.Rk4(f, Ts) # Discretization method end

prob = StateEstimationProblem(model, inputs, outputs; disturbance_inputs, df, dg, discretization, Ts)

We instantiate two different filters for comparison

ekf = getfilter(prob, ExtendedKalmanFilter) ukf = getfilter(prob, UnscentedKalmanFilter)

Simulate some data from the trajectory distribution implied by the model

u = [randn(nu) for _ in 1:30] # A random input sequence x,u,y = simulate(ekf, u, dynamicsnoise=true, measurementnoise=true)

Perform the filtering in batch since the entire input-output sequence is available

fsole = forwardtrajectory(ekf, u, y) fsolu = forwardtrajectory(ukf, u, y)

Wrap the filter solution objects in a StateEstimationSolution object

sole = StateEstimationSolution(fsole, prob) solu = StateEstimationSolution(fsolu, prob)

We can access the solution to any variable in the model easily

sole[cmodel.x] == sole[cmodel.y]

We can also obtain the solution as a trajectory of probability distributions

sole[cmodel.x, dist=true]

We can plot the filter solution object using the plot recipe from LowLevelParticleFilters

using Plots plot(fsole, size=(1000, 1000)) plot!(fsole.t, reduce(hcat, x)', lab="True x")

plot(fsolu, size=(1000, 1000)) plot!(fsolu.t, reduce(hcat, x)', lab="True x")

We can also plot the wrapped solution object

plot(sole) plot!(solu)

The wrapped solution object allows for symbolic indexing,

note how we can easily plot the posterior distribution over y^2 + 0.1*sin(u)

plot(sole, idxs=cmodel.y^2 + 0.1sin(cmodel.u)) plot!(solu, idxs=cmodel.y^2 + 0.1sin(cmodel.u)) ```

API

The following is a summary of the exported functions, followed by their docstrings

Summary

• StateEstimationProblem: A structure representing a state-estimation problem.
• StateEstimationSolution: A solution object that provides symbolic indexing to a KalmanFilteringSolution object.
• get_filter: Instantiate a filter from a state-estimation problem.
• propagate_distribution: Propagate a probability distribution dist through a nonlinear function f using the covariance-propagation method of filter kf.

StateEstimationProblem

StateEstimationProblem(model, inputs, outputs; disturbance_inputs, discretization, Ts, df, dg, d0)

A structure representing a state-estimation problem.

Arguments:

• model: An MTK ODESystem model, this model must not have undergone structural simplification.
• inputs: The inputs to the dynamical system, a vector of symbolic variables that must be of type @variables.
• outputs: The outputs of the dynamical system, a vector of symbolic variables that must be of type @variables.
• disturbance_inputs: The disturbance inputs to the dynamical system, a vector of symbolic variables that must be of type @variables. These disturbance inputs indicate where dynamics noise $w$ enters the system. The probability distribution $d_f$ is defined over these variables.
• discretization: A function discretization(f_cont, Ts, x_inds, alg_inds, nu) = f_disc that takes a continuous-tiem dynamics function f_cont(x,u,p,t) and returns a discrete-time dynamics function f_disc(x,u,p,t). x_inds is the indices of differential state variables, alg_inds is the indices of algebraic variables, and nu is the number of inputs.
• Ts: The discretization time step.
• df: The probability distribution of the dynamics noise $w$. When using Kalman-type estimators, this must be a MvNormal or SimpleMvNormal distribution.
• dg: The probability distribution of the measurement noise $e$. When using Kalman-type estimators, this must be a MvNormal or SimpleMvNormal distribution.
• d0: The probability distribution of the initial state $x_0$. When using Kalman-type estimators, this must be a MvNormal or SimpleMvNormal distribution.

Usage:

Pseudocode

julia prob = StateEstimationProblem(...) kf = get_filter(prob, ExtendedKalmanFilter) # or UnscentedKalmanFilter filtersol = forward_trajectory(kf, u, y) sol = StateEstimationSolution(filtersol, prob) # Package into higher-level solution object plot(sol, idxs=[prob.state; prob.outputs; prob.inputs]) # Plot the solution

StateEstimationSolution

julia StateEstimationSolution(sol::KalmanFilteringSolution, prob::StateEstimationProblem)

A solution object that provides symbolic indexing to a KalmanFilteringSolution object.

Fields:

• sol: a KalmanFilteringSolution object.
• prob: a StateEstimationProblem object.

Example

julia sol = StateEstimationSolution(kfsol, prob) sol[model.x] # Index with a variable sol[model.y^2] # Index with an expression sol[model.y^2, dist=true] # Obtain the posterior probability distribution of the provided expression sol[model.y^2, Nsamples=100] # Draw 100 samples from the posterior distribution of the provided expression

get_filter

julia get_filter(prob::StateEstimationProblem, ::Type{ExtendedKalmanFilter}; constant_R1=true, kwargs) get_filter(prob::StateEstimationProblem, ::Type{UnscentedKalmanFilter}; kwargs)

Instantiate a filter from a state-estimation problem. kwargs are sent to the filter constructor.

If constant_R1=true, the dynamics noise covariance matrix R1 is assumed to be constant and is computed at the initial state. Otherwise, R1 is computed at each time step throug repeated linearization w.r.t. the disturbance inputs w.

propagate_distribution

julia propagate_distribution(f, kf, dist, args...; kwargs...)

Propagate a probability distribution dist through a nonlinear function f using the covariance-propagation method of filter kf.

Arguments:

• f: A nonlinear function f(x, args...; kwargs...) that takes a vector x and returns a vector.
• kf: A state estimator, such as an ExtendedKalmanFilter or UnscentedKalmanFilter.
• dist: A probability distribution, such as a MvNormal or SimpleMvNormal.
• args: Additional arguments to f.
• kwargs: Additional keyword arguments to f.

Generate docs

julia io = IOBuffer() for n in names(LowLevelParticleFiltersMTK) n === :LowLevelParticleFiltersMTK && continue println(io, "# `", n, "`") println(io, Base.Docs.doc(getfield(LowLevelParticleFiltersMTK, n))) end s = String(take!(io)) clipboard(s)