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 Σ
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.
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
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; transition matrix C, control matrix B, and symmetric zero-mean measurement noise with symmetric covariance matrix V
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
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
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.
ExtendedKalmanFilter(d::DynamicsModel,o::ObservationModel)
KalmanFilter(d::DynamicsModel,o::ObservationModel,λ::Number,
α::Float,β::Float)Construct Extended Kalman filter with DynamicsModel d and ObservationModel o.
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.
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.
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.
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.
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.
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<:NumberReturns 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.
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<:NumberReturns 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.
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.
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.
Missing docstring for GaussianFilters.update. Check Documenter's build log for details.
Missing docstring for GaussianFilters.predict. Check Documenter's build log for details.
Missing docstring for GaussianFilters.measure. Check Documenter's build log for details.
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.
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.
Examples
Full implementation examples can be found in the notebooks folder of the repo: