This repository has been archived by the owner on Aug 26, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 3
/
vanilla_test.go
124 lines (110 loc) · 4.71 KB
/
vanilla_test.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
package gokalman
import (
"testing"
"github.com/gonum/matrix/mat64"
)
func TestNewVanillaErrors(t *testing.T) {
F, G, _ := Robot1DMatrices()
H := mat64.NewDense(2, 2, nil)
x0 := mat64.NewVector(2, nil)
Covar0 := mat64.NewSymDense(3, nil)
if _, _, err := NewVanilla(x0, Covar0, F, G, H, Noiseless{}); err == nil {
t.Fatal("x0 and Covar0 of incompatible sizes does not fail")
}
x0 = mat64.NewVector(3, nil)
if _, _, err := NewVanilla(x0, Covar0, F, G, H, Noiseless{}); err == nil {
t.Fatal("F and Covar0 of incompatible sizes does not fail")
}
x0 = mat64.NewVector(2, nil)
Covar0 = mat64.NewSymDense(2, nil)
H = mat64.NewDense(3, 3, nil)
if _, _, err := NewVanilla(x0, Covar0, F, G, H, Noiseless{}); err == nil {
t.Fatal("H and x0 of incompatible sizes does not fail")
}
}
func TestVanilla(t *testing.T) {
F, G, Δt := Midterm2Matrices()
Q := mat64.NewSymDense(3, []float64{2.5e-15, 6.25e-13, (25e-11) / 3, 6.25e-13, (5e-7) / 3, 2.5e-8, (25e-11) / 3, 2.5e-8, 5e-6})
R := mat64.NewSymDense(1, []float64{0.005 / Δt})
H := mat64.NewDense(1, 3, []float64{1, 0, 0})
noise := NewAWGN(Q, R)
x0 := mat64.NewVector(3, []float64{0, 0.35, 0})
Covar0 := ScaledIdentity(3, 10)
kf, est0, err := NewVanilla(x0, Covar0, F, G, H, noise)
if err != nil {
t.Fatal(err)
}
// Test setters
kf.SetStateTransition(F)
if !mat64.Equal(F, kf.GetStateTransition()) {
t.Fatal("SetStateTransition did not return the expected F")
}
kf.SetInputControl(G)
if !mat64.Equal(G, kf.GetInputControl()) {
t.Fatal("SetInputControl did not return the expected G")
}
kf.SetMeasurementMatrix(H)
if !mat64.Equal(H, kf.GetMeasurementMatrix()) {
t.Fatal("GetMeasurementMatrix did not return the expected H")
}
kf.SetNoise(noise)
if !mat64.Equal(noise.MeasurementMatrix(), kf.GetNoise().MeasurementMatrix()) {
t.Fatal("GetNoise/SetNoise issue")
}
var est Estimate
yacc := []float64{0.12758, 0.11748, 0.20925, 0.0984, 0.12824, -0.069948, -0.11166, 0.25519, 0.12713, -0.011207, 0.50973, 0.12334, -0.028878, 0.19208, 0.17605, -0.10383, 0.19707, -0.40455, 0.27355, 0.060617, 0.10369, 0.22131, -0.0038337, -0.60504, -0.10213, -0.021907, 0.030875, 0.17578, -0.45262, -0.086119, -0.12265, -0.056002, -0.11744, 0.01039, 0.028251, 0.053642, 0.17204, -0.052963, -0.16611, 0.078431, -0.20175, -0.23044, 0.38302, -0.33455, -0.35916, 0.28959, 0.097137, -0.29778, -0.23343, 0.21113, -0.22098, -0.057898, 0.17649, 0.058624, 0.045438, 0.11104, 0.37742, 0.0013074, 0.34331, 0.37244, 0.01434, -0.35709, 0.14435, -0.20445, -0.031335, -0.35165, -0.091494, -0.34382, 0.36144, -0.3835, 0.10339, -0.055055, -0.17677, -0.12108, -0.094458, -0.38408, 0.03215, 0.5759, 0.3297, -0.63341, 0.11228, 0.32364, -0.36897, 0.050504, 0.25338, -0.040326, 0.37904, 0.083807, -0.1023, 0.19609, 0.43701, -0.067234, 0.11835, 0.10064, 0.1024, 0.19084, 0.22646, -0.17419, 0.27345, 36.295}
for k := 1; k < 100; k++ {
yVec := mat64.NewVector(1, []float64{yacc[k]})
est, err = kf.Update(yVec, mat64.NewVector(1, nil))
if err != nil {
t.Fatal(err)
}
// At k=99, there is an especially high yacc in order to test another line in the code.
if !est.IsWithinNσ(2) && k != 99 {
t.Logf("[WARN] 2σ bound breached: k=%d -> %s ", k, est)
}
if k == 99 {
t.Logf("k=%d\n%s", k, est)
}
}
// Test reset
kf.Reset()
if kf.step != 0 {
t.Fatal("reset failed: step non nil")
}
if !mat64.Equal(kf.prevEst.State(), est0.State()) {
t.Fatal("reset failed: invalid initial state")
}
if _, err = kf.Update(mat64.NewVector(1, nil), mat64.NewVector(2, nil)); err == nil {
t.Fatal("using an invalid control vector does not fail")
}
if _, err = kf.Update(mat64.NewVector(2, nil), mat64.NewVector(1, nil)); err == nil {
t.Fatal("using an invalid measurement vector does not fail")
}
}
func TestVanillaMultiD(t *testing.T) {
// DT system
Δt := 0.01
F := mat64.NewDense(4, 4, []float64{1, 0.01, 5e-5, 0, 0, 1, 0.01, 0, 0, 0, 1, 0, 0, 0, 0, 1.0005})
G := mat64.NewDense(4, 1, []float64{(5e-7) / 3, 5e-5, 0.01, 0})
H := mat64.NewDense(2, 4, []float64{1, 0, 0, 0, 0, 0, 1, 1})
// Noise
Q := mat64.NewSymDense(4, []float64{2.5e-15, 6.25e-13, (25e-11) / 3, 0, 6.25e-13, (5e-7) / 3, 2.5e-8, 0, (25e-11) / 3, 2.5e-8, 5e-6, 0, 0, 0, 0, 5.302e-4})
R := mat64.NewSymDense(2, []float64{0.005 / Δt, 0, 0, 0.0005 / Δt})
// Vanilla KF
noise := NewAWGN(Q, R)
x0 := mat64.NewVector(4, []float64{0, 0.35, 0, 0})
Covar0 := ScaledIdentity(4, 10)
kf, _, err := NewVanilla(x0, Covar0, F, G, H, noise)
t.Logf("%s", kf)
if err != nil {
panic(err)
}
measurements := []*mat64.Vector{mat64.NewVector(2, []float64{-0.80832, -0.011207}), mat64.NewVector(2, []float64{0.39265, 0.060617})}
for _, measurement := range measurements {
_, err := kf.Update(measurement, mat64.NewVector(1, nil))
if err != nil {
t.Fatal(err)
}
}
}