Exported functions and types

AdvancedParticleFilter(N::Integer, dynamics::Function, measurement::Function, measurement_likelihood, dynamics_density, initial_density; p = NullParameters(), threads = false, kwargs...)

This type represents a standard particle filter but affords extra flexibility compared to the ParticleFilter type, e.g., non-additive noise in the dynamics and measurement functions.

See the docs for more information: https://baggepinnen.github.io/LowLevelParticleFilters.jl/stable/#AdvancedParticleFilter-1

Arguments:

• N: Number of particles
• dynamics: A discrete-time dynamics function (x, u, p, t, noise=false) -> x⁺. It's important that the noise argument defaults to false.
• measurement: A measurement function (x, u, p, t, noise=false) -> y. It's important that the noise argument defaults to false.
• measurement_likelihood: A function (x, u, y, p, t)->logl to evaluate the log-likelihood of a measurement.
• dynamics_density: This field is not used by the advanced filter and can be set to nothing.
• initial_density: The distribution of the initial state.
• threads: use threads to propagate particles in parallel. Only activate this if your dynamics is thread-safe. SeeToDee.SimpleColloc is not thread-safe by default due to the use of internal caches, but SeeToDee.Rk4 is.

Extended help

Multiple measurement models

The measurement_likelihood function is used to evaluate the likelihood of a measurement. If you have multiple sensors and want to perform individual correct! steps for each, call correct!(..., g = custom_likelihood_function).

AuxiliaryParticleFilter(args...; kwargs...)

Takes exactly the same arguments as ParticleFilter, or an instance of ParticleFilter.

CompositeMeasurementModel(model1, model2, ...)

A composite measurement model that combines multiple measurement models. This model acts as all component models concatenated. The tuple returned from correct! will be

• ll: The sum of the log-likelihood of all component models
• e: The concatenated innovation vector
• S: A vector of the innovation covariance matrices of the component models
• Sᵪ: A vector of the Cholesky factorizations of the innovation covariance matrices of the component models
• K: A vector of the Kalman gains of the component models

If all sensors operate on at the same rate, and all measurement models are of the same type, it's more efficient to use a single measurement model with a vector-valued measurement function.

Fields:

• models: A tuple of measurement models

EKFMeasurementModel{IPM}(measurement, R2, ny, Cjac, cache = nothing)

A measurement model for the Extended Kalman Filter.

Arguments:

• IPM: A boolean indicating if the measurement function is inplace
• measurement: The measurement function y = h(x, u, p, t)
• R2: The measurement noise covariance matrix
• ny: The number of measurement variables
• Cjac: The Jacobian of the measurement function Cjac(x, u, p, t). If none is provided, ForwardDiff will be used.

EKFMeasurementModel{T,IPM}(measurement::M, R2; nx, ny, Cjac = nothing)

• T is the element type used for arrays
• IPM is a boolean indicating if the measurement function is inplace

ExtendedKalmanFilter(kf, dynamics, measurement; Ajac, Cjac)
ExtendedKalmanFilter(dynamics, measurement, R1,R2,d0=MvNormal(Matrix(R1)); nu::Int, p = NullParameters(), α = 1.0, check = true)

A nonlinear state estimator propagating uncertainty using linearization.

The constructor to the extended Kalman filter takes dynamics and measurement functions, and either covariance matrices, or a KalmanFilter. If the former constructor is used, the number of inputs to the system dynamics, nu, must be explicitly provided with a keyword argument.

By default, the filter will internally linearize the dynamics using ForwardDiff. User provided Jacobian functions can be provided as keyword arguments Ajac and Cjac. These functions should have the signature (x,u,p,t)::AbstractMatrix where x is the state, u is the input, p is the parameters, and t is the time.

The dynamics and measurement function are on the following form

x(t+1) = dynamics(x, u, p, t) + w
y      = measurement(x, u, p, t) + e

where w ~ N(0, R1), e ~ N(0, R2) and x(0) ~ d0

See also UnscentedKalmanFilter which is typically more accurate than ExtendedKalmanFilter. See KalmanFilter for detailed instructions on how to set up a Kalman filter kf.

IMM(models, P, μ; check = true, p = NullParameters(), interact = true)

Interacting Multiple Model (IMM) filter. This filter is a combination of multiple Kalman-type filters, each with its own state and covariance. The IMM filter is a probabilistically weighted average of the states and covariances of the individual filters. The weights are determined by the probability matrix P and the mixing probabilities μ.

In addition to the predict! and correct! steps, the IMM filter has an interact! method that updates the states and covariances of the individual filters based on the mixing probabilities. The combine! method combines the states and covariances of the individual filters into a single state and covariance. These four functions are typically called in either of the orders

• correct!, combine!, interact!, predict! (as is done in update!)
• interact!, predict!, correct!, combine! (as is done in the reference cited below)

These two orders are cyclic permutations of each other, and the order used in update! is chosen to align with the order used in the other filters, where the initial condition is corrected using the first measurement, i.e., we assume the first measurement updates $x(0|-1)$ to $x(0|0)$.

The initial (combined) state and covariance of the IMM filter is made up of the weighted average of the states and covariances of the individual filters. The weights are the initial mixing probabilities μ.

Ref: "Interacting multiple model methods in target tracking: a survey", E. Mazor; A. Averbuch; Y. Bar-Shalom; J. Dayan

Arguments:

• models: An array of Kalman-type filters, such as KalmanFilter, ExtendedKalmanFilter, UnscentedKalmanFilter, etc. The state of each model must have the same meaning, such that forming a weighted average makes sense.
• P: The mode-transition probability matrix. P[i,j] is the probability of transitioning from mode i to mode j (each row must sum to one).
• μ: The initial mixing probabilities. μ[i] is the probability of being in mode i at the initial contidion (must sum to one).
• check: If true, check that the inputs are valid. If false, skip the checks.
• p: Parameters for the filter. NOTE: this p is shared among all internal filters. The internal p of each filter will be overridden by this one.
• interact: If true, the filter will run the interaction as part of update! and forward_trajectory. If false, the filter will not run the interaction step. This choice can be overridden by passing the keyword argument interact to the respective functions.

KalmanFilter(A,B,C,D,R1,R2,d0=MvNormal(R1); p = NullParameters(), α=1, check=true)

The matrices A,B,C,D define the dynamics

x' = Ax + Bu + w
y  = Cx + Du + e

where w ~ N(0, R1), e ~ N(0, R2) and x(0) ~ d0

The matrices can be time varying such that, e.g., A[:, :, t] contains the $A$ matrix at time index t. They can also be given as functions on the form

Afun(x, u, p, t) -> A

For maximum performance, provide statically sized matrices from StaticArrays.jl

α is an optional "forgetting factor", if this is set to a value > 1, such as 1.01-1.2, the filter will, in addition to the covariance inflation due to $R_1$, exhibit "exponential forgetting" similar to a Recursive Least-Squares (RLS) estimator. It is thus possible to get a RLS-like algorithm by setting $R_1=0, R_2 = 1/α$ and $α > 1$ ($α$ is the inverse of the traditional RLS parameter $α = 1/λ$). The exact form of the covariance update is

\[R(t+1|t) = α AR(t)A^T + R_1\]

If check = true (default) the function will check that the eigenvalues of A are less than 2 in absolute value. Large eigenvalues may be an indication that the system matrices are representing a continuous-time system and the user has forgotten to discretize it. Turn off this check by setting check = false.

Tutorials on Kalman filtering

The tutorial "How to tune a Kalman filter" details how to figure out appropriate covariance matrices for the Kalman filter, as well as how to add disturbance models to the system model. See also the tutorial in the documentation

KalmanFilteringSolution{Tx,Txt,TR,TRt,Tll} <: AbstractFilteringSolution

