Implements interface changes for #48

Author: ChristopherRabotin

chisquare.go

@@ -13,7 +13,7 @@ import (
 // of both the NEES and NIS tests.
 // Returns NEESmeans, NISmeans and an error if applicable
 // TODO: Change order of parameters.
-func NewChiSquare(kf KalmanFilter, runs MonteCarloRuns, controls []*mat64.Vector, withNEES, withNIS bool) ([]float64, []float64, error) {
+func NewChiSquare(kf LDKF, runs MonteCarloRuns, controls []*mat64.Vector, withNEES, withNIS bool) ([]float64, []float64, error) {
 	if !withNEES && !withNIS {
 		return nil, nil, errors.New("Chi Square requires either NEES or NIS or both")
 	}

hybrid.go

@@ -90,18 +90,18 @@ func (kf *HybridKF) PreparePNT(Γ *mat64.Dense) {
 
 // Update computes a full time and measurement update.
 // Will return an error if the KF is locked (call Prepare to unlock).
-func (kf *HybridKF) Update(realObservation, computedObservation *mat64.Vector) (est *HybridKFEstimate, err error) {
+func (kf *HybridKF) Update(realObservation, computedObservation *mat64.Vector) (est Estimate, err error) {
 	return kf.fullUpdate(false, realObservation, computedObservation)
 }
 
 // Predict computes only the time update (or prediction).
 // Will return an error if the KF is locked (call Prepare to unlock).
-func (kf *HybridKF) Predict() (est *HybridKFEstimate, err error) {
+func (kf *HybridKF) Predict() (est Estimate, err error) {
 	return kf.fullUpdate(true, nil, nil)
 }
 
 // fullUpdate performs all the steps of an update and allows to stop right after the pure prediction (or time update) step.
-func (kf *HybridKF) fullUpdate(purePrediction bool, realObservation, computedObservation *mat64.Vector) (est *HybridKFEstimate, err error) {
+func (kf *HybridKF) fullUpdate(purePrediction bool, realObservation, computedObservation *mat64.Vector) (est Estimate, err error) {
 	if kf.locked {
 		return nil, errors.New("kf is locked (call Prepare() first)")
 	}
@@ -135,7 +135,7 @@ func (kf *HybridKF) fullUpdate(purePrediction bool, realObservation, computedObs
 			return nil, symerr
 		}
 		est = &HybridKFEstimate{kf.Φ, kf.Γ, &xBar, mat64.NewVector(kf.measSize, nil), mat64.NewVector(kf.measSize, nil), mat64.NewVector(kf.measSize, nil), PBarSym, PBarSym, mat64.NewDense(1, 1, nil)}
-		kf.prevEst = *est
+		kf.prevEst = est.(HybridKFEstimate)
 		kf.step++
 		kf.sncEnabled = false
 		kf.locked = true
@@ -196,7 +196,7 @@ func (kf *HybridKF) fullUpdate(purePrediction bool, realObservation, computedObs
 		Γ = mat64.DenseCopyOf(kf.Γ)
 	}
 	est = &HybridKFEstimate{&Φ, Γ, &xHat, realObservation, &innov, &y, PSym, PBarSym, &K}
-	kf.prevEst = *est
+	kf.prevEst = est.(HybridKFEstimate)
 	kf.step++
 	kf.sncEnabled = false
 	kf.locked = true

hybrid_test.go

@@ -223,10 +223,11 @@ func hybridFullODExample(ekfTrigger int, ekfDisableTime, sncDisableTime float64,
 			}
 			// There is no truth measurement here, let's only predict the KF covariance.
 			kf.Prepare(state.Φ, nil)
-			est, perr := kf.Predict()
+			estI, perr := kf.Predict()
 			if perr != nil {
 				t.Fatalf("[ERR!] (#%04d)\n%s", measNo, perr)
 			}
+			est := estI.(*HybridKFEstimate)
 			if smoothing {
 				// Save to history in order to perform smoothing.
 				estHistory[stateNo-1] = est
@@ -307,11 +308,11 @@ func hybridFullODExample(ekfTrigger int, ekfDisableTime, sncDisableTime float64,
 				kf.PreparePNT(Γ)
 			}
 		}
-		est, err := kf.Update(measurement.StateVector(), computedObservation.StateVector())
+		estI, err := kf.Update(measurement.StateVector(), computedObservation.StateVector())
 		if err != nil {
 			t.Fatalf("[ERR!] %s", err)
 		}
-
+		est := estI.(*HybridKFEstimate)
 		if !est.IsWithin2σ() {
 			t.Logf("[Not within 2-sigma] %s", est)
 		}

kalman.go

@@ -2,8 +2,8 @@ package gokalman
 
 import "github.com/gonum/matrix/mat64"
 
-// KalmanFilter defines a general KF.
-type KalmanFilter interface {
+// LDKF defines a linear dynamics Kalman Filter.
+type LDKF interface {
 	Update(measurement, control *mat64.Vector) (Estimate, error)
 	GetNoise() Noise
 	GetStateTransition() mat64.Matrix
@@ -17,6 +17,14 @@ type KalmanFilter interface {
 	String() string
 }
 
+// NLDKF defines a non-linear dynamics Kalman Filter.
+// Operates and is architectured slightly differently than LDKF.
+type NLDKF interface {
+	Prepare(Φ, Htilde *mat64.Dense)
+	Predict() (est Estimate, err error)
+	Update(realObservation, computedObservation *mat64.Vector) (est Estimate, err error)
+}
+
 // Estimate is returned from Update() in any KF.
 // This allows to avoid some computations in other filters, e.g. in the Information filter.
 type Estimate interface {

kalman_test.go

@@ -2,17 +2,24 @@ package gokalman
 
 import "testing"
 
-func TestImplementsKF(t *testing.T) {
-	implements := func(KalmanFilter) {}
+func TestImplementsLDKF(t *testing.T) {
+	implements := func(LDKF) {}
 	implements(new(Vanilla))
 	implements(new(Information))
 	implements(new(SquareRoot))
 }
 
+func TestImplementsNLDKF(t *testing.T) {
+	implements := func(NLDKF) {}
+	implements(new(SRIF))
+	implements(new(HybridKF))
+}
+
 func TestImplementsEst(t *testing.T) {
 	implements := func(Estimate) {}
 	implements(VanillaEstimate{})
 	implements(InformationEstimate{})
 	implements(SquareRootEstimate{})
 	implements(HybridKFEstimate{})
+	implements(SRIFEstimate{})
 }

srif.go

@@ -68,18 +68,18 @@ func (kf *SRIF) Prepare(Φ, Htilde *mat64.Dense) {
 
 // Update computes a full time and measurement update.
 // Will return an error if the KF is locked (call Prepare to unlock).
-func (kf *SRIF) Update(realObservation, computedObservation *mat64.Vector) (est *SRIFEstimate, err error) {
+func (kf *SRIF) Update(realObservation, computedObservation *mat64.Vector) (est Estimate, err error) {
 	return kf.fullUpdate(false, realObservation, computedObservation)
 }
 
 // Predict computes only the time update (or prediction).
 // Will return an error if the KF is locked (call Prepare to unlock).
-func (kf *SRIF) Predict() (est *SRIFEstimate, err error) {
+func (kf *SRIF) Predict() (est Estimate, err error) {
 	return kf.fullUpdate(true, nil, nil)
 }
 
 // fullUpdate performs all the steps of an update and allows to stop right after the pure prediction (or time update) step.
-func (kf *SRIF) fullUpdate(purePrediction bool, realObservation, computedObservation *mat64.Vector) (est *SRIFEstimate, err error) {
+func (kf *SRIF) fullUpdate(purePrediction bool, realObservation, computedObservation *mat64.Vector) (est Estimate, err error) {
 	if kf.locked {
 		return nil, errors.New("kf is locked (call Prepare() first)")
 	}
@@ -115,7 +115,7 @@ func (kf *SRIF) fullUpdate(purePrediction bool, realObservation, computedObserva
 	if purePrediction {
 		tmpEst := NewSRIFEstimate(kf.Φ, &bBar, mat64.NewVector(kf.measSize, nil), mat64.NewVector(kf.measSize, nil), &RBar, &RBar)
 		est = &tmpEst
-		kf.prevEst = *est
+		kf.prevEst = est.(SRIFEstimate)
 		kf.step++
 		kf.locked = true
 		return
@@ -134,7 +134,7 @@ func (kf *SRIF) fullUpdate(purePrediction bool, realObservation, computedObserva
 	}
 	tmpEst := NewSRIFEstimate(kf.Φ, bk, realObservation, &y, Rk, &RBar)
 	est = &tmpEst
-	kf.prevEst = *est
+	kf.prevEst = est.(SRIFEstimate)
 	kf.step++
 	kf.locked = true
 	return
@@ -207,7 +207,7 @@ func (e SRIFEstimate) State() *mat64.Vector {
 		e.cachedState = mat64.NewVector(rState, nil)
 		var rInv mat64.Dense
 		if err := rInv.Inverse(e.R); err != nil {
-			panic("cannot invert R!")
+			panic(fmt.Errorf("cannot invert R: %s", err))
 		}
 		e.cachedState.MulVec(&rInv, e.sqinfoState)
 	}

srif_test.go

@@ -210,7 +210,7 @@ func _SRIFFullODExample(smoothing bool, t *testing.T) {
 			}
 			if smoothing {
 				// Save to history in order to perform smoothing.
-				estHistory[stateNo-1] = est
+				estHistory[stateNo-1] = est.(*SRIFEstimate)
 				stateHistory[stateNo-1] = nil
 			} else {
 				// Stream to CSV file
@@ -237,11 +237,11 @@ func _SRIFFullODExample(smoothing bool, t *testing.T) {
 
 		Htilde := computedObservation.HTilde()
 		kf.Prepare(state.Φ, Htilde)
-		est, err := kf.Update(measurement.StateVector(), computedObservation.StateVector())
+		estI, err := kf.Update(measurement.StateVector(), computedObservation.StateVector())
 		if err != nil {
 			t.Fatalf("[ERR!] %s", err)
 		}
-
+		est := estI.(*SRIFEstimate)
 		if !est.IsWithin2σ() {
 			t.Fatalf("%s", est)
 		}