GitHub

Kalman-Class Filters

The Kalman, Extended Kalman, and Unscented Kalman filters are used to estimate state using unimodal multivariate Gaussian distributions. A state estimate in this framework is defined as a GaussianBelief consisting of a mean and covariance.

GaussianFilters.GaussianBelief — Type
GaussianBelief(μ::AbstractVector,Σ::Symmetric)
GaussianBelief(μ::AbstractVector,Σ::AbstractMatrix)

Construct a gaussian belief, consisting of mean vector μ and symmetric covariance matrix Σ

source

Building a Filter

In general, Kalman-class filters can be built with either linear or non-linear dynamics and measurement models. Linear models should be defined with appropriately matrices. Non-linear models should be defined using an appropriate function of two variables, state and action. Both models should be defined with symmetric noise covariance matrices.

NOTE: There is no need to define Jacobians for non-linear models, since this package uses automatic forward differentiation to compute Jacobians in real time. Just make sure the models are forward differentiable in all possible belief locations.

GaussianFilters.LinearDynamicsModel — Type
LinearDynamicsModel(A::AbstractMatrix,B::AbstractMatrix,W::Symmetric)
LinearDynamicsModel(A::AbstractMatrix,B::AbstractMatrix,W::AbstractMatrix)

Construct linear dynamics model with transition matrix A, control matrix B, and symmetric zero-mean process noise with symmetric covariance matrix W

source
GaussianFilters.LinearObservationModel — Type
LinearObservationModel(C::AbstractMatrix,D::AbstractMatrix,V::Symmetric)
LinearObservationModel(C::AbstractMatrix,D::AbstractMatrix,V::AbstractMatrix)
LinearObservationModel(C::AbstractMatrix,V::Symmetric)
LinearObservationModel(C::AbstractMatrix,V::AbstractMatrix)

Construct linear observation dynamics model with output matrix C, feedthrough matrix D, and symmetric zero-mean measurement noise with symmetric covariance matrix V

source
GaussianFilters.NonlinearDynamicsModel — Type
NonlinearDynamicsModel(f::Function,W::Symmetric)
NonlinearDynamicsModel(f::Function,W::AbstractMatrix)

Construct nonlinear dynamics model with transition function f and symmetric zero-mean process noise with symmetric covariance matrix W

source
GaussianFilters.NonlinearObservationModel — Type
NonlinearObservationModel(h::Function,V::Symmetric)
NonlinearObservationModel(h::Function,V::AbstractMatrix)

Construct nonlinear observation dynamics model with measurement function h and symmetric zero-mean measurement noise with symmetric covariance matrix V

source

Use the filter constructors with the appropriately typed models to build a filter. It is recommended to always construct a Kalman filter type when both dynamics and observation models are linear.

GaussianFilters.KalmanFilter — Type
KalmanFilter(d::LinearDynamicsModel,o::LinearObservationModel)

Construct Kalman filter with LinearDynamicsModel d and LinearObservationModel o.

source
GaussianFilters.ExtendedKalmanFilter — Type
ExtendedKalmanFilter(d::DynamicsModel,o::ObservationModel)
KalmanFilter(d::DynamicsModel,o::ObservationModel,λ::Number,
    α::Float,β::Float)

Construct Extended Kalman filter with DynamicsModel d and ObservationModel o.

source
GaussianFilters.UnscentedKalmanFilter — Type
UnscentedKalmanFilter(d::DynamicsModel,o::ObservationModel,λ::Number,
    α::Float,β::Float)
UnscentedKalmanFilter(d::DynamicsModel,o::ObservationModel,λ::Number)
UnscentedKalmanFilter(d::DynamicsModel,o::ObservationModel)

Construct Unscented Kalman filter with DynamicsModel d, ObservationModel o, and UKF parameters λ, α, and β. Default constructor uses α/β formulation from Probabilistic Robotics, second constructor reduces complexity, third constructor defaults λ to 2, as is commonly done.

source

The UKF additionally exposes the sigma-point machinery used internally:

GaussianFilters.unscented_transform — Function

unscentedtransform(b::GaussianBelief, λ::Int=2, α::Number=1, β::Number=0; decompmethod::String = "cholesky")

Convert a single GaussianBelief (mean and covariance) to set of 2n+1 sigma points, with n being the dimensionality of the state space. Return an array of the points, and arrays for weights used in mean and covariance calculations. Uses formulation from ProbRob for α/β parameters on a separate covariance weighting, although this is not necessary.

source
GaussianFilters.unscented_transform_inverse — Function
unscented_transform_inverse(points::Vector{AbstractVector}, w_μ::Vector,
    w_Σ::Vector)

Convert a 2n+1 sigma points and weights back to a single measure for mean and covariance (GaussianBelief).

Uses formulation from ProbRob for α/β parameters on a separate covariance weighting, although this is not necessary.

source

Simulating Data

Given a filter, an initial belief, and an action sequence, you can either simulate state and measurement data all at once with simulation or one step at a time with simulate_step

GaussianFilters.simulation — Function
simulation(filter::AbstractFilter, b0::GaussianBelief,
            action_sequence::Vector{AbstractVector}})

Run a simulation to get positions and measurements. Samples starting point from GaussianBelief b0, the runs action_sequence with additive gaussian noise all specified by AbstractFilter filter to return a simulated state and measurement history.

source
GaussianFilters.simulate_step — Function
simulate_step(filter::AbstractFilter, x::AbstractVector, u::AbstractVector, rng::AbstractRNG=Random.GLOBAL_RNG)