Fields

• 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(t|t-1) = y - ŷ(t|t-1)$ (plotted if plote=true)

Plot

The solution object can be plotted

plot(sol, plotx=true, plotxt=true, plotR=true, plotRt=true, plote=true, plotu=true, ploty=true, plotyh=true, plotyht=true, name="")

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.

LinearMeasurementModel{CT, DT, RT, CAT}

A linear measurement model $y = C*x + D*u + e$.

Fields:

• C
• D
• R2: The measurement noise covariance matrix
• ny: The number of measurement variables

ParticleFilter(N::Integer, dynamics, measurement, dynamics_density, measurement_density, initial_density; threads = false, p = NullParameters(), kwargs...)

See the docs for more information: https://baggepinnen.github.io/LowLevelParticleFilters.jl/stable/#Particle-filter-1

Arguments:

• N: Number of particles
• dynamics: A discrete-time dynamics function (x, u, p, t) -> x⁺
• measurement: A measurement function (x, u, p, t) -> y
• dynamics_density: A probability-density function for additive noise in the dynamics. Use AdvancedParticleFilter for non-additive noise.
• measurement_density: A probability-density function for additive measurement noise. Use AdvancedParticleFilter for non-additive noise.
• initial_density: Distribution of the initial state.

ParticleFilteringSolution{F, Tu, Ty, Tx, Tw, Twe, Tll} <: AbstractFilteringSolution

Fields:

• f: The filter used to produce the solution.
• u: Input
• y: Output / measurements
• x: Particles, the size of this array is (N,T), where N is the number of particles and T is the number of time steps. Each element represents a weighted state hypothesis with weight given by we.
• w: Weights (log space). These are used for internal computations.
• we: Weights (exponentiated / original space). These are the ones to use to compute weighted means etc., they sum to one for each time step.
• ll: Log likelihood

Plot

The solution object can be plotted

plot(sol; nbinsy=30, xreal=nothing, dim=nothing, ploty=true)

SqKalmanFilter(A,B,C,D,R1,R2,d0=MvNormal(R1); p = NullParameters(), α=1)

A standard Kalman filter on square-root form. This filter may have better numerical performance when the covariance matrices are ill-conditioned.

The matrices A,B,C,D define the dynamics

x' = Ax + Bu + w
y  = Cx + Du + e

where w ~ N(0, R1), e ~ N(0, R2) and x(0) ~ d0

The matrices can be time varying such that, e.g., A[:, :, t] contains the $A$ matrix at time index t. They can also be given as functions on the form

Afun(x, u, p, t) -> A

The internal fields storing covariance matrices are for this filter storing the upper-triangular Cholesky factor.

α is an optional "forgetting factor", if this is set to a value > 1, such as 1.01-1.2, the filter will, in addition to the covariance inflation due to $R_1$, exhibit "exponential forgetting" similar to a Recursive Least-Squares (RLS) estimator. It is thus possible to get a RLS-like algorithm by setting $R_1=0, R_2 = 1/α$ and $α > 1$ ($α$ is the inverse of the traditional RLS parameter $α = 1/λ$). The form of the covariance update is

\[R(t+1|t) = α AR(t)A^T + R_1\]

Ref: "A Square-Root Kalman Filter Using Only QR Decompositions", Kevin Tracy https://arxiv.org/abs/2208.06452

UKFMeasurementModel{inplace_measurement,augmented_measurement}(measurement, R2, ny, ne, innovation, mean, cov, cross_cov, cache = nothing)

A measurement model for the Unscented Kalman Filter.

Arguments:

• measurement: The measurement function y = h(x, u, p, t)
• R2: The measurement noise covariance matrix
• ny: The number of measurement variables
• ne: If augmented_measurement is true, the number of measurement noise variables
• innovation(y::AbstractVector, yh::AbstractVector) where the arguments represent (measured output, predicted output)
• mean(ys::AbstractVector{<:AbstractVector}): computes the mean of the vector of vectors of output sigma points.
• cov(ys::AbstractVector{<:AbstractVector}, y::AbstractVector): computes the covariance matrix of the output sigma points.
• cross_cov(xs::AbstractVector{<:AbstractVector}, x::AbstractVector, ys::AbstractVector{<:AbstractVector}, y::AbstractVector) where the arguments represents (state sigma points, mean state, output sigma points, mean output). The function should return the cross-covariance matrix between the state and output sigma points.

UKFMeasurementModel{T,IPM,AUGM}(measurement, R2; nx, ny, ne = nothing, innovation = -, mean = safe_mean, cov = safe_cov, cross_cov = cross_cov, static = nothing)

• T is the element type used for arrays
• IPM is a boolean indicating if the measurement function is inplace
• AUGM is a boolean indicating if the measurement model is augmented

UnscentedKalmanFilter(dynamics, measurement, R1, R2, d0=MvNormal(Matrix(R1)); p = NullParameters(), ny, nu)
UnscentedKalmanFilter{IPD,IPM,AUGD,AUGM}(dynamics, measurement_model::AbstractMeasurementModel, R1, d0=SimpleMvNormal(R1); p=NullParameters(), nu)

A nonlinear state estimator propagating uncertainty using the unscented transform.

The dynamics and measurement function are on either of the following forms

x' = dynamics(x, u, p, t) + w
y  = measurement(x, u, p, t) + e

x' = dynamics(x, u, p, t, w)
y  = measurement(x, u, p, t, e)

where w ~ N(0, R1), e ~ N(0, R2) and x(0) ~ d0. The former (default) assums that the noise is additive and added after the dynamics and measurement updates, while the latter assumes that the dynamics functions take an additional argument corresponding to the noise term. The latter form (sometimes refered to as the "augmented" form) is useful when the noise is multiplicative or when the noise is added before the dynamics and measurement updates. See "Augmented UKF" below for more details on how to use this form.

The matrices R1, R2 can be time varying such that, e.g., R1[:, :, t] contains the $R_1$ matrix at time index t. They can also be given as functions on the form

Rfun(x, u, p, t) -> R

For maximum performance, provide statically sized matrices from StaticArrays.jl

ny, nu indicate the number of outputs and inputs.

Custom type of u

The input u may be of any type, e.g., a named tuple or a custom struct. The u provided in the input data is passed directly to the dynamics and measurement functions, so as long as the type is compatible with the dynamics it will work out. The one exception where this will not work is when calling simulate, which assumes that u is an array.

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,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.
• 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. (If the measurement noise has fewer degrees of freedom than the number of measurements, you may failure in Cholesky factorizations, see "Custom Cholesky factorization" below).

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.

Sigma-point rejection

For problems with challenging dynamics, a mechanism for rejection of sigma points after the dynamics update is provided. A function reject(x) -> Bool can be provided through the keyword argument reject that returns true if a sigma point for $x(t+1)$ should be rejected, e.g., if an instability or non-finite number is detected. A rejected point is replaced by the propagated mean point (the mean point cannot be rejected). This function may be provided either to the constructor of the UKF or passed to the predict! function.

Custom measurement models

By default, standard arithmetic mean and e(y, yh) = y - yh are used as mean and innovation functions.

By passing and explicitly created UKFMeasurementModel, one may provide custom functions that compute the mean, the covariance and the innovation. This is useful in situations where the state or a measurement lives on a manifold. One may further override the mean and covariance functions for the state sigma points by passing the keyword arguments state_mean and state_cov to the constructor.

• state_mean(xs::AbstractVector{<:AbstractVector}) computes the mean of the vector of vectors of state sigma points.
• state_cov(xs::AbstractVector{<:AbstractVector}, m = mean(xs)) where the first argument represent state sigma points and the second argument, which must be optional, represents the mean of those points. The function should return the covariance matrix of the state sigma points.

See UKFMeasurementModel for more details on how to set up a custom measurement model. Pass the custom measurement model as the second argument to the UKF constructor.

Custom Cholesky factorization

The UnscentedKalmanFilter supports providing a custom function to compute the Cholesky factorization of the covariance matrices for use in sigma-point generation.

If either of the following conditions are met, you may experience failure in internal Cholesky factorizations:

• The dynamics noise or measurement noise covariance matrices ($R_1, R_2$) are singular
• The measurement is augmented and the measurement noise has fewer degrees of freedom than the number of measurements
• (Under specific technical conditions) The dynamics is augmented and the dynamics noise has fewer degrees of freedom than the number of state variables. The technical conditions are easiest to understand in the linear-systems case, where it corresponds to the Riccati equation associated with the Kalman gain not having a solution. This may happen when the pair $(A, R1)$ has uncontrollable modes on the unit circle, for example, when there are integrating modes that are not affected through the noise.

The error message may look like

ERROR: PosDefException: matrix is not positive definite; Factorization failed.

In such situations, it is advicable to reconsider the noise model and covariance matrices, alternatively, you may provide a custom Cholesky factorization function to the UKF constructor through the keyword argument cholesky!. The function should have the signature cholesky!(A::AbstractMatrix)::Cholesky. A useful alternative factorizaiton when covariance matrices are expected to be singular is cholesky! = R->cholesky!(Positive, Matrix(R)) where the "positive" Cholesky factorization is provided by the package PositiveFactorizations.jl, which must be manually installed and loaded by the user.

combine!(imm::IMM)

Combine the models of the IMM filter into a single state imm.x and covariance imm.R. This is done by taking a weighted average of the states and covariances of the individual models, where the weights are the mixing probabilities μ.

commandplot(pf, u, y, p=parameters(pf); kwargs...)

Produce a helpful plot. For customization options (kwargs...), see ?pplot. After each time step, a command from the user is requested.

• q: quit
• s n: step n steps

ll, e = correct!(pf, u, y, p = parameters(f), t = index(f))

Update state/weights based on measurement y, returns log-likelihood and prediction error (the error is always 0 for particle filters).

Extended help

To perform separate measurement updates for different sensors, see the "Measurement models" in the documentation. For AdvancedParticleFilter, this can be realized by passing a custom measurement_likelihood function as the keyword argument g to correct!, or by calling the lower-level function measurement_equation! with a custom measurement_likelihood.

(; ll, e, S, Sᵪ, K) = correct!(kf::AbstractKalmanFilter, u, y, p = parameters(kf), t::Integer = index(kf), R2)

The correct step for a Kalman filter returns not only the log likelihood ll and the prediction error e, but also the covariance of the output S, its Cholesky factor Sᵪ and the Kalman gain K.

If R2 stored in kf is a function R2(x, u, p, t), this function is evaluated at the state before the correction is performed. The measurement noise covariance matrix R2 stored in the filter object can optionally be overridden by passing the argument R2, in this case R2 must be a matrix.

Extended help

To perform separate measurement updates for different sensors, see the "Measurement models" in the documentation.

correct!(kf::SqKalmanFilter, u, y, p = parameters(kf), t::Real = index(kf); R2 = get_mat(kf.R2, kf.x, u, p, t))

For the square-root Kalman filter, a custom provided R2 must be the upper triangular Cholesky factor of the covariance matrix of the measurement noise.

ll, lls, rest = correct!(imm::IMM, u, y, args; kwargs)

The correct step of the IMM filter corrects each model with the measurements y and control input u. The mixing probabilities imm.μ are updated based on the likelihood of each model given the measurements and the transition probability matrix P.

The returned tuple consists of the sum of the log-likelihood of all models, the vector of individual log-likelihoods and an array of the rest of the return values from the correct step of each model.

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 allows the user to override, R2, mean, cross_cov, innovation.

Arguments:

• u: The input
• y: The measurement
• p: The parameters
• t: The current time
• R2: The measurement noise covariance matrix, or a function that returns the covariance matrix (x,u,p,t)->R2.
• 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.

Extended help

To perform separate measurement updates for different sensors, see the "Measurement models" in the documentation

debugplot(pf, u, y, p=parameters(pf); runall=false, kwargs...)

Produce a helpful plot. For customization options (kwargs...), see ?pplot.

• runall=false: if true, runs all time steps befor displaying (faster), if false, displays the plot after each time step.

The generated plot becomes quite heavy. Initially, try limiting your input to 100 time steps to verify that it doesn't crash.

densityplot(x,[w])

Plot (weighted) particles densities

sol = forward_trajectory(pf, u::AbstractVector, y::AbstractVector, p=parameters(pf))

Run the particle filter for a sequence of inputs and measurements (offline / batch filtering). Return a solution with x,w,we,ll = particles, weights, expweights and loglikelihood

If MonteCarloMeasurements.jl is loaded, you may transform the output particles to Matrix{MonteCarloMeasurements.Particles} using Particles(x,we). Internally, the particles are then resampled such that they all have unit weight. This is conventient for making use of the plotting facilities of MonteCarloMeasurements.jl.

sol can be plotted

plot(sol::ParticleFilteringSolution; nbinsy=30, xreal=nothing, dim=nothing)

sol = forward_trajectory(kf::AbstractKalmanFilter, u::Vector, y::Vector, p=parameters(kf))

Run a Kalman filter forward to perform (offline / batch) filtering along an entire trajectory u, y.

Returns a KalmanFilteringSolution: with the following

• x: predictions $x(t|t-1)$
• xt: filtered estimates $x(t|t)$
• R: predicted covariance matrices $R(t|t-1)$
• Rt: filter covariances $R(t|t)$
• ll: loglik

sol can be plotted

plot(sol::KalmanFilteringSolution; plotx = true, plotxt=true, plotu=true, ploty=true)

See KalmanFilteringSolution for more details.

Extended help

Very large systems

If your system is very large, i.e., the dimension of the state is very large, and the arrays u,y are long, this function may use a lot of memory to store all covariance matrices R, Rt. If you do not need all the information retained by this function, you may opt to call one of the functions

• loglik
• LowLevelParticleFilters.sse
• LowLevelParticleFilters.prediction_errors!

That store significantly less information. The amount of computation performed by all of these functions is identical, the only difference lies in what is stored and returned.

forward_trajectory(imm::IMM, u, y, p = parameters(imm); interact = true)

When performing batch filtering using an IMM filter, one may

• Override the interact parameter of the filter
• Access the mode probabilities along the trajectory as the sol.extra field. This is a matrix of size (n_modes, T) where T is the length of the trajectory (length of u and y).

The returned solution object is of type KalmanFilteringSolution and has the following fields:

interact!(imm::IMM)

The interaction step of the IMM filter updates the state and covariance of each internal model based on the mixing probabilities imm.μ and the transition probability matrix imm.P.

Models with small mixing probabilities will have their states and covariances updated more towards the states and covariances of models with higher mixing probabilities, and vice versa.

ll(θ) = log_likelihood_fun(filter_from_parameters(θ::Vector)::Function, priors::Vector{Distribution}, u, y, p)

returns function θ -> p(y|θ)p(θ)

ll = loglik(filter, u, y, p=parameters(filter))

Calculate log-likelihood for entire sequences u,y

ll = logsumexp!(w, we [, maxw])

Normalizes the weight vector w and returns the weighted log-likelihood

https://arxiv.org/pdf/1412.8695.pdf eq 3.8 for p(y) https://discourse.julialang.org/t/fast-logsumexp/22827/7?u=baggepinnen for stable logsumexp

x,ll = mean_trajectory(pf, u::Vector{Vector}, y::Vector{Vector}, p=parameters(pf))

This method resets the particle filter to the initial state distribution upon start

mean_trajectory(sol::ParticleFilteringSolution)
mean_trajectory(x::AbstractMatrix, we::AbstractMatrix)

Compute the weighted mean along the trajectory of a particle-filter solution. Returns a matrix of size T × nx. If x and we are supplied, the weights are expected to be in the original space (not log space).

metropolis(ll::Function(θ), R::Int, θ₀::Vector, draw::Function(θ) = naive_sampler(θ₀))

Performs MCMC sampling using the marginal Metropolis (-Hastings) algorithm draw = θ -> θ' samples a new parameter vector given an old parameter vector. The distribution must be symmetric, e.g., a Gaussian. R is the number of iterations. See log_likelihood_fun

Example:

filter_from_parameters(θ) = ParticleFilter(N, dynamics, measurement, MvNormal(n,exp(θ[1])), MvNormal(p,exp(θ[2])), d0)
priors = [Normal(0,0.1),Normal(0,0.1)]
ll     = log_likelihood_fun(filter_from_parameters,priors,u,y,1)
θ₀ = log.([1.,1.]) # Initial point
draw = θ -> θ .+ rand(MvNormal(0.1ones(2))) # Function that proposes new parameters (has to be symmetric)
burnin = 200 # If using threaded call, provide number of burnin iterations
# @time theta, lls = metropolis(ll, 2000, θ₀, draw) # Run single threaded
# thetam = reduce(hcat, theta)'
@time thetalls = LowLevelParticleFilters.metropolis_threaded(burnin, ll, 5000, θ₀, draw) # run on all threads, will provide (2000-burnin)*nthreads() samples
histogram(exp.(thetalls[:,1:2]), layout=3)
plot!(thetalls[:,3], subplot=3) # if threaded call, log likelihoods are in the last column

reset!(kf::AbstractKalmanFilter; x0)

Reset the initial distribution of the state. Optionally, a new mean vector x0 can be provided.

Reset the filter to initial state and covariance/distribution

reset!(kf::SqKalmanFilter; x0)

Reset the initial distribution of the state. Optionally, a new mean vector x0 can be provided.

x,u,y = simulate(f::AbstractFilter, T::Int, du::Distribution, p=parameters(f), [N]; dynamics_noise=true, measurement_noise=true)
x,u,y = simulate(f::AbstractFilter, u, p=parameters(f); dynamics_noise=true, measurement_noise=true)

Simulate dynamical system forward in time T steps, or for the duration of u. Returns state sequence, inputs and measurements.

• u is an input-signal trajectory, alternatively, du is a distribution of random inputs.

A simulation can be considered a draw from the prior distribution over the evolution of the system implied by the selected noise models. Such a simulation is useful in order to evaluate whether or not the noise models are reasonable.

If MonteCarloMeasurements.jl is loaded, the argument N::Int can be supplied, in which case N simulations are done and the result is returned in the form of Vector{MonteCarloMeasurements.Particles}.

xT,RT,ll = smooth(sol, kf)
xT,RT,ll = smooth(kf::KalmanFilter, u::Vector, y::Vector, p=parameters(kf))
xT,RT,ll = smooth(kf::ExtendedKalmanFilter, u::Vector, y::Vector, p=parameters(kf))

Returns smoothed estimates of state x and covariance R given all input output data u,y or an existing solution sol obtained from forward_trajectory.

xb,ll = smooth(pf, M, u, y, p=parameters(pf))
xb,ll = smooth(pf, xf, wf, wef, ll, M, u, y, p=parameters(pf))

Perform particle smoothing using forward-filtering, backward simulation. Return smoothed particles and loglikelihood. See also smoothed_trajs, smoothed_mean, smoothed_cov

smoothed_cov(xb)

Helper function to calculate the covariance of smoothed particle trajectories

smoothed_mean(xb)

Helper function to calculate the mean of smoothed particle trajectories

smoothed_trajs(xb)

Helper function to get particle trajectories as a 3-dimensions array (N,M,T) instead of matrix of vectors.

ll, e = update!(f::AbstractFilter, u, y, p = parameters(f), t = index(f))

Perform one step of predict! and correct!, returns log-likelihood and prediction error

update!(imm::IMM, u, y, p, t; correct_kwargs = (;), predict_kwargs = (;), interact = true)

The combined udpate for an IMM filter performs the following steps:

1. Correct each model with the measurements y and control input u.
2. Combine the models into a single state and covariance.
3. Interact the models to update their respective state and covariance.
4. Predict each model to the next time step.

This differs slightly from the udpate step of other filters, where at the end of an update the state of the filter is the one-step ahead predicted value, whereas here each individual filter has a predicted state, but the combine! step of the IMM filter hasn't been performed on the predictions yet. The state of the IMM filter is thus $x(t|t)$ and not $x(t+1|t)$ like it is for other filters, and each filter internal to the IMM.

Arguments:

• correct_kwargs: An optional named tuple of keyword arguments that are sent to correct!.
• predict_kwargs: An optional named tuple of keyword arguments that are sent to predict!.
• interact: Whether or not to run the interaction step.

weighted_cov(x,we)

Similar to weighted_mean, but returns covariances

x̂ = weighted_mean(x,we)

Calculated weighted mean of particle trajectories. we are expweights.

x̂ = weighted_mean(pf)
x̂ = weighted_mean(s::PFstate)

predict!(kf::SqKalmanFilter, u, p = parameters(kf), t::Real = index(kf); R1 = get_mat(kf.R1, kf.x, u, p, t), α = kf.α)

For the square-root Kalman filter, a custom provided R1 must be the upper triangular Cholesky factor of the covariance matrix of the process noise.

predict!(f, u, p = parameters(f), t = index(f))

Move filter state forward in time using dynamics equation and input vector u.

predict!(kf::AbstractKalmanFilter, u, p = parameters(kf), t::Integer = index(kf); R1, α = kf.α)

Perform the prediction step (updating the state estimate to $x(t+1|t)$). If R1 stored in kf is a function R1(x, u, p, t), this function is evaluated at the state before the prediction is performed. The dynamics noise covariance matrix R1 stored in kf can optionally be overridden by passing the argument R1, in this case R1 must be a matrix.

predict!(ukf::UnscentedKalmanFilter, u, p = parameters(ukf), t::Real = index(ukf) * ukf.Ts; R1 = get_mat(ukf.R1, ukf.x, u, p, t), reject, mean, cov, dynamics)

The prediction step for an UnscentedKalmanFilter allows the user to override, R1 and any of the functions, reject, mean, cov, dynamics`.

Arguments:

• u: The input
• p: The parameters
• t: The current time
• R1: The dynamics noise covariance matrix, or a function that returns the covariance matrix.
• reject: A function that takes a sigma point and returns true if it should be rejected.
• mean: The function that computes the mean of the state sigma points.
• cov: The function that computes the covariance of the state sigma points.

prediction_errors!(res, f::AbstractFilter, u, y, p = parameters(f), λ = 1)

Calculate the prediction errors and store the result in res. Similar to sse, this function is useful for sum-of-squares optimization. In contrast to sse, this function returns the residuals themselves rather than their sum of squares. This is useful for Gauss-Newton style optimizers, such as LeastSquaresOptim.LevenbergMarquardt.

Arguments:

• res: A vector of length ny*length(y). Note, for each datapoint in u and u, there are ny outputs, and thus ny residuals.
• f: Any filter
• λ: A weighting factor to minimize dot(e, λ, e). A commonly used metric is λ = Diagonal(1 ./ (mag.^2)), where mag is a vector of the "typical magnitude" of each output. Internally, the square root of W = sqrt(λ) is calculated so that the residuals stored in res are W*e.

See example in Solving using Gauss-Newton optimization.