740 lines
28 KiB
Go
740 lines
28 KiB
Go
|
// Copyright (c) 2026 Petr Balvín <opensource@petrbalvin.org> (https://petrbalvin.org)
|
|||
|
|
// SPDX-License-Identifier: MIT
|
|||
|
|
|
|||
|
|
package signal
|
|||
|
|
|
|||
|
|
import (
|
|||
|
|
"errors"
|
|||
|
|
"math"
|
|||
|
|
"strings"
|
|||
|
|
"testing"
|
|||
|
|
|
|||
|
|
"sourcedock.dev/petrbalvin/tensor/internal/core"
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
// scalarKFRun is the scalar Kalman recursion written out by hand, the
|
|||
|
|
// closed-form reference the library's matrix machinery is pinned
|
|||
|
|
// against: the same predict-and-correct cycle in plain arithmetic.
|
|||
|
|
func scalarKFRun(z []float64, f, q, h, r, x0, p0 float64) (xs, ps, inns, ss []float64, logLik float64) {
|
|||
|
|
x, p := x0, p0
|
|||
|
|
xs = make([]float64, len(z))
|
|||
|
|
ps = make([]float64, len(z))
|
|||
|
|
inns = make([]float64, len(z))
|
|||
|
|
ss = make([]float64, len(z))
|
|||
|
|
for t, zt := range z {
|
|||
|
|
xp := f * x
|
|||
|
|
pp := f*p*f + q
|
|||
|
|
s := h*pp*h + r
|
|||
|
|
k := pp * h / s
|
|||
|
|
inn := zt - h*xp
|
|||
|
|
x = xp + k*inn
|
|||
|
|
p = pp - k*s*k // the standard form: (1−k·h)·pp
|
|||
|
|
xs[t], ps[t], inns[t], ss[t] = x, p, inn, s
|
|||
|
|
logLik += -0.5 * (math.Log(2*math.Pi*s) + inn*inn/s)
|
|||
|
|
}
|
|||
|
|
return xs, ps, inns, ss, logLik
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// cholPD reports whether a symmetric row-major block factors as
|
|||
|
|
// positive definite.
|
|||
|
|
func cholPD(a []float64, n int) bool {
|
|||
|
|
l := make([]float64, n*n)
|
|||
|
|
for i := range n {
|
|||
|
|
for j := range i + 1 {
|
|||
|
|
total := a[i*n+j]
|
|||
|
|
for k := range j {
|
|||
|
|
total -= l[i*n+k] * l[j*n+k]
|
|||
|
|
}
|
|||
|
|
if i == j {
|
|||
|
|
if !(total > 0) {
|
|||
|
|
return false
|
|||
|
|
}
|
|||
|
|
l[i*n+i] = math.Sqrt(total)
|
|||
|
|
} else {
|
|||
|
|
l[i*n+j] = total / l[j*n+j]
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return true
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// kalmanResultBlock reads one (rows × cols) block out of a result
|
|||
|
|
// stack at step t.
|
|||
|
|
func kalmanResultBlock(t *testing.T, a *core.Array, step, rows, cols int) []float64 {
|
|||
|
|
t.Helper()
|
|||
|
|
block := make([]float64, rows*cols)
|
|||
|
|
for i := range rows * cols {
|
|||
|
|
block[i] = a.FloatAt(step*rows*cols + i)
|
|||
|
|
}
|
|||
|
|
return block
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// TestKalmanScalarSteadyState pins the Riccati recursion against the
|
|||
|
|
// scalar steady state solved by hand: for x_{t+1} = x_t + w,
|
|||
|
|
// z_t = x_t + v with unit variances the fixed point of p⁻ = p + 1,
|
|||
|
|
// p = p⁻·r/(p⁻ + r) is the golden ratio, and the filtered covariance
|
|||
|
|
// settles on its reciprocal.
|
|||
|
|
func TestKalmanScalarSteadyState(t *testing.T) {
|
|||
|
|
const n = 400
|
|||
|
|
g := core.NewGenerator(3)
|
|||
|
|
z := make([]float64, n)
|
|||
|
|
for i := range z {
|
|||
|
|
z[i] = g.NormalUnit()
|
|||
|
|
}
|
|||
|
|
f, err := KalmanFilter(mustFloats(t, z), mustFloats(t, []float64{1}, 1, 1), mustFloats(t, []float64{1}, 1, 1),
|
|||
|
|
KalmanOptions{ProcessNoise: mustFloats(t, []float64{1}, 1, 1), MeasurementNoise: mustFloats(t, []float64{1}, 1, 1)})
|
|||
|
|
if err != nil {
|
|||
|
|
t.Fatalf("KalmanFilter: %v", err)
|
|||
|
|
}
|
|||
|
|
wantP := (math.Sqrt(5) - 1) / 2 // 1/φ
|
|||
|
|
if got := f.Covariances.FloatAt(n - 1); math.Abs(got-wantP) > 1e-9 {
|
|||
|
|
t.Fatalf("steady filtered covariance = %.12f, want %.12f", got, wantP)
|
|||
|
|
}
|
|||
|
|
// The state path must match the hand-run scalar recursion.
|
|||
|
|
xs, _, _, _, _ := scalarKFRun(z, 1, 1, 1, 1, 0, 1)
|
|||
|
|
if got, want := f.States.FloatAt(n-1), xs[n-1]; math.Abs(got-want) > 1e-9 {
|
|||
|
|
t.Fatalf("steady filtered state = %.12f, hand recursion %.12f", got, want)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// TestKalmanAR1Reference pins the filtered mean of a scalar AR(1) plus
|
|||
|
|
// noise against the closed-form recursion, and the accumulated
|
|||
|
|
// log-likelihood against the direct sum of the Gaussian log densities
|
|||
|
|
// of the innovations and innovation variances the result itself
|
|||
|
|
// publishes.
|
|||
|
|
func TestKalmanAR1Reference(t *testing.T) {
|
|||
|
|
const (
|
|||
|
|
phi = 0.7
|
|||
|
|
q = 0.04
|
|||
|
|
r = 0.25
|
|||
|
|
n = 80
|
|||
|
|
)
|
|||
|
|
g := core.NewGenerator(5)
|
|||
|
|
z := make([]float64, n)
|
|||
|
|
for i := range z {
|
|||
|
|
z[i] = g.NormalUnit()
|
|||
|
|
}
|
|||
|
|
f, err := KalmanFilter(mustFloats(t, z), mustFloats(t, []float64{phi}, 1, 1),
|
|||
|
|
mustFloats(t, []float64{1}, 1, 1),
|
|||
|
|
KalmanOptions{
|
|||
|
|
ProcessNoise: mustFloats(t, []float64{q}, 1, 1),
|
|||
|
|
MeasurementNoise: mustFloats(t, []float64{r}, 1, 1),
|
|||
|
|
})
|
|||
|
|
if err != nil {
|
|||
|
|
t.Fatalf("KalmanFilter: %v", err)
|
|||
|
|
}
|
|||
|
|
xs, ps, inns, ss, wantLL := scalarKFRun(z, phi, q, 1, r, 0, 1)
|
|||
|
|
for step := range n {
|
|||
|
|
if got := f.States.FloatAt(step); math.Abs(got-xs[step]) > 1e-9 {
|
|||
|
|
t.Fatalf("filtered mean at %d = %.12f, want %.12f", step, got, xs[step])
|
|||
|
|
}
|
|||
|
|
if got := f.Covariances.FloatAt(step); math.Abs(got-ps[step]) > 1e-9 {
|
|||
|
|
t.Fatalf("filtered variance at %d = %.12f, want %.12f", step, got, ps[step])
|
|||
|
|
}
|
|||
|
|
if got := f.Innovations.FloatAt(step); math.Abs(got-inns[step]) > 1e-9 {
|
|||
|
|
t.Fatalf("innovation at %d = %.12f, want %.12f", step, got, inns[step])
|
|||
|
|
}
|
|||
|
|
if got := f.InnovationCovariances.FloatAt(step); math.Abs(got-ss[step]) > 1e-9 {
|
|||
|
|
t.Fatalf("innovation variance at %d = %.12f, want %.12f", step, got, ss[step])
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if math.Abs(f.LogLikelihood-wantLL) > 1e-7 {
|
|||
|
|
t.Fatalf("log-likelihood = %.10f, closed form %.10f", f.LogLikelihood, wantLL)
|
|||
|
|
}
|
|||
|
|
// The published likelihood must be the sum of the published
|
|||
|
|
// innovations' own Gaussian log densities.
|
|||
|
|
total := 0.0
|
|||
|
|
for t := range n {
|
|||
|
|
inn := f.Innovations.FloatAt(t)
|
|||
|
|
s := f.InnovationCovariances.FloatAt(t)
|
|||
|
|
total += -0.5 * (math.Log(2*math.Pi*s) + inn*inn/s)
|
|||
|
|
}
|
|||
|
|
if math.Abs(total-f.LogLikelihood) > 1e-9 {
|
|||
|
|
t.Fatalf("log-likelihood %.12f is not the innovations' own sum %.12f", f.LogLikelihood, total)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// TestKalmanMatchesUKFLinear runs the unscented filter on an exactly
|
|||
|
|
// linear model: the sigma-point transforms are exact there, so the
|
|||
|
|
// UKF must degenerate to the Kalman answer to rounding.
|
|||
|
|
func TestKalmanMatchesUKFLinear(t *testing.T) {
|
|||
|
|
const n = 120
|
|||
|
|
fMat := mustFloats(t, []float64{1, 1, 0, 1}, 2, 2)
|
|||
|
|
hMat := mustFloats(t, []float64{1, 0}, 1, 2)
|
|||
|
|
opts := KalmanOptions{
|
|||
|
|
InitialState: mustFloats(t, []float64{1, 0}),
|
|||
|
|
InitialCovariance: mustFloats(t, []float64{4, 0, 0, 1}, 2, 2),
|
|||
|
|
ProcessNoise: mustFloats(t, []float64{1e-4, 0, 0, 0.01}, 2, 2),
|
|||
|
|
MeasurementNoise: mustFloats(t, []float64{0.25}, 1, 1),
|
|||
|
|
}
|
|||
|
|
g := core.NewGenerator(9)
|
|||
|
|
z := make([]float64, n)
|
|||
|
|
for i := range z {
|
|||
|
|
z[i] = g.NormalUnit()
|
|||
|
|
}
|
|||
|
|
zArr := mustFloats(t, z)
|
|||
|
|
kf, err := KalmanFilter(zArr, fMat, hMat, opts)
|
|||
|
|
if err != nil {
|
|||
|
|
t.Fatalf("KalmanFilter: %v", err)
|
|||
|
|
}
|
|||
|
|
linear := func(m *core.Array) StateFunc {
|
|||
|
|
return func(x *core.Array) (*core.Array, error) {
|
|||
|
|
rows := m.Len() / x.Len()
|
|||
|
|
out := make([]float64, rows)
|
|||
|
|
for i := range out {
|
|||
|
|
total := 0.0
|
|||
|
|
for j := range x.Len() {
|
|||
|
|
total += m.FloatAt(i*x.Len()+j) * x.FloatAt(j)
|
|||
|
|
}
|
|||
|
|
out[i] = total
|
|||
|
|
}
|
|||
|
|
return core.FromFloats(out, len(out))
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
ukf, err := UnscentedKalmanFilter(zArr, linear(fMat), linear(hMat), opts)
|
|||
|
|
if err != nil {
|
|||
|
|
t.Fatalf("UnscentedKalmanFilter: %v", err)
|
|||
|
|
}
|
|||
|
|
for step := range n {
|
|||
|
|
for i := range 2 {
|
|||
|
|
got, want := ukf.States.FloatAt(step*2+i), kf.States.FloatAt(step*2+i)
|
|||
|
|
if math.Abs(got-want) > 1e-8 {
|
|||
|
|
t.Fatalf("UKF state (%d, %d) = %.10f against KF %.10f", step, i, got, want)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for i := range 4 {
|
|||
|
|
got, want := ukf.Covariances.FloatAt(step*4+i), kf.Covariances.FloatAt(step*4+i)
|
|||
|
|
if math.Abs(got-want) > 1e-8 {
|
|||
|
|
t.Fatalf("UKF covariance (%d, %d) = %.10f against KF %.10f", step, i, got, want)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if math.Abs(ukf.LogLikelihood-kf.LogLikelihood) > 1e-6 {
|
|||
|
|
t.Fatalf("UKF log-likelihood %.10f against KF %.10f", ukf.LogLikelihood, kf.LogLikelihood)
|
|||
|
|
}
|
|||
|
|
// The extended filter with central-difference Jacobians sees the
|
|||
|
|
// same linear maps exactly, so it must land on the KF too.
|
|||
|
|
ekf, err := ExtendedKalmanFilter(zArr, linear(fMat), linear(hMat), opts)
|
|||
|
|
if err != nil {
|
|||
|
|
t.Fatalf("ExtendedKalmanFilter: %v", err)
|
|||
|
|
}
|
|||
|
|
for step := range n {
|
|||
|
|
for i := range 2 {
|
|||
|
|
got, want := ekf.States.FloatAt(step*2+i), kf.States.FloatAt(step*2+i)
|
|||
|
|
if math.Abs(got-want) > 1e-8 {
|
|||
|
|
t.Fatalf("EKF state (%d, %d) = %.10f against KF %.10f", step, i, got, want)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// TestUnscentedQuadraticMoment pins the sigma-point transform on a
|
|||
|
|
// genuinely nonlinear map: with x ~ N(0, 2) and f(x) = x², the exact
|
|||
|
|
// predicted second moment is 2, and with α = 1, κ = 0 the symmetric
|
|||
|
|
// sigma pair reproduces it exactly. The inert measurement noise keeps
|
|||
|
|
// the correction from moving the answer.
|
|||
|
|
func TestUnscentedQuadraticMoment(t *testing.T) {
|
|||
|
|
square := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return core.FromFloats([]float64{x.FloatAt(0) * x.FloatAt(0)}, 1)
|
|||
|
|
}
|
|||
|
|
identity := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return core.FromFloats([]float64{x.FloatAt(0)}, 1)
|
|||
|
|
}
|
|||
|
|
f, err := UnscentedKalmanFilter(mustFloats(t, []float64{0}), square, identity, KalmanOptions{
|
|||
|
|
InitialState: mustFloats(t, []float64{0}),
|
|||
|
|
InitialCovariance: mustFloats(t, []float64{2}, 1, 1),
|
|||
|
|
ProcessNoise: mustFloats(t, []float64{0}, 1, 1),
|
|||
|
|
MeasurementNoise: mustFloats(t, []float64{1e18}, 1, 1),
|
|||
|
|
SigmaAlpha: 1,
|
|||
|
|
SigmaBeta: 2,
|
|||
|
|
SigmaKappa: 0,
|
|||
|
|
})
|
|||
|
|
if err != nil {
|
|||
|
|
t.Fatalf("UnscentedKalmanFilter: %v", err)
|
|||
|
|
}
|
|||
|
|
if got, want := f.States.FloatAt(0), 2.0; math.Abs(got-want) > 1e-9 {
|
|||
|
|
t.Fatalf("predicted second moment = %.12f, want %.12f", got, want)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// TestExtendedKalmanBearingOnly tracks a constant-velocity target from
|
|||
|
|
// bearing-only measurements, the classical case where the observation
|
|||
|
|
// map is genuinely nonlinear and the posterior is not Gaussian. The
|
|||
|
|
// honest pin is Monte Carlo: a bootstrap particle filter over the same
|
|||
|
|
// model and data approximates the true posterior mean, and both the
|
|||
|
|
// extended and the unscented filter must land within a few posterior
|
|||
|
|
// standard deviations of it, and of each other.
|
|||
|
|
func TestExtendedKalmanBearingOnly(t *testing.T) {
|
|||
|
|
const (
|
|||
|
|
n = 60
|
|||
|
|
sigmaB = 0.005
|
|||
|
|
truthX0 = 2000.0
|
|||
|
|
truthY0 = 100.0
|
|||
|
|
truthVX = -15.0
|
|||
|
|
truthVY = 3.0
|
|||
|
|
)
|
|||
|
|
// Truth and bearings, through the house generator.
|
|||
|
|
g := core.NewGenerator(101)
|
|||
|
|
z := make([]float64, n)
|
|||
|
|
for t := range n {
|
|||
|
|
px := truthX0 + truthVX*float64(t+1)
|
|||
|
|
py := truthY0 + truthVY*float64(t+1)
|
|||
|
|
z[t] = math.Atan2(py, px) + sigmaB*g.NormalUnit()
|
|||
|
|
}
|
|||
|
|
cv := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return core.FromFloats([]float64{
|
|||
|
|
x.FloatAt(0) + x.FloatAt(2),
|
|||
|
|
x.FloatAt(1) + x.FloatAt(3),
|
|||
|
|
x.FloatAt(2),
|
|||
|
|
x.FloatAt(3),
|
|||
|
|
}, 4)
|
|||
|
|
}
|
|||
|
|
bearing := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return core.FromFloats([]float64{math.Atan2(x.FloatAt(1), x.FloatAt(0))}, 1)
|
|||
|
|
}
|
|||
|
|
bearingJac := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
px, py := x.FloatAt(0), x.FloatAt(1)
|
|||
|
|
r2 := px*px + py*py
|
|||
|
|
return core.FromFloats([]float64{-py / r2, px / r2, 0, 0}, 1, 4)
|
|||
|
|
}
|
|||
|
|
opts := KalmanOptions{
|
|||
|
|
InitialState: mustFloats(t, []float64{1900, 80, 0, 0}),
|
|||
|
|
InitialCovariance: mustFloats(t, []float64{1e4, 0, 0, 0, 0, 1e4, 0, 0, 0, 0, 25, 0, 0, 0, 0, 25}, 4, 4),
|
|||
|
|
ProcessNoise: mustFloats(t, []float64{1e-4, 0, 0, 0, 0, 1e-4, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0.01}, 4, 4),
|
|||
|
|
MeasurementNoise: mustFloats(t, []float64{sigmaB * sigmaB}, 1, 1),
|
|||
|
|
ObservationJacobian: bearingJac,
|
|||
|
|
}
|
|||
|
|
zArr := mustFloats(t, z)
|
|||
|
|
ekf, err := ExtendedKalmanFilter(zArr, cv, bearing, opts)
|
|||
|
|
if err != nil {
|
|||
|
|
t.Fatalf("ExtendedKalmanFilter: %v", err)
|
|||
|
|
}
|
|||
|
|
ukf, err := UnscentedKalmanFilter(zArr, cv, bearing, opts)
|
|||
|
|
if err != nil {
|
|||
|
|
t.Fatalf("UnscentedKalmanFilter: %v", err)
|
|||
|
|
}
|
|||
|
|
// The bootstrap particle filter: 20000 particles propagated
|
|||
|
|
// through the same dynamics, weighted by the bearing likelihood,
|
|||
|
|
// systematically resampled every step.
|
|||
|
|
const particles = 20000
|
|||
|
|
pg := core.NewGenerator(202)
|
|||
|
|
px := make([]float64, particles)
|
|||
|
|
py := make([]float64, particles)
|
|||
|
|
vx := make([]float64, particles)
|
|||
|
|
vy := make([]float64, particles)
|
|||
|
|
weights := make([]float64, particles)
|
|||
|
|
for i := range particles {
|
|||
|
|
px[i] = 1900 + 100*pg.NormalUnit()
|
|||
|
|
py[i] = 80 + 100*pg.NormalUnit()
|
|||
|
|
vx[i] = 0 + 5*pg.NormalUnit()
|
|||
|
|
vy[i] = 0 + 5*pg.NormalUnit()
|
|||
|
|
}
|
|||
|
|
var meanX, meanY, varX, varY float64
|
|||
|
|
for t := range n {
|
|||
|
|
for i := range particles {
|
|||
|
|
px[i] += vx[i] + 0.01*pg.NormalUnit()
|
|||
|
|
py[i] += vy[i] + 0.01*pg.NormalUnit()
|
|||
|
|
vx[i] += 0.1 * pg.NormalUnit()
|
|||
|
|
vy[i] += 0.1 * pg.NormalUnit()
|
|||
|
|
}
|
|||
|
|
// Log weights against this step's bearing.
|
|||
|
|
maxLog := math.Inf(-1)
|
|||
|
|
for i := range particles {
|
|||
|
|
res := (z[t] - math.Atan2(py[i], px[i])) / sigmaB
|
|||
|
|
weights[i] = -0.5 * res * res
|
|||
|
|
maxLog = max(maxLog, weights[i])
|
|||
|
|
}
|
|||
|
|
total := 0.0
|
|||
|
|
for i := range particles {
|
|||
|
|
weights[i] = math.Exp(weights[i] - maxLog)
|
|||
|
|
total += weights[i]
|
|||
|
|
}
|
|||
|
|
// The weighted posterior moments, recorded at the last step.
|
|||
|
|
if t == n-1 {
|
|||
|
|
for i := range particles {
|
|||
|
|
w := weights[i] / total
|
|||
|
|
meanX += w * px[i]
|
|||
|
|
meanY += w * py[i]
|
|||
|
|
}
|
|||
|
|
for i := range particles {
|
|||
|
|
w := weights[i] / total
|
|||
|
|
varX += w * (px[i] - meanX) * (px[i] - meanX)
|
|||
|
|
varY += w * (py[i] - meanY) * (py[i] - meanY)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
// Systematic resampling: one uniform start, N evenly spaced
|
|||
|
|
// pointers walked through the cumulative weights.
|
|||
|
|
ancestorsX := make([]float64, particles)
|
|||
|
|
ancestorsY := make([]float64, particles)
|
|||
|
|
ancestorsVX := make([]float64, particles)
|
|||
|
|
ancestorsVY := make([]float64, particles)
|
|||
|
|
copy(ancestorsX, px)
|
|||
|
|
copy(ancestorsY, py)
|
|||
|
|
copy(ancestorsVX, vx)
|
|||
|
|
copy(ancestorsVY, vy)
|
|||
|
|
u := pg.Unit() / particles
|
|||
|
|
cum := 0.0
|
|||
|
|
j := 0
|
|||
|
|
cum += weights[0] / total
|
|||
|
|
for i := range particles {
|
|||
|
|
pointer := u + float64(i)/particles
|
|||
|
|
for cum < pointer && j < particles-1 {
|
|||
|
|
j++
|
|||
|
|
cum += weights[j] / total
|
|||
|
|
}
|
|||
|
|
px[i] = ancestorsX[j]
|
|||
|
|
py[i] = ancestorsY[j]
|
|||
|
|
vx[i] = ancestorsVX[j]
|
|||
|
|
vy[i] = ancestorsVY[j]
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
// The EKF and UKF positions against the Monte Carlo posterior,
|
|||
|
|
// with a tolerance of five posterior standard deviations.
|
|||
|
|
for name, state := range map[string]float64{
|
|||
|
|
"ekf x": ekf.States.FloatAt((n-1)*4 + 0),
|
|||
|
|
"ekf y": ekf.States.FloatAt((n-1)*4 + 1),
|
|||
|
|
"ukf x": ukf.States.FloatAt((n-1)*4 + 0),
|
|||
|
|
"ukf y": ukf.States.FloatAt((n-1)*4 + 1),
|
|||
|
|
} {
|
|||
|
|
want, spread := meanX, 5*math.Sqrt(varX)
|
|||
|
|
if strings.HasSuffix(name, "y") {
|
|||
|
|
want, spread = meanY, 5*math.Sqrt(varY)
|
|||
|
|
}
|
|||
|
|
if math.Abs(state-want) > spread {
|
|||
|
|
t.Fatalf("%s = %.3f, Monte Carlo posterior mean %.3f with sd %.3f",
|
|||
|
|
name, state, want, math.Sqrt(spread*spread/25))
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
// The two filters agree with each other to well under a posterior
|
|||
|
|
// standard deviation.
|
|||
|
|
for i, name := range []string{"x", "y"} {
|
|||
|
|
d := math.Abs(ekf.States.FloatAt((n-1)*4+i) - ukf.States.FloatAt((n-1)*4+i))
|
|||
|
|
spread := math.Sqrt(varX)
|
|||
|
|
if i == 1 {
|
|||
|
|
spread = math.Sqrt(varY)
|
|||
|
|
}
|
|||
|
|
if d > spread {
|
|||
|
|
t.Fatalf("EKF and UKF %s differ by %.3f, a posterior sd of %.3f", name, d, spread)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
// Plausible posterior: the filtered position tracks the truth
|
|||
|
|
// within four of its own standard deviations, and every published
|
|||
|
|
// covariance stayed symmetric and positive definite.
|
|||
|
|
cov := kalmanResultBlock(t, ekf.Covariances, n-1, 4, 4)
|
|||
|
|
errX := math.Abs(ekf.States.FloatAt((n-1)*4+0) - (truthX0 + truthVX*float64(n)))
|
|||
|
|
errY := math.Abs(ekf.States.FloatAt((n-1)*4+1) - (truthY0 + truthVY*float64(n)))
|
|||
|
|
if errX > 4*math.Sqrt(cov[0]) || errY > 4*math.Sqrt(cov[5]) {
|
|||
|
|
t.Fatalf("final position error (%.3f, %.3f) exceeds four filtered sds (%.3f, %.3f)",
|
|||
|
|
errX, errY, 4*math.Sqrt(cov[0]), 4*math.Sqrt(cov[5]))
|
|||
|
|
}
|
|||
|
|
if !cholPD(cov, 4) {
|
|||
|
|
t.Fatal("the final EKF covariance is not positive definite")
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// TestKalmanMultiChannel pins the multi-channel path: two observed
|
|||
|
|
// states with cross-coupled noise, against an independent two-by-two
|
|||
|
|
// reference recursion written in the test.
|
|||
|
|
func TestKalmanMultiChannel(t *testing.T) {
|
|||
|
|
const n = 100
|
|||
|
|
g := core.NewGenerator(13)
|
|||
|
|
z := make([]float64, 2*n)
|
|||
|
|
for i := range z {
|
|||
|
|
z[i] = g.NormalUnit()
|
|||
|
|
}
|
|||
|
|
f, err := KalmanFilter(mustFloats(t, z, n, 2), mustFloats(t, []float64{1, 1, 0, 1}, 2, 2),
|
|||
|
|
mustFloats(t, []float64{1, 0, 0, 1}, 2, 2),
|
|||
|
|
KalmanOptions{
|
|||
|
|
ProcessNoise: mustFloats(t, []float64{1e-4, 0, 0, 1e-2}, 2, 2),
|
|||
|
|
MeasurementNoise: mustFloats(t, []float64{0.1, 0, 0, 0.4}, 2, 2),
|
|||
|
|
})
|
|||
|
|
if err != nil {
|
|||
|
|
t.Fatalf("KalmanFilter: %v", err)
|
|||
|
|
}
|
|||
|
|
// The independent reference: the same recursion with a
|
|||
|
|
// standard-form update and an explicit two-by-two solve.
|
|||
|
|
x := []float64{0, 0}
|
|||
|
|
p := []float64{1, 0, 0, 1}
|
|||
|
|
logLik := 0.0
|
|||
|
|
solve2 := func(s []float64, b []float64) []float64 {
|
|||
|
|
det := s[0]*s[3] - s[1]*s[2]
|
|||
|
|
return []float64{(s[3]*b[0] - s[1]*b[1]) / det, (s[0]*b[1] - s[2]*b[0]) / det}
|
|||
|
|
}
|
|||
|
|
// mat2 multiplies two row-major two-by-two matrices.
|
|||
|
|
mat2 := func(x, y []float64) []float64 {
|
|||
|
|
return []float64{
|
|||
|
|
x[0]*y[0] + x[1]*y[2], x[0]*y[1] + x[1]*y[3],
|
|||
|
|
x[2]*y[0] + x[3]*y[2], x[2]*y[1] + x[3]*y[3],
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for step := range n {
|
|||
|
|
xp := []float64{x[0] + x[1], x[1]}
|
|||
|
|
pp := []float64{
|
|||
|
|
p[0] + p[1] + p[2] + p[3] + 1e-4, p[1] + p[3],
|
|||
|
|
p[2] + p[3], p[3] + 1e-2,
|
|||
|
|
}
|
|||
|
|
inn := []float64{z[2*step] - xp[0], z[2*step+1] - xp[1]}
|
|||
|
|
s := []float64{pp[0] + 0.1, pp[1], pp[2], pp[3] + 0.4}
|
|||
|
|
k := [][]float64{
|
|||
|
|
solve2(s, []float64{pp[0], pp[2]}),
|
|||
|
|
solve2(s, []float64{pp[1], pp[3]}),
|
|||
|
|
}
|
|||
|
|
solved := solve2(s, inn)
|
|||
|
|
quad := inn[0]*solved[0] + inn[1]*solved[1]
|
|||
|
|
det := s[0]*s[3] - s[1]*s[2]
|
|||
|
|
logLik += -0.5 * (2*math.Log(2*math.Pi) + math.Log(det) + quad)
|
|||
|
|
x = []float64{xp[0] + k[0][0]*inn[0] + k[0][1]*inn[1], xp[1] + k[1][0]*inn[0] + k[1][1]*inn[1]}
|
|||
|
|
a := []float64{1 - k[0][0], -k[0][1], -k[1][0], 1 - k[1][1]}
|
|||
|
|
at := []float64{a[0], a[2], a[1], a[3]}
|
|||
|
|
kr := []float64{k[0][0] * 0.1, k[0][1] * 0.4, k[1][0] * 0.1, k[1][1] * 0.4}
|
|||
|
|
kt := []float64{k[0][0], k[1][0], k[0][1], k[1][1]}
|
|||
|
|
aph := mat2(a, pp)
|
|||
|
|
joseph := mat2(aph, at)
|
|||
|
|
krkt := mat2(kr, kt)
|
|||
|
|
p = []float64{joseph[0] + krkt[0], joseph[1] + krkt[1], joseph[2] + krkt[2], joseph[3] + krkt[3]}
|
|||
|
|
if got := f.States.FloatAt(2 * step); math.Abs(got-x[0]) > 1e-9 {
|
|||
|
|
t.Fatalf("filtered mean 0 at %d = %.10f, want %.10f", step, got, x[0])
|
|||
|
|
}
|
|||
|
|
if got := f.States.FloatAt(2*step + 1); math.Abs(got-x[1]) > 1e-9 {
|
|||
|
|
t.Fatalf("filtered mean 1 at %d = %.10f, want %.10f", step, got, x[1])
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if math.Abs(f.LogLikelihood-logLik) > 1e-6 {
|
|||
|
|
t.Fatalf("log-likelihood %.8f, reference %.8f", f.LogLikelihood, logLik)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// TestKalmanCovarianceGuards walks a long filter and demands every
|
|||
|
|
// published covariance come back symmetric and positive definite,
|
|||
|
|
// then pins the input gates and the step-named failure reports.
|
|||
|
|
func TestKalmanCovarianceGuards(t *testing.T) {
|
|||
|
|
const n = 200
|
|||
|
|
g := core.NewGenerator(17)
|
|||
|
|
z := make([]float64, n)
|
|||
|
|
for i := range z {
|
|||
|
|
z[i] = g.NormalUnit()
|
|||
|
|
}
|
|||
|
|
f, err := KalmanFilter(mustFloats(t, z), mustFloats(t, []float64{1, 1, 0, 1}, 2, 2),
|
|||
|
|
mustFloats(t, []float64{1, 0}, 1, 2),
|
|||
|
|
KalmanOptions{
|
|||
|
|
ProcessNoise: mustFloats(t, []float64{1e-4, 0, 0, 1e-2}, 2, 2),
|
|||
|
|
MeasurementNoise: mustFloats(t, []float64{0.25}, 1, 1),
|
|||
|
|
})
|
|||
|
|
if err != nil {
|
|||
|
|
t.Fatalf("KalmanFilter: %v", err)
|
|||
|
|
}
|
|||
|
|
for step := range n {
|
|||
|
|
cov := kalmanResultBlock(t, f.Covariances, step, 2, 2)
|
|||
|
|
if math.Abs(cov[1]-cov[2]) > 1e-12*math.Max(math.Abs(cov[1]), math.Abs(cov[2])) {
|
|||
|
|
t.Fatalf("covariance at %d is not symmetric: %g against %g", step, cov[1], cov[2])
|
|||
|
|
}
|
|||
|
|
if !cholPD(cov, 2) {
|
|||
|
|
t.Fatalf("covariance at %d is not positive definite", step)
|
|||
|
|
}
|
|||
|
|
if !cholPD(kalmanResultBlock(t, f.InnovationCovariances, step, 1, 1), 1) {
|
|||
|
|
t.Fatalf("innovation covariance at %d is not positive definite", step)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
// The input gates.
|
|||
|
|
good := mustFloats(t, []float64{1, 1, 0, 1}, 2, 2)
|
|||
|
|
hGood := mustFloats(t, []float64{1, 0}, 1, 2)
|
|||
|
|
zOne := mustFloats(t, []float64{0.5})
|
|||
|
|
bad := []struct {
|
|||
|
|
name string
|
|||
|
|
run func() (*KalmanResult, error)
|
|||
|
|
}{
|
|||
|
|
{"nil transition", func() (*KalmanResult, error) { return KalmanFilter(zOne, nil, hGood, KalmanOptions{}) }},
|
|||
|
|
{"non-square transition", func() (*KalmanResult, error) {
|
|||
|
|
return KalmanFilter(zOne, mustFloats(t, []float64{1, 1}, 1, 2), hGood, KalmanOptions{})
|
|||
|
|
}},
|
|||
|
|
{"observation width mismatch", func() (*KalmanResult, error) {
|
|||
|
|
return KalmanFilter(zOne, good, mustFloats(t, []float64{1, 0, 0}, 1, 3), KalmanOptions{})
|
|||
|
|
}},
|
|||
|
|
{"asymmetric process noise", func() (*KalmanResult, error) {
|
|||
|
|
return KalmanFilter(zOne, good, hGood, KalmanOptions{ProcessNoise: mustFloats(t, []float64{1, 1, 0, 1}, 2, 2)})
|
|||
|
|
}},
|
|||
|
|
{"singular measurement noise", func() (*KalmanResult, error) {
|
|||
|
|
return KalmanFilter(zOne, good, hGood, KalmanOptions{MeasurementNoise: mustFloats(t, []float64{0}, 1, 1)})
|
|||
|
|
}},
|
|||
|
|
{"non-positive-definite initial covariance", func() (*KalmanResult, error) {
|
|||
|
|
return KalmanFilter(zOne, good, hGood, KalmanOptions{
|
|||
|
|
InitialCovariance: mustFloats(t, []float64{1, 0, 0, -1}, 2, 2)})
|
|||
|
|
}},
|
|||
|
|
{"empty measurements", func() (*KalmanResult, error) {
|
|||
|
|
return KalmanFilter(mustFloats(t, nil), good, hGood, KalmanOptions{})
|
|||
|
|
}},
|
|||
|
|
{"non-finite measurement", func() (*KalmanResult, error) {
|
|||
|
|
return KalmanFilter(mustFloats(t, []float64{math.NaN()}), good, hGood, KalmanOptions{})
|
|||
|
|
}},
|
|||
|
|
{"rank-3 measurements", func() (*KalmanResult, error) {
|
|||
|
|
return KalmanFilter(mustFloats(t, []float64{1, 0, 1, 0, 1, 0, 1, 0}, 2, 2, 2), good, hGood, KalmanOptions{})
|
|||
|
|
}},
|
|||
|
|
}
|
|||
|
|
for _, b := range bad {
|
|||
|
|
if _, err := b.run(); err == nil {
|
|||
|
|
t.Errorf("%s accepted", b.name)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
// The nonlinear filters need the state dimension spelled out.
|
|||
|
|
cv := func(x *core.Array) (*core.Array, error) { return x, nil }
|
|||
|
|
id := func(x *core.Array) (*core.Array, error) { return x, nil }
|
|||
|
|
if _, err := ExtendedKalmanFilter(zOne, cv, id, KalmanOptions{}); err == nil {
|
|||
|
|
t.Error("the extended filter accepted no initial state")
|
|||
|
|
}
|
|||
|
|
if _, err := UnscentedKalmanFilter(zOne, cv, id, KalmanOptions{}); err == nil {
|
|||
|
|
t.Error("the unscented filter accepted no initial state")
|
|||
|
|
}
|
|||
|
|
if _, err := UnscentedKalmanFilter(zOne, cv, id, KalmanOptions{
|
|||
|
|
InitialState: mustFloats(t, []float64{0}),
|
|||
|
|
SigmaAlpha: -1,
|
|||
|
|
}); err == nil {
|
|||
|
|
t.Error("the unscented filter accepted a negative alpha")
|
|||
|
|
}
|
|||
|
|
if _, err := UnscentedKalmanFilter(zOne, cv, id, KalmanOptions{
|
|||
|
|
InitialState: mustFloats(t, []float64{0}),
|
|||
|
|
SigmaAlpha: 1e-3,
|
|||
|
|
SigmaKappa: -2,
|
|||
|
|
}); err == nil {
|
|||
|
|
t.Error("the unscented filter accepted a kappa that kills the sigma scale")
|
|||
|
|
}
|
|||
|
|
// A singular state covariance mid-run is refused with the step
|
|||
|
|
// named: a transition that annihilates every state with zero
|
|||
|
|
// process noise collapses the predicted spread onto a point, and
|
|||
|
|
// the next predict cannot factor it.
|
|||
|
|
annihilate := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return core.FromFloats([]float64{0}, 1)
|
|||
|
|
}
|
|||
|
|
_, err = UnscentedKalmanFilter(mustFloats(t, []float64{0, 0}), annihilate, id, KalmanOptions{
|
|||
|
|
InitialState: mustFloats(t, []float64{0}),
|
|||
|
|
InitialCovariance: mustFloats(t, []float64{2}, 1, 1),
|
|||
|
|
ProcessNoise: mustFloats(t, []float64{0}, 1, 1),
|
|||
|
|
MeasurementNoise: mustFloats(t, []float64{1e18}, 1, 1),
|
|||
|
|
SigmaAlpha: 1,
|
|||
|
|
})
|
|||
|
|
if err == nil || !strings.Contains(err.Error(), "at step 1") {
|
|||
|
|
t.Fatalf("the collapsed sigma spread was not reported at step 1, got %v", err)
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// TestKalmanNonlinearGates pins the validation branches the nonlinear
|
|||
|
|
// filters own: callback presence, the state dimension's sources, the
|
|||
|
|
// complex and shape gates on every array the filters read, and the
|
|||
|
|
// step-named reports for failing or misbehaving callbacks.
|
|||
|
|
func TestKalmanNonlinearGates(t *testing.T) {
|
|||
|
|
identity := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return core.FromFloats([]float64{x.FloatAt(0)}, 1)
|
|||
|
|
}
|
|||
|
|
doubling := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return core.FromFloats([]float64{2 * x.FloatAt(0)}, 1)
|
|||
|
|
}
|
|||
|
|
base := KalmanOptions{
|
|||
|
|
InitialState: mustFloats(t, []float64{1}),
|
|||
|
|
InitialCovariance: mustFloats(t, []float64{1}, 1, 1),
|
|||
|
|
MeasurementNoise: mustFloats(t, []float64{1}, 1, 1),
|
|||
|
|
}
|
|||
|
|
zOne := mustFloats(t, []float64{1})
|
|||
|
|
if _, err := ExtendedKalmanFilter(zOne, nil, identity, base); err == nil {
|
|||
|
|
t.Error("the extended filter accepted a nil transition")
|
|||
|
|
}
|
|||
|
|
if _, err := UnscentedKalmanFilter(zOne, identity, nil, base); err == nil {
|
|||
|
|
t.Error("the unscented filter accepted a nil observation")
|
|||
|
|
}
|
|||
|
|
// The state dimension may come from the initial covariance alone.
|
|||
|
|
withoutState := base
|
|||
|
|
withoutState.InitialState = nil
|
|||
|
|
run, err := UnscentedKalmanFilter(zOne, doubling, identity, withoutState)
|
|||
|
|
if err != nil {
|
|||
|
|
t.Fatalf("the unscented filter refused a dimension from the initial covariance: %v", err)
|
|||
|
|
}
|
|||
|
|
// The gain from a unit prior under the doubling map: pp = 4,
|
|||
|
|
// s = 5, K = 4/5, and the measurement 1 lands the state at 4/5.
|
|||
|
|
if math.Abs(run.States.FloatAt(0)-0.8) > 1e-12 {
|
|||
|
|
t.Fatalf("state %.12g from a covariance-only start, want 0.8", run.States.FloatAt(0))
|
|||
|
|
}
|
|||
|
|
badRank, _ := core.FromFloats([]float64{1}, 1)
|
|||
|
|
covBadRank := withoutState
|
|||
|
|
covBadRank.InitialCovariance = badRank
|
|||
|
|
if _, err := ExtendedKalmanFilter(zOne, doubling, identity, covBadRank); err == nil {
|
|||
|
|
t.Error("a rank-1 initial covariance accepted")
|
|||
|
|
}
|
|||
|
|
// The complex gates on the arrays the filters read.
|
|||
|
|
complexState, _ := core.ComplexFromArray([]complex128{1}, 1)
|
|||
|
|
withComplex := base
|
|||
|
|
withComplex.InitialState = complexState
|
|||
|
|
if _, err := ExtendedKalmanFilter(zOne, doubling, identity, withComplex); err == nil {
|
|||
|
|
t.Error("a complex initial state accepted")
|
|||
|
|
}
|
|||
|
|
complexCov, _ := core.ComplexFromArray([]complex128{1}, 1, 1)
|
|||
|
|
withComplexCov := withoutState
|
|||
|
|
withComplexCov.InitialCovariance = complexCov
|
|||
|
|
if _, err := ExtendedKalmanFilter(zOne, doubling, identity, withComplexCov); err == nil {
|
|||
|
|
t.Error("a complex initial covariance accepted")
|
|||
|
|
}
|
|||
|
|
complexZ, _ := core.ComplexFromArray([]complex128{1}, 1)
|
|||
|
|
if _, err := ExtendedKalmanFilter(complexZ, doubling, identity, base); err == nil {
|
|||
|
|
t.Error("complex measurements accepted")
|
|||
|
|
}
|
|||
|
|
if _, err := UnscentedKalmanFilter(complexZ, doubling, identity, base); err == nil {
|
|||
|
|
t.Error("complex measurements accepted by the unscented filter")
|
|||
|
|
}
|
|||
|
|
// A measurement stack with zero width is refused before the walk.
|
|||
|
|
empty, _ := core.FromFloats([]float64{}, 2, 0)
|
|||
|
|
if _, err := ExtendedKalmanFilter(empty, doubling, identity, base); err == nil {
|
|||
|
|
t.Error("zero-width measurements accepted")
|
|||
|
|
}
|
|||
|
|
// A misbehaving transition is reported with the step.
|
|||
|
|
exploding := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return nil, errors.New("no")
|
|||
|
|
}
|
|||
|
|
_, err = ExtendedKalmanFilter(mustFloats(t, []float64{1, 1}), exploding, identity, base)
|
|||
|
|
if err == nil || !strings.Contains(err.Error(), "the transition") || !strings.Contains(err.Error(), "at step 1") {
|
|||
|
|
t.Fatalf("a failing transition was not reported at its step, got %v", err)
|
|||
|
|
}
|
|||
|
|
// A transition with the wrong output length likewise.
|
|||
|
|
wrong := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return core.FromFloats([]float64{1, 2}, 2)
|
|||
|
|
}
|
|||
|
|
if _, err := ExtendedKalmanFilter(mustFloats(t, []float64{1, 1}), wrong, identity, base); err == nil {
|
|||
|
|
t.Error("a transition with the wrong output length accepted")
|
|||
|
|
}
|
|||
|
|
if _, err := UnscentedKalmanFilter(mustFloats(t, []float64{1, 1}), identity, wrong, base); err == nil {
|
|||
|
|
t.Error("an observation with the wrong output length accepted")
|
|||
|
|
}
|
|||
|
|
// A non-finite transition output is refused where it appears.
|
|||
|
|
nonFinite := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return core.FromFloats([]float64{math.NaN()}, 1)
|
|||
|
|
}
|
|||
|
|
if _, err := UnscentedKalmanFilter(mustFloats(t, []float64{1, 1}), nonFinite, identity, base); err == nil {
|
|||
|
|
t.Error("a non-finite transition output accepted")
|
|||
|
|
}
|
|||
|
|
// A Jacobian of the wrong shape or a failing one is reported.
|
|||
|
|
badJac := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return core.FromFloats([]float64{1, 1}, 2, 1)
|
|||
|
|
}
|
|||
|
|
withBadJac := base
|
|||
|
|
withBadJac.ObservationJacobian = badJac
|
|||
|
|
if _, err := ExtendedKalmanFilter(mustFloats(t, []float64{1, 1}), doubling, identity, withBadJac); err == nil {
|
|||
|
|
t.Error("an observation Jacobian of the wrong shape accepted")
|
|||
|
|
}
|
|||
|
|
errJac := func(x *core.Array) (*core.Array, error) {
|
|||
|
|
return nil, errors.New("no")
|
|||
|
|
}
|
|||
|
|
withErrJac := base
|
|||
|
|
withErrJac.TransitionJacobian = errJac
|
|||
|
|
_, err = ExtendedKalmanFilter(mustFloats(t, []float64{1, 1}), doubling, identity, withErrJac)
|
|||
|
|
if err == nil || !strings.Contains(err.Error(), "the transition Jacobian") {
|
|||
|
|
t.Fatalf("a failing transition Jacobian was not reported, got %v", err)
|
|||
|
|
}
|
|||
|
|
withErrObsJac := base
|
|||
|
|
withErrObsJac.ObservationJacobian = errJac
|
|||
|
|
_, err = ExtendedKalmanFilter(mustFloats(t, []float64{1, 1}), doubling, identity, withErrObsJac)
|
|||
|
|
if err == nil || !strings.Contains(err.Error(), "the observation Jacobian") {
|
|||
|
|
t.Fatalf("a failing observation Jacobian was not reported, got %v", err)
|
|||
|
|
}
|
|||
|
|
// The linear filter's rank gate on the transition matrix.
|
|||
|
|
if _, err := KalmanFilter(mustFloats(t, []float64{1}), mustFloats(t, []float64{1, 1}, 1, 2),
|
|||
|
|
mustFloats(t, []float64{1, 0}, 1, 2), KalmanOptions{}); err == nil {
|
|||
|
|
t.Error("a rank-1 transition matrix accepted")
|
|||
|
|
}
|
|||
|
|
}
|