Run a step of simulation starting at state x, taking action u, and using the motion and measurement equations specified by the filter.

source

In addition, the dynamics and observation models can be queried on a single state control input using the predict and measure methods respectively.

GaussianFilters.predict — Method
predict(m::LinearDynamicsModel, x::AbstractVector{<:Number}, u::AbstractVector{<:Number})
predict(m::LinearDynamicsModel, x::AbstractVector{<:Number}, u::AbstractVector{<:Number}, rng::AbstractRNG)

Uses the linear dynamics model to propagate the state x one step forward in time with control input u. If rng is given, it adds process noise.

source
GaussianFilters.predict — Method
predict(m::NonLinearDynamicsModel, x::AbstractVector{<:Number}, u::AbstractVector{<:Number})
predict(m::NonLinearDynamicsModel, x::AbstractVector{<:Number}, u::AbstractVector{<:Number}, rng::AbstractRNG)

Uses the non linear dynamics model to propagate the state x one step forward in time with control input u. If rng is given, it adds process noise.

source
GaussianFilters.measure — Method
measure(m::LinearObservationModel, x::AbstractVector{<:Number}, u::AbstractVector{<:Number})
measure(m::LinearObservationModel, x::AbstractVector{T}, u::AbstractVector{T}, rng::AbstractRNG) where T<:Number

Returns an observation of state x according to the linear observation model m, with control inputs u. If rng is passed, adds additive Gaussian noise to the observation.

source
GaussianFilters.measure — Method
measure(m::NonlinearObservationModel, x::AbstractVector{<:Number}, u::AbstractVector{<:Number})
measure(m::NonlinearObservationModel, x::AbstractVector{T}, u::AbstractVector{T}, rng::AbstractRNG) where T<:Number

Returns an observation of state x according to the non linear observation model m, with control inputs u. If rng is passed, adds additive Gaussian noise to the observation.

source

Running a Filter

You can run a filter on a sequential measurement data using the run_filter function.

GaussianFilters.run_filter — Function
run_filter(filter::AbstractFilter, b0::GaussianBelief, action_history::Vector{AbstractVector},
        measurement_history::Vector{AbstractVector})

Given an initial belief b0, matched-size arrays for action and measurement histories and a filter, update the beliefs using the filter, and return a vector of all beliefs.

source

Alternatively, you can make step-wise belief updates using the update function, which consists of a two-step process to a) predict the next state given a known action and b) make measurement-based belief updates with measure.

GaussianFilters.update — Method
update(filter::AbstractFilter, b0::GaussianBelief, u::AbstractVector,
    y::AbstractVector)

Uses AbstractFilter filter to update gaussian belief b0, given control vector u and measurement vector y.

source
GaussianFilters.predict — Method
predict(filter::KalmanFilter, b0::GaussianBelief, u::AbstractVector)

Uses Kalman filter to run prediction step on gaussian belief b0, given control vector u.

source
GaussianFilters.predict — Method
predict(filter::ExtendedKalmanFilter, b0::GaussianBelief, u::AbstractVector)

Uses Extended Kalman filter to run prediction step on gaussian belief b0, given control vector u.

source
GaussianFilters.predict — Method
predict(filter::UnscentedKalmanFilter, b0::GaussianBelief, u::AbstractVector)

Uses Unscented Kalman filter to run prediction step on gaussian belief b0, given control vector u.

source
GaussianFilters.measure — Method
measure(filter::KalmanFilter, bp::GaussianBelief, y::AbstractVector;
    u::AbstractVector = [false])

Uses Kalman filter to run measurement update on predicted gaussian belief bp, given measurement vector y. If u is specified and filter.o.D has been declared, then matrix D will be factored into the y predictions

source
GaussianFilters.measure — Method
measure(filter::ExtendedKalmanFilter, bp::GaussianBelief, y::AbstractVector;
    u::AbstractVector = [false])

Uses Extended Kalman filter to run measurement update on predicted gaussian belief bp, given measurement vector y. If u is specified and filter.o.D has been declared, then matrix D will be factored into the y predictions.

source
GaussianFilters.measure — Method
measure(filter::UnscentedKalmanFilter, bp::GaussianBelief, y::AbstractVector;
    u::AbstractVector = [false])

Uses Unscented Kalman filter to run measurement update on predicted gaussian belief bp, given measurement vector y. If u is specified and filter.o.D has been declared, then matrix D will be factored into the y predictions.

source

Utilities

The output of running a filter is a GaussianBelief vector, which can be condensed into appropriate tensors with unpack.

GaussianFilters.unpack — Function
unpack(belief_history::Vector{<:GaussianBelief};
    dims::Vector{Int}=[])

Given a history of beliefs, return an unpacked (time steps, state dim)-sized array of predicted means and a (time steps, state dim, state dim)-sized array of covariances. One can optionally specify dimensions indices dims to output reduced state information.

source

belief_ellipse can be used to convert a 2-D Gaussian belief into points along a confidence interval ellipse for plotting.

GaussianFilters.belief_ellipse — Function
belief_ellipse(b::GaussianBelief, P::Float=0.95; δ::Number=5)

Construct and return the x and y points of a 2D gaussian belief, with P being the total probability captured by the ellipse (P ∈ (0,1)), and δ the degree increment between points.

source

For interoperability with the POMDPs.jl belief-updater interface, see the POMDPs.jl Integration page.

Examples

Full implementation examples can be found in the examples/ directory of the repo: