Skip to content

Commit b720475

Browse files
milosgajdosmilosgajdos
authored
Merge pull request #8 from soypat/master
Add missing dimensions to `Model.Dims()` for MIMO systems. Resolves #6
2 parents fd47d1b + 1509273 commit b720475

File tree

16 files changed

+268
-232
lines changed

16 files changed

+268
-232
lines changed

‎filter.go

Lines changed: 28 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -6,22 +6,25 @@ import (
66

77
// Filter is a dynamical system filter.
88
type Filter interface {
9-
// Predict estimates the next internal state of the system
10-
Predict(mat.Vector, mat.Vector) (Estimate, error)
11-
// Update updates the system state based on external measurement
12-
Update(mat.Vector, mat.Vector, mat.Vector) (Estimate, error)
9+
// Predict returns a prediction of which will be
10+
// next internal state
11+
Predict(x, u mat.Vector) (Estimate, error)
12+
// Update returns estimated system state based on external measurement ym.
13+
Update(x, u, ym mat.Vector) (Estimate, error)
1314
}
1415

1516
// Propagator propagates internal state of the system to the next step
1617
type Propagator interface {
17-
// Propagate propagates internal state of the system to the next step
18-
Propagate(mat.Vector, mat.Vector, mat.Vector) (mat.Vector, error)
18+
// Propagate propagates internal state of the system to the next step.
19+
// x is starting state, u is input vector, and z is disturbance input
20+
Propagate(x, u, z mat.Vector) (mat.Vector, error)
1921
}
2022

2123
// Observer observes external state (output) of the system
2224
type Observer interface {
23-
// Observe observes external state of the system
24-
Observe(mat.Vector, mat.Vector, mat.Vector) (mat.Vector, error)
25+
// Observe observes external state of the system.
26+
// Result for a linear system would be y=C*x+D*u+wn (last term is measurement noise)
27+
Observe(x, u, wn mat.Vector) (y mat.Vector, err error)
2528
}
2629

2730
// Model is a model of a dynamical system
@@ -30,8 +33,15 @@ type Model interface {
3033
Propagator
3134
// Observer is system observer
3235
Observer
33-
// Dims returns input and output dimensions of the model
34-
Dims() (in int, out int)
36+
// SystemDims returns the dimension of state vector, input vector,
37+
// output (measurements, written as y) vector and disturbance vector (only dynamical systems).
38+
// Below are dimension of matrices as returned by SystemDims() (row,column)
39+
// nx, nx = A.SystemDims()
40+
// nx, nu = B.SystemDims()
41+
// ny, nx = C.SystemDims()
42+
// ny, nu = D.SystemDims()
43+
// nx, nz = E.SystemDims()
44+
SystemDims() (nx, nu, ny, nz int)
3545
}
3646

3747
// Smoother is a filter smoother
@@ -45,14 +55,15 @@ type Smoother interface {
4555
type DiscreteModel interface {
4656
// Model is a model of a dynamical system
4757
Model
48-
// StateMatrix returns state propagation matrix
49-
StateMatrix() mat.Matrix
50-
// StateCtlMatrix returns state propagation control matrix
51-
StateCtlMatrix() mat.Matrix
58+
// SystemMatrix returns state propagation matrix
59+
SystemMatrix() (A mat.Matrix)
60+
// ControlMatrix returns state propagation control matrix
61+
ControlMatrix() (B mat.Matrix)
5262
// OutputMatrix returns observation matrix
53-
OutputMatrix() mat.Matrix
54-
// OutputCtlMatrix returns observation control matrix
55-
OutputCtlMatrix() mat.Matrix
63+
OutputMatrix() (C mat.Matrix)
64+
// FeedForwardMatrix returns observation control matrix
65+
FeedForwardMatrix() (D mat.Matrix)
66+
// TODO DisturbanceMatrix
5667
}
5768

5869
// InitCond is initial state condition of the filter

‎kalman/ekf/ekf.go

Lines changed: 25 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -50,30 +50,30 @@ type EKF struct {
5050
// - invalid state or output noise is given: noise covariance must either be nil or match the model dimensions
5151
func New(m filter.Model, init filter.InitCond, q, r filter.Noise) (*EKF, error) {
5252
// size of the input and output vectors
53-
in, out := m.Dims()
54-
if in <= 0 || out <= 0 {
55-
return nil, fmt.Errorf("Invalid model dimensions: [%d x %d]", in, out)
53+
nx, _, ny, _ := m.SystemDims()
54+
if nx <= 0 || ny <= 0 {
55+
return nil, fmt.Errorf("invalid model dimensions: [%d x %d]", nx, ny)
5656
}
5757

5858
if q != nil {
59-
if q.Cov().Symmetric() != in {
60-
return nil, fmt.Errorf("Invalid state noise dimension: %d", q.Cov().Symmetric())
59+
if q.Cov().Symmetric() != nx {
60+
return nil, fmt.Errorf("invalid state noise dimension: %d", q.Cov().Symmetric())
6161
}
6262
} else {
6363
q, _ = noise.NewNone()
6464
}
6565

6666
if r != nil {
67-
if r.Cov().Symmetric() != out {
68-
return nil, fmt.Errorf("Invalid output noise dimension: %d", r.Cov().Symmetric())
67+
if r.Cov().Symmetric() != ny {
68+
return nil, fmt.Errorf("invalid output noise dimension: %d", r.Cov().Symmetric())
6969
}
7070
} else {
7171
r, _ = noise.NewNone()
7272
}
7373

7474
// propagation Jacobian
7575
fJacFn := func(u mat.Vector) func([]float64, []float64) {
76-
q, _ := noise.NewZero(in)
76+
q, _ := noise.NewZero(nx)
7777

7878
return func(xOut, xNow []float64) {
7979
x := mat.NewVecDense(len(xNow), xNow)
@@ -87,11 +87,11 @@ func New(m filter.Model, init filter.InitCond, q, r filter.Noise) (*EKF, error)
8787
}
8888
}
8989
}
90-
f := mat.NewDense(in, in, nil)
90+
f := mat.NewDense(nx, nx, nil)
9191

9292
// observation Jacobian
9393
hJacFn := func(u mat.Vector) func([]float64, []float64) {
94-
r, _ := noise.NewZero(out)
94+
r, _ := noise.NewZero(ny)
9595

9696
return func(y, xNow []float64) {
9797
x := mat.NewVecDense(len(xNow), xNow)
@@ -106,7 +106,7 @@ func New(m filter.Model, init filter.InitCond, q, r filter.Noise) (*EKF, error)
106106
}
107107
}
108108
}
109-
h := mat.NewDense(out, in, nil)
109+
h := mat.NewDense(ny, nx, nil)
110110

111111
// initialize covariance matrix to initial condition covariance
112112
p := mat.NewSymDense(init.Cov().Symmetric(), nil)
@@ -116,10 +116,10 @@ func New(m filter.Model, init filter.InitCond, q, r filter.Noise) (*EKF, error)
116116
pNext := mat.NewSymDense(init.Cov().Symmetric(), nil)
117117

118118
// innovation vector
119-
inn := mat.NewVecDense(out, nil)
119+
inn := mat.NewVecDense(ny, nil)
120120

121121
// kalman gain
122-
k := mat.NewDense(in, out, nil)
122+
k := mat.NewDense(nx, ny, nil)
123123

124124
return &EKF{
125125
m: m,
@@ -143,7 +143,7 @@ func (k *EKF) Predict(x, u mat.Vector) (filter.Estimate, error) {
143143
// propagate input state to the next step
144144
xNext, err := k.m.Propagate(x, u, k.q.Sample())
145145
if err != nil {
146-
return nil, fmt.Errorf("System state propagation failed: %v", err)
146+
return nil, fmt.Errorf("system state propagation failed: %v", err)
147147
}
148148

149149
// calculate propagation Jacobian matrix
@@ -174,16 +174,16 @@ func (k *EKF) Predict(x, u mat.Vector) (filter.Estimate, error) {
174174
// Update corrects state x using the measurement z, given control intput u and returns corrected estimate.
175175
// It returns error if either invalid state was supplied or if it fails to calculate system output estimate.
176176
func (k *EKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
177-
in, out := k.m.Dims()
177+
nx, _, ny, _ := k.m.SystemDims()
178178

179-
if z.Len() != out {
180-
return nil, fmt.Errorf("Invalid measurement supplied: %v", z)
179+
if z.Len() != ny {
180+
return nil, fmt.Errorf("invalid measurement supplied: %v", z)
181181
}
182182

183183
// observe system output in the next step
184184
y, err := k.m.Observe(x, u, k.r.Sample())
185185
if err != nil {
186-
return nil, fmt.Errorf("Failed to observe system output: %v", err)
186+
return nil, fmt.Errorf("failed to observe system output: %v", err)
187187
}
188188

189189
// calculate observation Jacobian matrix
@@ -192,8 +192,8 @@ func (k *EKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
192192
Concurrent: true,
193193
})
194194

195-
pxy := mat.NewDense(in, out, nil)
196-
pyy := mat.NewDense(out, out, nil)
195+
pxy := mat.NewDense(nx, ny, nil)
196+
pyy := mat.NewDense(ny, ny, nil)
197197

198198
// P*H'
199199
pxy.Mul(k.pNext, k.h.T())
@@ -209,7 +209,7 @@ func (k *EKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
209209
// calculate Kalman gain
210210
pyyInv := &mat.Dense{}
211211
if err := pyyInv.Inverse(pyy); err != nil {
212-
return nil, fmt.Errorf("Failed to calculat Pyy inverse: %v", err)
212+
return nil, fmt.Errorf("failed to calculat Pyy inverse: %v", err)
213213
}
214214
gain := &mat.Dense{}
215215
gain.Mul(pxy, pyyInv)
@@ -257,8 +257,8 @@ func (k *EKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
257257
k.inn.CopyVec(inn)
258258
k.k.Copy(gain)
259259
// update EKF covariance matrix
260-
for i := 0; i < in; i++ {
261-
for j := i; j < in; j++ {
260+
for i := 0; i < nx; i++ {
261+
for j := i; j < nx; j++ {
262262
k.p.SetSym(i, j, pCorr.At(i, j))
263263
}
264264
}
@@ -310,11 +310,11 @@ func (k *EKF) Cov() mat.Symmetric {
310310
// It returns error if either cov is nil or its dimensions are not the same as EKF covariance dimensions.
311311
func (k *EKF) SetCov(cov mat.Symmetric) error {
312312
if cov == nil {
313-
return fmt.Errorf("Invalid covariance matrix: %v", cov)
313+
return fmt.Errorf("invalid covariance matrix: %v", cov)
314314
}
315315

316316
if cov.Symmetric() != k.p.Symmetric() {
317-
return fmt.Errorf("Invalid covariance matrix dims: [%d x %d]", cov.Symmetric(), cov.Symmetric())
317+
return fmt.Errorf("invalid covariance matrix dims: [%d x %d]", cov.Symmetric(), cov.Symmetric())
318318
}
319319

320320
k.p.CopySym(cov)

‎kalman/ekf/ekf_test.go

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,8 @@ type invalidModel struct {
1515
filter.Model
1616
}
1717

18-
func (m *invalidModel) Dims() (int, int) {
19-
return -10, 8
18+
func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) {
19+
return -10, 0, 8, 0 // a system may have 0 inputs, this is not "invalid". Negative dimension is invalid
2020
}
2121

2222
var (

‎kalman/ekf/iekf.go

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ type IEKF struct {
3131
// - invalid number of update iterations is given: n must be non-negative
3232
func NewIter(m filter.Model, ic filter.InitCond, q, r filter.Noise, n int) (*IEKF, error) {
3333
if n <= 0 {
34-
return nil, fmt.Errorf("Invalid number of update iterations: %d", n)
34+
return nil, fmt.Errorf("invalid number of update iterations: %d", n)
3535
}
3636

3737
// IEKF is EKF which uses iterating updates
@@ -49,20 +49,20 @@ func NewIter(m filter.Model, ic filter.InitCond, q, r filter.Noise, n int) (*IEK
4949
// Update corrects state x using the measurement z, given control intput u and returns corrected estimate of x.
5050
// It returns error if either invalid state was supplied or if it fails to calculate system output estimate.
5151
func (k *IEKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
52-
in, out := k.m.Dims()
52+
nx, _, ny, _ := k.m.SystemDims()
5353

54-
if z.Len() != out {
55-
return nil, fmt.Errorf("Invalid measurement supplied: %v", z)
54+
if z.Len() != ny {
55+
return nil, fmt.Errorf("invalid measurement supplied: %v", z)
5656
}
5757

5858
// observe system output in the next step
5959
y, err := k.m.Observe(x, u, k.OutputNoise().Sample())
6060
if err != nil {
61-
return nil, fmt.Errorf("Failed to observe system output: %v", err)
61+
return nil, fmt.Errorf("failed to observe system output: %v", err)
6262
}
6363

64-
pxy := mat.NewDense(in, out, nil)
65-
pyy := mat.NewDense(out, out, nil)
64+
pxy := mat.NewDense(nx, ny, nil)
65+
pyy := mat.NewDense(ny, ny, nil)
6666

6767
// innovation vector
6868
inn := &mat.VecDense{}
@@ -96,7 +96,7 @@ func (k *IEKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
9696
// calculate Kalman gain
9797
pyyInv := &mat.Dense{}
9898
if err := pyyInv.Inverse(pyy); err != nil {
99-
return nil, fmt.Errorf("Failed to calculat Pyy inverse: %v", err)
99+
return nil, fmt.Errorf("failed to calculat Pyy inverse: %v", err)
100100
}
101101
gain.Mul(pxy, pyyInv)
102102

@@ -139,8 +139,8 @@ func (k *IEKF) Update(x, u, z mat.Vector) (filter.Estimate, error) {
139139
k.inn.CopyVec(inn)
140140
k.k.Copy(gain)
141141
// update EKF covariance matrix
142-
for i := 0; i < in; i++ {
143-
for j := i; j < in; j++ {
142+
for i := 0; i < nx; i++ {
143+
for j := i; j < nx; j++ {
144144
k.p.SetSym(i, j, pCorr.At(i, j))
145145
}
146146
}

0 commit comments

Comments
 (0)