ukf

README

Unscented Kalman Filter

This package implements Unscented Kalman Filter also known as Sigma-point filter.

Example output

Documentation

Index

• type Config
• type SigmaPoints
• type UKF
  • func New(m filter.Model, init filter.InitCond, q, r filter.Noise, c *Config) (*UKF, error)
  • func (k *UKF) Cov() mat.Symmetric
  • func (k *UKF) Gain() mat.Matrix
  • func (k *UKF) GenSigmaPoints(x mat.Vector) (*SigmaPoints, error)
  • func (k *UKF) Model() filter.Model
  • func (k *UKF) OutputNoise() filter.Noise
  • func (k *UKF) Predict(x, u mat.Vector) (filter.Estimate, error)
  • func (k *UKF) Run(x, u, z mat.Vector) (filter.Estimate, error)
  • func (k *UKF) SetCov(cov mat.Symmetric) error
  • func (k *UKF) StateNoise() filter.Noise
  • func (k *UKF) Update(x, u, z mat.Vector) (filter.Estimate, error)

Types

type Config

type Config struct {
	// Alpha is alpha parameter (0,1]
	Alpha float64
	// Beta is beta parameter (2 is optimal choice for Gaussian)
	Beta float64
	// Kappa is kappa parameter (must be non-negative)
	Kappa float64
}

Config contains UKF [unitless] configuration parameters

type SigmaPoints

type SigmaPoints struct {
	// X stores sigma points in its columns
	X *mat.Dense
	// Cov is sigma points covariance
	Cov *mat.SymDense
}

SigmaPoints represents UKF sigma points and their covariance

type UKF

type UKF struct {

	// Wm0 is mean sigma point weight
	Wm0 float64
	// Wc0 is mean sigma point covariance weight
	Wc0 float64
	// Wsp is weight for regular sigma points and covariances
	W float64
	// contains filtered or unexported fields
}

UKF is Unscented (a.k.a. Sigma Point) Kalman Filter

func New

func New(m filter.Model, init filter.InitCond, q, r filter.Noise, c *Config) (*UKF, error)

New creates new UKF and returns it. It accepts the following parameters: - m: dynamical system model - init: initial condition of the filter - q: state a.k.a. process noise - r: output a.k.a. measurement noise - c: filter configuration It returns error if either of the following conditions is met: - invalid model is given: model dimensions must be positive integers - invalid state or output noise is given: noise covariance must either be nil or match the model dimensions - invalid sigma points parameters (alpha, beta, kappa) are supplied - sigma points fail to be generated: due to covariance SVD factorizations failure

func (*UKF) Cov

func (k *UKF) Cov() mat.Symmetric

Cov returns UKF covariance

func (*UKF) Gain

func (k *UKF) Gain() mat.Matrix

Gain returns Kalman gain

func (*UKF) GenSigmaPoints

func (k *UKF) GenSigmaPoints(x mat.Vector) (*SigmaPoints, error)

GenSigmaPoints generates UKF sigma points around x and returns them. It returns error if it fails to generate new sigma points due to covariance SVD facrtorization failure.

func (*UKF) Model

func (k *UKF) Model() filter.Model

Model returns UKF model

func (*UKF) OutputNoise

func (k *UKF) OutputNoise() filter.Noise

OutputNoise retruns output noise

func (*UKF) Predict

func (k *UKF) Predict(x, u mat.Vector) (filter.Estimate, error)

Predict calculates the next system state given the state x and input u and returns its estimate. It first generates new sigma points around x and then attempts to propagate them to the next step. It returns error if it either fails to generate or propagate the sigma points (and x) to the next step.

func (*UKF) Run

func (k *UKF) Run(x, u, z mat.Vector) (filter.Estimate, error)

Run runs one step of UKF for given state x, input u and measurement z. It corrects system state x using measurement z and returns new system estimate. It returns error if it either fails to propagate or correct state x or UKF sigma points.

func (*UKF) SetCov

func (k *UKF) SetCov(cov mat.Symmetric) error

SetCov sets UKF covariance matrix to cov. It returns error if either cov is nil or its dimensions are not the same as UKF covariance dimensions.

func (*UKF) StateNoise

func (k *UKF) StateNoise() filter.Noise

StateNoise retruns state noise

func (*UKF) Update

func (k *UKF) Update(x, u, z mat.Vector) (filter.Estimate, error)

Update corrects state x using the measurement z, given control intput u and returns corrected estimate. It returns error if either invalid state was supplied or if it fails to calculate system output estimate.