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)
}