christopherrabotin / gokalman

A Kalman Filter library in go. Includes several examples in statistical orbit determination.

gokalman's Issues

Xi-squared test

Needed to fulfill question 9.

• NIS test
• NEES test
• README.md to examples/robot

Allow truth plotting with missing data

Use case: in OD there are only a few measurements, but I still need to plot the full time duration of the estimation. So there needs to be a way to plot the error from the truth but also plot the covariance all throughout the estimation (covariance can be predicted without any measurement).

Extended Kalman Filter

Monte Carlo

Allow for easy MC

Error estimate

Having an Error estimate would be the easiest way to plot errors as it would integrate directly into the CSV Exporter.

Unsented Kalman Filter

Van Loan's Method

Implement Van Loan's Method.

Custom travis-ci file

• The example folder does not need tests.
• Add Source Clear (mostly for fun) Go not supported

Orbital pertubations with J2 via STM

My space mission design library allows for pure dynamics, which is best for simulations and mission design, but not state estimation. The goal of this issue is to create an example of using a state transition matrix to compute the linearized dynamics including perturbations.

Update README.md after Interface change

Vanilla KF

Given an x0 as mat64.Vector, mu0 as mat64.Symmetric and F, G, H, Γ (Gamma) and Q all as mat64.Dense, return a Vanilla KF object which has a measurement y_{k+1} input channel and two output channels x_hat and y_hat (all of which take a mat64.Vector input).

Use NewVanilla (gokalman.NewVanilla if imported) to initialize. This will panic if dimensions do not match.

Note that mat64 refers to gonum's BLAS/LAPACK implementation of matrices, cf. https://godoc.org/github.com/gonum/matrix.

Estimate exporting interface

It shall be possible to export the data from an Estimate to a variety of plotting tools, including KST.
The best solution is probably to define an interface, but I'm not too sure how that adds value yet: the only function that would be defined in that interface probably only takes an Estimate (if so, I need to figure out how to handle the plotting of the error which requires the real a posteriori data in addition to the KF estimate, knowing that the x_{k} isn't in the Estimate -- or at least isn't in the VanillaEstimate).

Batch Kalman Filter

Square Root Information filter

Add tests for Hybrid KF

This is important to maintain a good coverage and ensure regression.

The best is possibly to write a test based on an example of orbital determination.

Vanilla Kalman *may* be incorrect

Comparing the results between the Matlab implementation and the Go implementation shows discrepancies.

Do like for IF: rewrite the equations from the code and compare. Also follow up on gonum/matrix#410 given that leads to issues for IF.

Rename interfaces

Helps for ChristopherRabotin/smd#119 .

Hybrid Classical Kalman Filter

For StatOD, we need a filter whose state transition matrix is continuous, but the state is discreet. Ugh.

Simple statistical orbit determination example

Do the example for the main project.

Generate Gaussian PDFs

Implement the equivalent of normpdf .

Have a look at gonum's stat.

Fix for Factorize signature change

Cf. gonum/matrix#404

Square Root Filter

package gokalman

import (
"fmt"
"math"
"github.com/gonum/matrix/mat64"
)

// NewSqrt returns a new Square Root KF. To get the next estimate, push to
// the MeasChan the next measurement and read from StateEst and MeasEst to get
// the next state estimate (\hat{x}{k+1}^{+}) and next measurement estimate (\hat{y}{k+1}).
// The Covar channel stores the next covariance of the system (P_{k+1}^{+}).
// Parameters:
// - x0: initial state
// - Covar0: initial covariance matrix
// - F: state update matrix
// - G: control matrix (if all zeros, then control vector will not be used)
// - H: measurement update matrix
// - noise: Noise

func NewSqrt(x0 *mat64.Vector, Covar0 mat64.Symmetric, F, G, H mat64.Matrix, noise Noise) (*SqrtFil, error) {
// Check the dimensions of each matrix to avoid errors.
if err := checkMatDims(x0, Covar0, "x0", "Covar0", rows2cols); err != nil {
return nil, err
}
if err := checkMatDims(F, Covar0, "F", "Covar0", rows2cols); err != nil {
return nil, err
}
if err := checkMatDims(H, x0, "H", "x0", cols2rows); err != nil {
return nil, err
}

// Get s0 from Covariance
s0 = LFromCholesky(Covar0)

// Populate with the initial values.
rowsH, _ := H.Dims()

// Get the new estimate
est0 := SqrtEstimate{x0, mat64.NewVector(rowsH, nil), s0, nil}
// Return the state and estimate to the SqrtFil structure.
return &SqrtFil{F, G, H, noise, !IsNil(G), est0, 0}, nil
}

// SqrtFil defines a square root kalman filter. Use NewSqrt to initialize.
type SqrtFil struct {
F mat64.Matrix
G mat64.Matrix
H mat64.Matrix
Noise Noise
needCtrl bool
prevEst SqrtEstimate
step int
}

// Prints the output.
func (kf *SqrtFil) String() string {
return fmt.Sprintf("F=%v\nG=%v\nH=%v\n%s", mat64.Formatted(kf.F, mat64.Prefix(" ")), mat64.Formatted(kf.G, mat64.Prefix(" ")), mat64.Formatted(kf.H, mat64.Prefix(" ")), kf.Noise)
}

// SetF updates the F matrix.
func (kf *SqrtFil) SetF(F mat64.Matrix) {
kf.F = F
}

// SetG updates the G matrix.
func (kf *SqrtFil) SetG(G mat64.Matrix) {
kf.G = G
}

// SetH updates the H matrix.
func (kf *SqrtFil) SetH(H mat64.Matrix) {
kf.H = H
}

// SetNoise updates the Noise.
func (kf *SqrtFil) SetNoise(n Noise) {
kf.Noise = n
}

// Update implements the KalmanFilter interface.
func (kf *SqrtFil) Update(measurement, control *mat64.Vector) (est Estimate, err error) {
// Check for matrix dimensions errors.
if err = checkMatDims(control, kf.G, "control (u)", "G", rows2cols); kf.needCtrl && err != nil {
return nil, err
}
if err = checkMatDims(measurement, kf.H, "measurement (y)", "H", rows2rows); err != nil {
return nil, err
}

// Prediction Step //
// Get xKp1Minus
var xKp1Minus, xKp1Minus1, xKp1Minus2 mat64.Vector
xKp1Minus1.MulVec(kf.F, kf.prevEst.State())
if kf.needCtrl {
xKp1Minus2.MulVec(kf.G, control)
xKp1Minus.AddVec(&xKp1Minus1, &xKp1Minus2)
} else {
xKp1Minus = xKp1Minus1
}

// Get sKplus
sKPlus = kf.prevEst.Smatrix()

// Get sKp1Minus
// Cbars Matrix
var Cbars mat64.Vector
chol_Q = LFromCholesky(&kf.Noise.ProcessMatrix())
Cbars[0].Mul(sKPlus.T(), F.T())
Cbars[1].chol_Q.T()
// QR Factorization & the Uc matrix
Uc := mat64.RFromQR(qr *Cbars)
// Get sKp1Minus from first element of QR decomposition.
sKp1Minus := Uc[0].T()

// Delta Matrix
var Delta mat64.Dense
chol_R = LFromCholesky(kf.Noise.MeasurementMatrix())
Delta[1][1] = chol_R.T()
Delta[1][2] = 0
Delta[2][1].Mul(sKp1Minus.T(), H.T())
Delta[2][2] = sKp1Minus.T()
// QR Factorization & the U_delta Matrix
U_delta := mat64.RFromQR(qr *Delta)
sqrtPyy := U_delta[1][1]
sKp1Plus := U_delta[2][2].T()
omega := U_delta[1][2].T()
Pkp1Plus = Mul(sKp1Plus, sKp1Plus.T())

// Get Kalman gain
Kkp1 := Mul(omega, Inverse(sqrtPyy))

// Measurement update
var xkp1Plus, xkp1Plus1, xkp1Plus2 mat64.Vector
xkp1Plus1.MulVec(kf.H, &xKp1Minus)
xkp1Plus1.SubVec(measurement, &xkp1Plus1)
if rX, _ := xkp1Plus1.Dims(); rX == 1 {
	// xkp1Plus1 is a scalar and mat64 won't be happy.
	var sKkp1 mat64.Dense
	sKkp1.Scale(xkp1Plus1.At(0, 0), &Kkp1)
	rGain, _ := sKkp1.Dims()
	xkp1Plus2.AddVec(sKkp1.ColView(0), mat64.NewVector(rGain, nil))
} else {
	xkp1Plus2.MulVec(&Kkp1, &xkp1Plus1)
}
xkp1Plus.AddVec(&xKp1Minus, &xkp1Plus2)
xkp1Plus.AddVec(&xkp1Plus, kf.Noise.Process(kf.step))

var Pkp1Plus, Pkp1Plus1, Kkp1H, Kkp1R, Kkp1RKkp1 mat64.Dense
Kkp1H.Mul(&Kkp1, kf.H)
n, _ := Kkp1H.Dims()
Kkp1H.Sub(Identity(n), &Kkp1H)
Pkp1Plus1.Mul(&Kkp1H, &Pkp1Minus)
Pkp1Plus.Mul(&Pkp1Plus1, Kkp1H.T())
Kkp1R.Mul(&Kkp1, kf.Noise.MeasurementMatrix())
Kkp1RKkp1.Mul(&Kkp1R, Kkp1.T())
Pkp1Plus.Add(&Pkp1Plus, &Kkp1RKkp1)

Pkp1PlusSym, err := AsSymDense(&Pkp1Plus)
if err != nil {
	return nil, err
}
est = VanillaEstimate{&xkp1Plus, &ykHat, Pkp1PlusSym, &Kkp1}
kf.prevEst = est.(VanillaEstimate)
kf.step++
return

}

// SqrtEstimate is the output of each update state of the Square Root KF.
// It implements the Estimate interface.
type SqrtEstimate struct {
state, meas *mat64.Vector
covar mat64.Symmetric
gain mat64.Matrix
}

// IsWithin2σ returns whether the estimation is within the 2σ bounds.
func (e SqrtEstimate) IsWithin2σ() bool {
for i := 0; i < e.state.Len(); i++ {
twoσ := 2 * math.Sqrt(e.covar.At(i, i))
if e.state.At(i, 0) > twoσ || e.state.At(i, 0) < -twoσ {
return false
}
}
return true
}

// State implements the Estimate interface.
func (e SqrtEstimate) State() *mat64.Vector {
return e.state
}

// Measurement implements the Estimate interface.
func (e SqrtEstimate) Measurement() *mat64.Vector {
return e.meas
}

// Smatrix implements the Estimate interface.
func (e SqrtEstimate) Smatrix() mat64.Symmetric {
return e.covar
}

// Gain the Estimate interface.
func (e SqrtEstimate) Gain() mat64.Matrix {
return e.gain
}

func (e SqrtEstimate) String() string {
state := mat64.Formatted(e.State(), mat64.Prefix(" "))
meas := mat64.Formatted(e.Measurement(), mat64.Prefix(" "))
covar := mat64.Formatted(e.Smatrix(), mat64.Prefix(" "))
gain := mat64.Formatted(e.Gain(), mat64.Prefix(" "))
return fmt.Sprintf("{\ns=%v\ny=%v\nP=%v\nK=%v\n}", state, meas, covar, gain)
}

Information filter

• Information Filter as per textbook
• The IF Estimate should contain fields and methods in such a way to avoid computing matrix inverses (that's the whole point of the IF).

Add SRIF iterator

It seems to be effectively the same thing as the smoothing process for the CKF.

Square Root Information Filter