kalman

README

kalman

Installation

1. install matrix

go get -u github.com/mrfyo/kalman

1. import it in your code

import "github.com/mrfyo/kalman"

Example

Linear Kalman Filter

package main

import (
	"fmt"

	"github.com/mrfyo/kalman"
	"github.com/mrfyo/matrix"
)

func main() {
	dimX := 4
	dimZ := 2
	dt := 1.0

	QStd := 0.001
	RStd := 0.4

	F := matrix.Builder().Row().
		Link(1, dt, 0, 0).
		Link(0, 1, 0, 0).
		Link(0, 0, 1, dt).
		Link(0, 0, 0, 1).Build()

	H := matrix.Builder().Row().
		Link(1, 0, 0, 0).
		Link(0, 0, 1, 0).Build()

	X := matrix.Builder().Col().Link(0, 1, 0, 0.5).Build()

	P := matrix.Diag([]float64{10, 4, 10, 4})

	Q := matrix.Builder().Row().
		Link(0.25*math.Pow(dt, 4), 0.5*math.Pow(dt, 3), 0, 0).
		Link(0.5*math.Pow(dt, 3), dt*dt, 0, 0).
		Link(0, 0, 0.25*math.Pow(dt, 4), 0.5*math.Pow(dt, 3)).
		Link(0, 0, 0.5*math.Pow(dt, 3), dt*dt).Build().
		ScaleMul(QStd)

	R := matrix.Eye(dimZ).ScaleMul(RStd * RStd)

	kf := kalman.NewKalmanFilter(dimX, dimZ, dt, F, H)
	kf.Init(X, P, Q, R)

	N := 20
	D := CreateTrack(N, X.GetIndex(0), X.GetIndex(1), X.GetIndex(2), X.GetIndex(3), dt, RStd, QStd)

	for i := 1; i < N; i++ {
		x := D.Get(i, 2)
		y := D.Get(i, 3)
		z := matrix.NewVector([]float64{x, y}, 1)

		kf.Predict()
		X := kf.Update(z)

        fmt.Println(X)
	}
}

Unscented Kalman Filter

func main() {
	dimX := 4
	dimZ := 2
	dt := 1.0

	QStd := 0.001
	RStd := 0.4

	fx := func(dt float64, X matrix.Matrix) matrix.Matrix {
		F := matrix.Builder().Row().
			Link(1, dt, 0, 0).
			Link(0, 1, 0, 0).
			Link(0, 0, 1, dt).
			Link(0, 0, 0, 1).Build()

		return F.Dot(X)
	}

	hx := func(dt float64, X matrix.Matrix) matrix.Matrix {
		H := matrix.Builder().Row().
			Link(1, 0, 0, 0).
			Link(0, 0, 1, 0).Build()

		return H.Dot(X)
	}

	X := matrix.Builder().Col().Link(0, 1, 0, 0.5).Build()

	P := matrix.Diag([]float64{10, 4, 10, 4})

	Q := matrix.Builder().Row().
		Link(0.25*math.Pow(dt, 4), 0.5*math.Pow(dt, 3), 0, 0).
		Link(0.5*math.Pow(dt, 3), dt*dt, 0, 0).
		Link(0, 0, 0.25*math.Pow(dt, 4), 0.5*math.Pow(dt, 3)).
		Link(0, 0, 0.5*math.Pow(dt, 3), dt*dt).Build().
		ScaleMul(QStd)

	R := matrix.Eye(dimZ).ScaleMul(RStd * RStd)

	kf := kalman.NewUnscentedKalmanFilter(dimX, dimZ, dt, fx, hx)
	kf.Init(X, P, Q, R)
}

Cubature Kalman Filter

	// Unscented Kalman Filter Example
	// ...
	kf := kalman.NewCubatureKalmanFilter(dimX, dimZ, dt, fx, hx)

Documentation

Index

• type CubatureKalmanFilter
  • func NewCubatureKalmanFilter(dimX int, dimZ int, dt float64, Fx FilterFun, Hx FilterFun) *CubatureKalmanFilter
  • func (kf *CubatureKalmanFilter) Init(x, P, Q, R Matrix)
  • func (kf *CubatureKalmanFilter) Predict()
  • func (kf *CubatureKalmanFilter) Update(z Matrix) Matrix
• type Filter
• type FilterFun
• type LinearKalmanFilter
  • func NewKalmanFilter(dimX int, dimZ int, dt float64, F Matrix, H Matrix) *LinearKalmanFilter
  • func (kf *LinearKalmanFilter) Init(x, P, Q, R Matrix)
  • func (kf *LinearKalmanFilter) Predict()
  • func (kf *LinearKalmanFilter) Update(z Matrix) (X Matrix)
• type Matrix
• type Shape
• type UnscentedKalmanFilter
  • func NewUnscentedKalmanFilter(dimX int, dimZ int, dt float64, Fx FilterFun, Hx FilterFun) *UnscentedKalmanFilter
  • func (kf *UnscentedKalmanFilter) Init(x, P, Q, R Matrix)
  • func (kf *UnscentedKalmanFilter) Predict()
  • func (kf *UnscentedKalmanFilter) Update(z Matrix) Matrix

Types

type CubatureKalmanFilter

type CubatureKalmanFilter struct {
	DimX   int       // n
	DimZ   int       // m
	Dt     float64   // collect duration
	Fx     FilterFun // (n, 1)
	Hx     FilterFun // (m, 1)
	X      Matrix    // (n, 1)
	P      Matrix    // (n, n)
	R      Matrix    // (m, n)
	Q      Matrix    // (n, n)
	PriorX Matrix    // (n, 1)
	PriorP Matrix    // (n, n)
}

func NewCubatureKalmanFilter

func NewCubatureKalmanFilter(dimX int, dimZ int, dt float64, Fx FilterFun, Hx FilterFun) *CubatureKalmanFilter

func (*CubatureKalmanFilter) Init

func (kf *CubatureKalmanFilter) Init(x, P, Q, R Matrix)

func (*CubatureKalmanFilter) Predict

func (kf *CubatureKalmanFilter) Predict()

func (*CubatureKalmanFilter) Update

func (kf *CubatureKalmanFilter) Update(z Matrix) Matrix

type Filter

type Filter interface {
	Predict()
	Update(z Matrix) (X Matrix)
}

type FilterFun

type FilterFun func(dt float64, X Matrix) Matrix

type LinearKalmanFilter

type LinearKalmanFilter struct {
	DimX   int
	DimZ   int
	Dt     float64
	F      Matrix
	H      Matrix
	X      Matrix
	P      Matrix
	R      Matrix
	Q      Matrix
	PriorX Matrix
	PriorP Matrix
}

func NewKalmanFilter

func NewKalmanFilter(dimX int, dimZ int, dt float64, F Matrix, H Matrix) *LinearKalmanFilter

func (*LinearKalmanFilter) Init

func (kf *LinearKalmanFilter) Init(x, P, Q, R Matrix)

func (*LinearKalmanFilter) Predict

func (kf *LinearKalmanFilter) Predict()

func (*LinearKalmanFilter) Update

func (kf *LinearKalmanFilter) Update(z Matrix) (X Matrix)

type Matrix

type Matrix = mat.Matrix

type Shape

type Shape = mat.Shape

type UnscentedKalmanFilter

type UnscentedKalmanFilter struct {
	DimX        int       // n
	DimZ        int       // m
	Dt          float64   // collect duration
	Fx          FilterFun // (n, 1)
	Hx          FilterFun // (m, 1)
	X           Matrix    // (n, 1)
	P           Matrix    // (n, n)
	R           Matrix    // (m, n)
	Q           Matrix    // (n, n)
	PriorX      Matrix    // (n, 1)
	PriorP      Matrix    // (n, n)
	Wm          Matrix    // (1, 2*n+1)
	Wc          Matrix    // (1, 2*n+1)
	SigmaXs     Matrix    // (n, 2*n+1)
	SigmaParams []float64 //(alpha, kappa, beta)
}

func NewUnscentedKalmanFilter

func NewUnscentedKalmanFilter(dimX int, dimZ int, dt float64, Fx FilterFun, Hx FilterFun) *UnscentedKalmanFilter

func (*UnscentedKalmanFilter) Init

func (kf *UnscentedKalmanFilter) Init(x, P, Q, R Matrix)

func (*UnscentedKalmanFilter) Predict

func (kf *UnscentedKalmanFilter) Predict()

func (*UnscentedKalmanFilter) Update

func (kf *UnscentedKalmanFilter) Update(z Matrix) Matrix