add input & disturbance vector dimension as per modern control theory

Author: soypat

filter.go

@@ -6,22 +6,24 @@ import (
 
 // Filter is a dynamical system filter.
 type Filter interface {
-	// Predict estimates the next internal state of the system
-	Predict(mat.Vector, mat.Vector) (Estimate, error)
-	// Update updates the system state based on external measurement
-	Update(mat.Vector, mat.Vector, mat.Vector) (Estimate, error)
+	// Predict returns the expected change in internal state
+	Predict(x, u mat.Vector) (Estimate, error)
+	// Update returns estimated system state based on external measurement ym.
+	Update(x, u, ym mat.Vector) (Estimate, error)
 }
 
 // Propagator propagates internal state of the system to the next step
 type Propagator interface {
-	// Propagate propagates internal state of the system to the next step
-	Propagate(mat.Vector, mat.Vector, mat.Vector) (mat.Vector, error)
+	// Propagate propagates internal state of the system to the next step.
+	// x is starting state, u is input vector, and z is disturbance input
+	Propagate(x, u, z mat.Vector) (mat.Vector, error)
 }
 
 // Observer observes external state (output) of the system
 type Observer interface {
-	// Observe observes external state of the system
-	Observe(mat.Vector, mat.Vector, mat.Vector) (mat.Vector, error)
+	// Observe observes external state of the system.
+	// Result for a linear system would be y=C*x+D*u+wn (last term is measurement noise)
+	Observe(x, u, wn mat.Vector) (y mat.Vector, err error)
 }
 
 // Model is a model of a dynamical system
@@ -30,8 +32,15 @@ type Model interface {
 	Propagator
 	// Observer is system observer
 	Observer
-	// Dims returns input and output dimensions of the model
-	Dims() (in int, out int)
+	// Dims returns the dimension of state vector, input vector,
+	// output (measurements, written as y) vector and disturbance vector (only dynamical systems).
+	// Below are dimension of matrices as returned by Dims() (row,column)
+	//  nx, nx = A.Dims()
+	//  nx, nu = B.Dims()
+	//  ny, nx = C.Dims()
+	//  ny, nu = D.Dims()
+	//  nx, nz = E.Dims()
+	Dims() (nx, nu, ny, nz int)
 }
 
 // Smoother is a filter smoother
@@ -45,14 +54,15 @@ type Smoother interface {
 type DiscreteModel interface {
 	// Model is a model of a dynamical system
 	Model
-	// StateMatrix returns state propagation matrix
-	StateMatrix() mat.Matrix
-	// StateCtlMatrix returns state propagation control matrix
-	StateCtlMatrix() mat.Matrix
+	// SystemMatrix returns state propagation matrix
+	SystemMatrix() (A mat.Matrix) // TODO rename to SystemMatrix
+	// ControlMatrix returns state propagation control matrix
+	ControlMatrix() (B mat.Matrix) // TODO rename to ControlMatrix
 	// OutputMatrix returns observation matrix
-	OutputMatrix() mat.Matrix
-	// OutputCtlMatrix returns observation control matrix
-	OutputCtlMatrix() mat.Matrix
+	OutputMatrix() (C mat.Matrix)
+	// FeedForwardMatrix returns observation control matrix
+	FeedForwardMatrix() (D mat.Matrix) // TODO Rename to FeedMatrix/FeedForwardMatrix
+	// TODO DisturbanceMatrix
 }
 
 // InitCond is initial state condition of the filter

go.mod

@@ -1,9 +1,8 @@
-module github.com/soypat/go-estimate
+module github.com/milosgajdos/go-estimate
 
 go 1.14
 
 require (
-	github.com/milosgajdos/go-estimate v0.0.1
 	github.com/milosgajdos/matrix v0.0.1
 	github.com/stretchr/testify v1.6.1
 	golang.org/x/exp v0.0.0-20190125153040-c74c464bbbf2

go.sum

@@ -8,8 +8,6 @@ github.com/golang/freetype v0.0.0-20170609003504-e2365dfdc4a0 h1:DACJavvAHhabrF0
 github.com/golang/freetype v0.0.0-20170609003504-e2365dfdc4a0/go.mod h1:E/TSTwGwJL78qG/PmXZO1EjYhfJinVAhrmmHX6Z8B9k=
 github.com/jung-kurt/gofpdf v1.0.3-0.20190309125859-24315acbbda5 h1:PJr+ZMXIecYc1Ey2zucXdR73SMBtgjPgwa31099IMv0=
 github.com/jung-kurt/gofpdf v1.0.3-0.20190309125859-24315acbbda5/go.mod h1:7Id9E/uU8ce6rXgefFLlgrJj/GYY22cpxn+r32jIOes=
-github.com/milosgajdos/go-estimate v0.0.1 h1:Z8h+bYKK3hZcZNKlt+XovQZyvj/+hz3ovguIEi9jkT4=
-github.com/milosgajdos/go-estimate v0.0.1/go.mod h1:Prob5p2Ahwlkuoo/7y25uLj3vmjoJ8UiLuCd3p5PHfs=
 github.com/milosgajdos/matrix v0.0.1 h1:zDV91rki6Y6W1yMiSoCpPIKYLoUf0DyEvCA+SloKo1Q=
 github.com/milosgajdos/matrix v0.0.1/go.mod h1:nwsbBAQ4Z39KwGp0YbTK6gnuM9bFyVmwGJKizDeI7BY=
 github.com/pmezard/go-difflib v1.0.0 h1:4DBwDE0NGyQoBHbLQYPwSUPoCMWR5BEzIk/f1lZbAQM=

kalman/ekf/ekf.go

@@ -50,30 +50,30 @@ type EKF struct {
 // - invalid state or output noise is given: noise covariance must either be nil or match the model dimensions
 func New(m filter.Model, init filter.InitCond, q, r filter.Noise) (*EKF, error) {
 	// size of the input and output vectors
-	in, out := m.Dims()
-	if in <= 0 || out <= 0 {
-		return nil, fmt.Errorf("Invalid model dimensions: [%d x %d]", in, out)
+	nx, _, ny, _ := m.Dims()
+	if nx <= 0 || ny <= 0 {
+		return nil, fmt.Errorf("invalid model dimensions: [%d x %d]", nx, ny)
 	}
 
 	if q != nil {
-		if q.Cov().Symmetric() != in {
-			return nil, fmt.Errorf("Invalid state noise dimension: %d", q.Cov().Symmetric())
+		if q.Cov().Symmetric() != nx {
+			return nil, fmt.Errorf("invalid state noise dimension: %d", q.Cov().Symmetric())
 		}
 	} else {
 		q, _ = noise.NewNone()
 	}
 
 	if r != nil {
-		if r.Cov().Symmetric() != out {
-			return nil, fmt.Errorf("Invalid output noise dimension: %d", r.Cov().Symmetric())
+		if r.Cov().Symmetric() != ny {
+			return nil, fmt.Errorf("invalid output noise dimension: %d", r.Cov().Symmetric())
 		}
 	} else {
 		r, _ = noise.NewNone()
 	}
 
 	// propagation Jacobian
 	fJacFn := func(u mat.Vector) func([]float64, []float64) {
-		q, _ := noise.NewZero(in)
+		q, _ := noise.NewZero(nx)
 
 		return func(xOut, xNow []float64) {
 			x := mat.NewVecDense(len(xNow), xNow)
@@ -87,11 +87,11 @@ func New(m filter.Model, init filter.InitCond, q, r filter.Noise) (*EKF, error)
 			}
 		}
 	}
-	f := mat.NewDense(in, in, nil)
+	f := mat.NewDense(nx, nx, nil)
 
 	// observation Jacobian
 	hJacFn := func(u mat.Vector) func([]float64, []float64) {
-		r, _ := noise.NewZero(out)
+		r, _ := noise.NewZero(ny)
 
 		return func(y, xNow []float64) {
 			x := mat.NewVecDense(len(xNow), xNow)
@@ -106,7 +106,7 @@ func New(m filter.Model, init filter.InitCond, q, r filter.Noise) (*EKF, error)
 			}
 		}
 	}
-	h := mat.NewDense(out, in, nil)
+	h := mat.NewDense(ny, nx, nil)
 
 	// initialize covariance matrix to initial condition covariance
 	p := mat.NewSymDense(init.Cov().Symmetric(), nil)
@@ -116,10 +116,10 @@ func New(m filter.Model, init filter.InitCond, q, r filter.Noise) (*EKF, error)
 	pNext := mat.NewSymDense(init.Cov().Symmetric(), nil)
 
 	// innovation vector
-	inn := mat.NewVecDense(out, nil)
+	inn := mat.NewVecDense(ny, nil)
 
 	// kalman gain
-	k := mat.NewDense(in, out, nil)
+	k := mat.NewDense(nx, ny, nil)
 
 	return &EKF{
 		m:      m,
@@ -143,7 +143,7 @@ func (k *EKF) Predict(x, u mat.Vector) (filter.Estimate, error) {
 	// propagate input state to the next step
 	xNext, err := k.m.Propagate(x, u, k.q.Sample())
 	if err != nil {
-		return nil, fmt.Errorf("System state propagation failed: %v", err)
+		return nil, fmt.Errorf("system state propagation failed: %v", err)
 	}
 
 	// calculate propagation Jacobian matrix
@@ -174,16 +174,16 @@ func (k *EKF) Predict(x, u 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.
 func (k *EKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
-	in, out := k.m.Dims()
+	nx, _, ny, _ := k.m.Dims()
 
-	if z.Len() != out {
-		return nil, fmt.Errorf("Invalid measurement supplied: %v", z)
+	if z.Len() != ny {
+		return nil, fmt.Errorf("invalid measurement supplied: %v", z)
 	}
 
 	// observe system output in the next step
 	y, err := k.m.Observe(x, u, k.r.Sample())
 	if err != nil {
-		return nil, fmt.Errorf("Failed to observe system output: %v", err)
+		return nil, fmt.Errorf("failed to observe system output: %v", err)
 	}
 
 	// calculate observation Jacobian matrix
@@ -192,8 +192,8 @@ func (k *EKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
 		Concurrent: true,
 	})
 
-	pxy := mat.NewDense(in, out, nil)
-	pyy := mat.NewDense(out, out, nil)
+	pxy := mat.NewDense(nx, ny, nil)
+	pyy := mat.NewDense(ny, ny, nil)
 
 	// P*H'
 	pxy.Mul(k.pNext, k.h.T())
@@ -209,7 +209,7 @@ func (k *EKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
 	// calculate Kalman gain
 	pyyInv := &mat.Dense{}
 	if err := pyyInv.Inverse(pyy); err != nil {
-		return nil, fmt.Errorf("Failed to calculat Pyy inverse: %v", err)
+		return nil, fmt.Errorf("failed to calculat Pyy inverse: %v", err)
 	}
 	gain := &mat.Dense{}
 	gain.Mul(pxy, pyyInv)
@@ -257,8 +257,8 @@ func (k *EKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
 	k.inn.CopyVec(inn)
 	k.k.Copy(gain)
 	// update EKF covariance matrix
-	for i := 0; i < in; i++ {
-		for j := i; j < in; j++ {
+	for i := 0; i < nx; i++ {
+		for j := i; j < nx; j++ {
 			k.p.SetSym(i, j, pCorr.At(i, j))
 		}
 	}
@@ -310,11 +310,11 @@ func (k *EKF) Cov() mat.Symmetric {
 // It returns error if either cov is nil or its dimensions are not the same as EKF covariance dimensions.
 func (k *EKF) SetCov(cov mat.Symmetric) error {
 	if cov == nil {
-		return fmt.Errorf("Invalid covariance matrix: %v", cov)
+		return fmt.Errorf("invalid covariance matrix: %v", cov)
 	}
 
 	if cov.Symmetric() != k.p.Symmetric() {
-		return fmt.Errorf("Invalid covariance matrix dims: [%d x %d]", cov.Symmetric(), cov.Symmetric())
+		return fmt.Errorf("invalid covariance matrix dims: [%d x %d]", cov.Symmetric(), cov.Symmetric())
 	}
 
 	k.p.CopySym(cov)

kalman/ekf/ekf_test.go

@@ -15,8 +15,8 @@ type invalidModel struct {
 	filter.Model
 }
 
-func (m *invalidModel) Dims() (int, int) {
-	return -10, 8
+func (m *invalidModel) Dims() (nx, nu, ny, nz int) {
+	return -10, 0, 8, 0
 }
 
 var (

kalman/ekf/iekf.go

@@ -31,7 +31,7 @@ type IEKF struct {
 // - invalid number of update iterations is given: n must be non-negative
 func NewIter(m filter.Model, ic filter.InitCond, q, r filter.Noise, n int) (*IEKF, error) {
 	if n <= 0 {
-		return nil, fmt.Errorf("Invalid number of update iterations: %d", n)
+		return nil, fmt.Errorf("invalid number of update iterations: %d", n)
 	}
 
 	// IEKF is EKF which uses iterating updates
@@ -49,20 +49,20 @@ func NewIter(m filter.Model, ic filter.InitCond, q, r filter.Noise, n int) (*IEK
 // 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.
 func (k *IEKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
-	in, out := k.m.Dims()
+	_nx, _, _ny, _ := k.m.Dims()
 
-	if z.Len() != out {
-		return nil, fmt.Errorf("Invalid measurement supplied: %v", z)
+	if z.Len() != _ny {
+		return nil, fmt.Errorf("invalid measurement supplied: %v", z)
 	}
 
 	// observe system output in the next step
 	y, err := k.m.Observe(x, u, k.OutputNoise().Sample())
 	if err != nil {
-		return nil, fmt.Errorf("Failed to observe system output: %v", err)
+		return nil, fmt.Errorf("failed to observe system output: %v", err)
 	}
 
-	pxy := mat.NewDense(in, out, nil)
-	pyy := mat.NewDense(out, out, nil)
+	pxy := mat.NewDense(_nx, _ny, nil)
+	pyy := mat.NewDense(_ny, _ny, nil)
 
 	// innovation vector
 	inn := &mat.VecDense{}
@@ -96,7 +96,7 @@ func (k *IEKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
 		// calculate Kalman gain
 		pyyInv := &mat.Dense{}
 		if err := pyyInv.Inverse(pyy); err != nil {
-			return nil, fmt.Errorf("Failed to calculat Pyy inverse: %v", err)
+			return nil, fmt.Errorf("failed to calculat Pyy inverse: %v", err)
 		}
 		gain.Mul(pxy, pyyInv)
 
@@ -139,8 +139,8 @@ func (k *IEKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
 	k.inn.CopyVec(inn)
 	k.k.Copy(gain)
 	// update EKF covariance matrix
-	for i := 0; i < in; i++ {
-		for j := i; j < in; j++ {
+	for i := 0; i < _nx; i++ {
+		for j := i; j < _nx; j++ {
 			k.p.SetSym(i, j, pCorr.At(i, j))
 		}
 	}