ekf

README

Extended Kalman Filter

This package implements Extended Kalman Filter.

Example output

Documentation

Index

• type EKF
  • func New(m filter.DiscreteModel, init filter.InitCond, q, r filter.Noise) (*EKF, error)
  • func (k *EKF) Cov() mat.Symmetric
  • func (k *EKF) Gain() mat.Matrix
  • func (k *EKF) Model() filter.DiscreteModel
  • func (k *EKF) OutputNoise() filter.Noise
  • func (k *EKF) Predict(x, u mat.Vector) (filter.Estimate, error)
  • func (k *EKF) Run(x, u, z mat.Vector) (filter.Estimate, error)
  • func (k *EKF) SetCov(cov mat.Symmetric) error
  • func (k *EKF) StateNoise() filter.Noise
  • func (k *EKF) Update(x, u, z mat.Vector) (filter.Estimate, error)
• type IEKF
  • func NewIter(m filter.DiscreteModel, ic filter.InitCond, q, r filter.Noise, n int) (*IEKF, error)
  • func (k *IEKF) Run(x, u, z mat.Vector) (filter.Estimate, error)
  • func (k *IEKF) Update(x, u, z mat.Vector) (filter.Estimate, error)
• type JacFunc

Types

type EKF

type EKF struct {

	// FJacFn is propagation Jacobian function
	FJacFn JacFunc

	// HJacFn is observation Jacobian function
	HJacFn JacFunc
	// contains filtered or unexported fields
}

EKF is Extended Kalman Filter

func New

func New(m filter.DiscreteModel, init filter.InitCond, q, r filter.Noise) (*EKF, error)

New creates new EKF 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 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

func (*EKF) Cov

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

Cov returns EKF covariance

func (*EKF) Gain

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

Gain returns Kalman gain

func (*EKF) Model

func (k *EKF) Model() filter.DiscreteModel

Model returns EKF model

func (*EKF) OutputNoise

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

OutputNoise retruns output noise

func (*EKF) Predict

func (k *EKF) 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 (*EKF) Run

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

Run runs one step of EKF 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.

func (*EKF) SetCov

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

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

func (*EKF) StateNoise

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

StateNoise retruns state noise

func (*EKF) Update

func (k *EKF) 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.

type IEKF

type IEKF struct {
	// ekf.EKF is extended Kalman filter
	*EKF
	// contains filtered or unexported fields
}

IEKF is Iterated Extended Kalman Filter

func NewIter

func NewIter(m filter.DiscreteModel, ic filter.InitCond, q, r filter.Noise, n int) (*IEKF, error)

NewIter creates new Iterated EKF and returns it. It accepts the following parameters: - m: dynamical system model - ic: initial condition of the filter - q: state a.k.a. process noise - r: output a.k.a. measurement noise - n: number of update iterations 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 number of update iterations is given: n must be non-negative

func (*IEKF) Run

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

Run runs one step of IEKF 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.

func (*IEKF) Update

func (k *IEKF) 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 of x. It returns error if either invalid state was supplied or if it fails to calculate system output estimate.

type JacFunc

type JacFunc func(u mat.Vector) func(y, x []float64)

JacFunc defines jacobian function to calculate Jacobian matrix