From 8e2e84c769f6594c79441c88eaa67bfb24054608 Mon Sep 17 00:00:00 2001 From: soypat Date: Sat, 21 Aug 2021 13:10:01 -0300 Subject: [PATCH] create distinction between discrete and continuous systems --- filter.go | 19 +++-- kalman/ekf/ekf.go | 6 +- kalman/ekf/ekf_test.go | 6 +- kalman/ekf/iekf.go | 2 +- kalman/kf/kf.go | 6 +- kalman/kf/kf_test.go | 8 +- kalman/ukf/ukf.go | 6 +- kalman/ukf/ukf_test.go | 6 +- particle/bf/bf.go | 4 +- particle/bf/bf_test.go | 6 +- sim/continuous_time.go | 109 +++++++++++++++++++++++++ sim/discrete_time.go | 51 ++++++++++++ sim/model.go | 172 --------------------------------------- sim/model_test.go | 10 +-- sim/sim.go | 39 +++++++++ sim/system.go | 114 ++++++++++++++++++++++++++ smooth/erts/erts.go | 4 +- smooth/erts/erts_test.go | 8 +- smooth/rts/rts.go | 4 +- smooth/rts/rts_test.go | 8 +- 20 files changed, 365 insertions(+), 223 deletions(-) create mode 100644 sim/continuous_time.go create mode 100644 sim/discrete_time.go delete mode 100644 sim/model.go create mode 100644 sim/sim.go create mode 100644 sim/system.go diff --git a/filter.go b/filter.go index 2396661..e898c93 100644 --- a/filter.go +++ b/filter.go @@ -13,8 +13,8 @@ type Filter interface { Update(x, u, ym mat.Vector) (Estimate, error) } -// Propagator propagates internal state of the system to the next step -type Propagator interface { +// DiscretePropagator propagates internal state of a discrete-time system to the next step +type DiscretePropagator interface { // Propagate propagates internal state of the system to the next step. // x is starting state, u is input vector, and z is disturbance input Propagate(x, u, z mat.Vector) (mat.Vector, error) @@ -27,10 +27,11 @@ type Observer interface { Observe(x, u, wn mat.Vector) (y mat.Vector, err error) } -// Model is a model of a dynamical system -type Model interface { +// DiscreteModel is a discrete-time model of a dynamical system which contains all the +// logic to propagate internal state and observe external state. +type DiscreteModel interface { // Propagator is system propagator - Propagator + DiscretePropagator // Observer is system observer Observer // SystemDims returns the dimension of state vector, input vector, @@ -50,11 +51,11 @@ type Smoother interface { Smooth([]Estimate, []mat.Vector) ([]Estimate, error) } -// DiscreteModel is a dynamical system whose state is driven by +// DiscreteControlSystem is a dynamical system whose state is driven by // static propagation and observation dynamics matrices -type DiscreteModel interface { - // Model is a model of a dynamical system - Model +type DiscreteControlSystem interface { + // DiscreteModel is a model of a dynamical discrete-time system + DiscreteModel // SystemMatrix returns state propagation matrix SystemMatrix() (A mat.Matrix) // ControlMatrix returns state propagation control matrix diff --git a/kalman/ekf/ekf.go b/kalman/ekf/ekf.go index f717d9e..925d9b4 100644 --- a/kalman/ekf/ekf.go +++ b/kalman/ekf/ekf.go @@ -16,7 +16,7 @@ type JacFunc func(u mat.Vector) func(y, x []float64) // EKF is Extended Kalman Filter type EKF struct { // m is EKF system model - m filter.Model + m filter.DiscreteModel // q is state noise a.k.a. process noise q filter.Noise // r is output noise a.k.a. measurement noise @@ -48,7 +48,7 @@ type EKF struct { // It returns error if either of the following conditions is met: // - invalid model is given: model dimensions must be positive integers // - invalid state or output noise is given: noise covariance must either be nil or match the model dimensions -func New(m filter.Model, init filter.InitCond, q, r filter.Noise) (*EKF, error) { +func New(m filter.DiscreteModel, init filter.InitCond, q, r filter.Noise) (*EKF, error) { // size of the input and output vectors nx, _, ny, _ := m.SystemDims() if nx <= 0 || ny <= 0 { @@ -284,7 +284,7 @@ func (k *EKF) Run(x, u, z mat.Vector) (filter.Estimate, error) { } // Model returns EKF model -func (k *EKF) Model() filter.Model { +func (k *EKF) Model() filter.DiscreteModel { return k.m } diff --git a/kalman/ekf/ekf_test.go b/kalman/ekf/ekf_test.go index 059a76c..b5d4761 100644 --- a/kalman/ekf/ekf_test.go +++ b/kalman/ekf/ekf_test.go @@ -12,7 +12,7 @@ import ( ) type invalidModel struct { - filter.Model + filter.DiscreteModel } func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) { @@ -20,7 +20,7 @@ func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) { } var ( - okModel *sim.BaseModel + okModel *sim.Discrete badModel *invalidModel ic *sim.InitCond q filter.Noise @@ -47,7 +47,7 @@ func setup() { C := mat.NewDense(1, 2, []float64{1.0, 0.0}) D := mat.NewDense(1, 1, []float64{0.0}) - okModel = &sim.BaseModel{A: A, B: B, C: C, D: D} + okModel, _ = sim.NewDiscrete(A, B, C, D, nil) badModel = &invalidModel{okModel} } diff --git a/kalman/ekf/iekf.go b/kalman/ekf/iekf.go index 892ca57..fe6e6f5 100644 --- a/kalman/ekf/iekf.go +++ b/kalman/ekf/iekf.go @@ -29,7 +29,7 @@ type IEKF struct { // - invalid model is given: model dimensions must be positive integers // - invalid state or output noise is given: noise covariance must either be nil or match the model dimensions // - invalid number of update iterations is given: n must be non-negative -func NewIter(m filter.Model, ic filter.InitCond, q, r filter.Noise, n int) (*IEKF, error) { +func NewIter(m filter.DiscreteModel, ic filter.InitCond, q, r filter.Noise, n int) (*IEKF, error) { if n <= 0 { return nil, fmt.Errorf("invalid number of update iterations: %d", n) } diff --git a/kalman/kf/kf.go b/kalman/kf/kf.go index 3d3bc29..8c4582f 100644 --- a/kalman/kf/kf.go +++ b/kalman/kf/kf.go @@ -12,7 +12,7 @@ import ( // KF is Kalman Filter type KF struct { // m is KF system model - m filter.DiscreteModel + m filter.DiscreteControlSystem // q is state noise a.k.a. process noise q filter.Noise // r is output noise a.k.a. measurement noise @@ -37,7 +37,7 @@ type KF struct { // It returns error if either of the following conditions is met: // - invalid model is given: model dimensions must be positive integers // - invalid state or output noise is given: noise covariance must either be nil or match the model dimensions -func New(m filter.DiscreteModel, init filter.InitCond, z, wn filter.Noise) (*KF, error) { +func New(m filter.DiscreteControlSystem, init filter.InitCond, z, wn filter.Noise) (*KF, error) { // size of the input and output vectors nx, _, ny, _ := m.SystemDims() if nx <= 0 || ny <= 0 { @@ -243,7 +243,7 @@ func (k *KF) Run(x, u, z mat.Vector) (filter.Estimate, error) { } // Model returns KF models -func (k *KF) Model() filter.Model { +func (k *KF) Model() filter.DiscreteModel { return k.m } diff --git a/kalman/kf/kf_test.go b/kalman/kf/kf_test.go index c1b6aea..48cc65f 100644 --- a/kalman/kf/kf_test.go +++ b/kalman/kf/kf_test.go @@ -12,7 +12,7 @@ import ( ) type invalidModel struct { - filter.DiscreteModel + filter.DiscreteControlSystem nx int nu int ny int @@ -23,7 +23,7 @@ func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) { } var ( - okModel *sim.BaseModel + okModel *sim.Discrete badModel *invalidModel ic *sim.InitCond q filter.Noise @@ -50,8 +50,8 @@ func setup() { C := mat.NewDense(1, 2, []float64{1.0, 0.0}) D := mat.NewDense(1, 1, []float64{0.0}) - okModel = &sim.BaseModel{A: A, B: B, C: C, D: D} - badModel = &invalidModel{DiscreteModel: okModel, nx: 10, ny: 10} + okModel, _ = sim.NewDiscrete(A, B, C, D, nil) + badModel = &invalidModel{DiscreteControlSystem: okModel, nx: 10, ny: 10} } func TestMain(m *testing.M) { diff --git a/kalman/ukf/ukf.go b/kalman/ukf/ukf.go index d852ee2..8773fda 100644 --- a/kalman/ukf/ukf.go +++ b/kalman/ukf/ukf.go @@ -38,7 +38,7 @@ type Config struct { // UKF is Unscented (a.k.a. Sigma Point) Kalman Filter type UKF struct { // m is UKF system model - m filter.Model + m filter.DiscreteModel // q is state noise a.k.a. process noise q filter.Noise // r is output noise a.k.a. measurement noise @@ -77,7 +77,7 @@ type UKF struct { // - invalid state or output noise is given: noise covariance must either be nil or match the model dimensions // - invalid sigma points parameters (alpha, beta, kappa) are supplied // - sigma points fail to be generated: due to covariance SVD factorizations failure -func New(m filter.Model, init filter.InitCond, q, r filter.Noise, c *Config) (*UKF, error) { +func New(m filter.DiscreteModel, init filter.InitCond, q, r filter.Noise, c *Config) (*UKF, error) { // size of the input and output vectors nx, _, ny, _ := m.SystemDims() if nx <= 0 || ny <= 0 { @@ -456,7 +456,7 @@ func (k *UKF) Run(x, u, z mat.Vector) (filter.Estimate, error) { } // Model returns UKF model -func (k *UKF) Model() filter.Model { +func (k *UKF) Model() filter.DiscreteModel { return k.m } diff --git a/kalman/ukf/ukf_test.go b/kalman/ukf/ukf_test.go index 77e8527..21e72e6 100644 --- a/kalman/ukf/ukf_test.go +++ b/kalman/ukf/ukf_test.go @@ -12,7 +12,7 @@ import ( ) type invalidModel struct { - filter.Model + filter.DiscreteModel } func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) { @@ -20,7 +20,7 @@ func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) { } var ( - okModel *sim.BaseModel + okModel *sim.Discrete badModel *invalidModel ic *sim.InitCond q filter.Noise @@ -48,7 +48,7 @@ func setup() { C := mat.NewDense(1, 2, []float64{1.0, 0.0}) D := mat.NewDense(1, 1, []float64{0.0}) - okModel = &sim.BaseModel{A: A, B: B, C: C, D: D} + okModel, _ = sim.NewDiscrete(A, B, C, D, nil) badModel = &invalidModel{okModel} c = &Config{ diff --git a/particle/bf/bf.go b/particle/bf/bf.go index 47b6c25..74ec4fc 100644 --- a/particle/bf/bf.go +++ b/particle/bf/bf.go @@ -19,7 +19,7 @@ import ( // https://en.wikipedia.org/wiki/Particle_filter#The_bootstrap_filter type BF struct { // model is bootstrap filter model - model filter.Model + model filter.DiscreteModel // w stores particle weights w []float64 // x stores filter particles as column vectors @@ -47,7 +47,7 @@ type BF struct { // - p: number of filter particles // - pdf: Probability Density Function (PDF) of filter output error // New returns error if non-positive number of particles is given or if the particles fail to be generated. -func New(m filter.Model, ic filter.InitCond, q, r filter.Noise, p int, pdf distmv.LogProber) (*BF, error) { +func New(m filter.DiscreteModel, ic filter.InitCond, q, r filter.Noise, p int, pdf distmv.LogProber) (*BF, error) { // must have at least one particle; can't be negative if p <= 0 { return nil, fmt.Errorf("invalid particle count: %d", p) diff --git a/particle/bf/bf_test.go b/particle/bf/bf_test.go index 1f7d0ff..f3be3d5 100644 --- a/particle/bf/bf_test.go +++ b/particle/bf/bf_test.go @@ -13,7 +13,7 @@ import ( ) type invalidModel struct { - filter.Model + filter.DiscreteModel } func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) { @@ -21,7 +21,7 @@ func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) { } var ( - okModel *sim.BaseModel + okModel *sim.Discrete badModel *invalidModel ic *sim.InitCond p int @@ -55,7 +55,7 @@ func setup() { C := mat.NewDense(1, 2, []float64{1.0, 0.0}) D := mat.NewDense(1, 1, []float64{0.0}) - okModel = &sim.BaseModel{A: A, B: B, C: C, D: D} + okModel, _ = sim.NewDiscrete(A, B, C, D, nil) badModel = &invalidModel{okModel} } diff --git a/sim/continuous_time.go b/sim/continuous_time.go new file mode 100644 index 0000000..537c1db --- /dev/null +++ b/sim/continuous_time.go @@ -0,0 +1,109 @@ +package sim + +import ( + "fmt" + + "github.com/milosgajdos/matrix" + "gonum.org/v1/gonum/mat" +) + +// Continuous is a basic model of a linear, continuous-time, dynamical system +type Continuous struct { + System +} + +// NewContinuous creates a linear continuous-time model based on the control theory equations +// which is advanced by timestep dt. +// +// dx/dt = A*x + B*u + E*z (disturbances E not implemented yet) +// y = C*x + D*u +func NewContinuous(A, B, C, D, E *mat.Dense) (*Continuous, error) { + if A == nil { + return nil, fmt.Errorf("system matrix must be defined for a model") + } + sys := newSystem(A, B, C, D, E) + return &Continuous{System: sys}, nil +} + +// ToDiscrete creates a discrete-time model from a continuous time model +// using Ts as the sampling time. +// +// It is calculated using Euler's method, an approximation valid for small timesteps. +// TODO(soypat): implement Discrete system method ToContinuous to be able to test conversion between the two. +func (ct *Continuous) ToDiscrete(Ts float64) (*Discrete, error) { + nx, _, _, _ := ct.SystemDims() + dsys := newSystem(ct.A, ct.B, ct.C, ct.D, ct.E) + // continuous -> discrete time conversion + // See Discrete-Time Control Systems by Katsuhiko Ogata + // Eq. (5-73) p. 315 Second Edition (Spanish) + dsys.A.Scale(Ts, dsys.A) + dsys.A.Exp(dsys.A) + + // shorthand name for discrete B matrix + Bd := dsys.B + Aaux := mat.NewDense(nx, nx, nil) + // Given A is not singular, the following is valid + // Bd(Ts) = (exp(A*Ts) - I)*inv(A)*B Eq. (5-74 bis) Ogata + eye, _ := matrix.NewDenseValIdentity(nx, 1.0) + + Aaux.Sub(dsys.A, eye) + Ainv := mat.NewDense(nx, nx, nil) + err := Ainv.Inverse(ct.A) + if err == nil { + Aaux.Mul(Aaux, Ainv) + // Store subtraction result in Bd + Bd.Mul(Aaux, ct.B) + return &Discrete{dsys}, nil + } + + Asum := Ainv // change identifier to not confuse + Asum.Scale(0, Asum) // reset data + // if A matrix is singular we integrate with closed form + // from 0 to Ts + // Bd = integrate( exp(A*t)dt, 0, Ts ) * B Eq. (5-74) Ogata + const n = 100 // TODO parametrize C2D settings + dt := Ts / float64(n-1) + for i := 0; i < n; i++ { + Aaux.Scale(dt*float64(i), ct.A) + Aaux.Exp(Aaux) + Aaux.Scale(dt, Aaux) + Asum.Add(Asum, Aaux) + } + Bd.Mul(Asum, ct.B) + return &Discrete{dsys}, nil +} + +// Propagate propagates returns the next internal state x +// of a linear, continuous-time system given an input vector u and a +// disturbance input z. (wd is process noise, z not implemented yet). It propagates +// the solution by a timestep `dt`. +func (ct *Continuous) Propagate(x, u, wd mat.Vector, dt float64) (mat.Vector, error) { + nx, nu, _, _ := ct.SystemDims() + if u != nil && u.Len() != nu { + return nil, fmt.Errorf("invalid input vector") + } + + if x.Len() != nx { + return nil, fmt.Errorf("invalid state vector") + } + + out := new(mat.Dense) + out.Mul(ct.A, x) + if u != nil && ct.B != nil { + outU := new(mat.Dense) + outU.Mul(ct.B, u) + + out.Add(out, outU) + } + + if wd != nil && wd.Len() == nx { // TODO change _nx to _nz when switching to z and disturbance matrix implementation + // outZ := new(mat.Dense) // TODO add E disturbance matrix + // outZ.Mul(b.E, z) + // out.Add(out, outZ) + out.Add(out, wd) + } + // integrate the first order derivatives calculated: dx/dt = A*x + B*u + wd + out.Scale(dt, out) + out.Add(x, out) + return out.ColView(0), nil +} diff --git a/sim/discrete_time.go b/sim/discrete_time.go new file mode 100644 index 0000000..843ca4e --- /dev/null +++ b/sim/discrete_time.go @@ -0,0 +1,51 @@ +package sim + +import ( + "fmt" + + "gonum.org/v1/gonum/mat" +) + +// Discrete is a basic model of a linear, discrete-time, dynamical system +type Discrete struct { + System +} + +// NewDiscrete creates a linear discrete-time model based on the control theory equations. +// +// x[n+1] = A*x[n] + B*u[n] + E*z[n] (disturbances E not implemented yet) +// y[n] = C*x[n] + D*u[n] +func NewDiscrete(A, B, C, D, E *mat.Dense) (*Discrete, error) { + if A == nil { + return nil, fmt.Errorf("system matrix must be defined for a model") + } + return &Discrete{System: System{A: A, B: B, C: C, D: D, E: E}}, nil +} + +// Propagate propagates returns the next internal state x +// of a linear, continuous-time system given an input vector u and a +// disturbance input z. (wd is process noise, z not implemented yet) +func (dt *Discrete) Propagate(x, u, wd mat.Vector) (mat.Vector, error) { + nx, nu, _, _ := dt.SystemDims() + if u != nil && u.Len() != nu { + return nil, fmt.Errorf("invalid input vector") + } + + if x.Len() != nx { + return nil, fmt.Errorf("invalid state vector") + } + + out := new(mat.Dense) + out.Mul(dt.A, x) + if u != nil && dt.B != nil { + outU := new(mat.Dense) + outU.Mul(dt.B, u) + + out.Add(out, outU) + } + + if wd != nil && wd.Len() == nx { + out.Add(out, wd) + } + return out.ColView(0), nil +} diff --git a/sim/model.go b/sim/model.go deleted file mode 100644 index b3abc65..0000000 --- a/sim/model.go +++ /dev/null @@ -1,172 +0,0 @@ -package sim - -import ( - "fmt" - - "gonum.org/v1/gonum/mat" -) - -// InitCond implements filter.InitCond -type InitCond struct { - state *mat.VecDense - cov *mat.SymDense -} - -// NewInitCond creates new InitCond and returns it -func NewInitCond(state mat.Vector, cov mat.Symmetric) *InitCond { - s := &mat.VecDense{} - s.CloneFromVec(state) - - c := mat.NewSymDense(cov.Symmetric(), nil) - c.CopySym(cov) - - return &InitCond{ - state: s, - cov: c, - } -} - -// State returns initial state -func (c *InitCond) State() mat.Vector { - state := mat.NewVecDense(c.state.Len(), nil) - state.CloneFromVec(c.state) - - return state -} - -// Cov returns initial covariance -func (c *InitCond) Cov() mat.Symmetric { - cov := mat.NewSymDense(c.cov.Symmetric(), nil) - cov.CopySym(c.cov) - - return cov -} - -// BaseModel is a basic model of a dynamical system -type BaseModel struct { - // A is internal state matrix - A *mat.Dense - // B is control matrix - B *mat.Dense - // C is output state matrix - C *mat.Dense - // D is output control matrix - D *mat.Dense - // E is Disturbance matrix - E *mat.Dense -} - -// NewBaseModel creates a model of falling ball and returns it -func NewBaseModel(A, B, C, D, E *mat.Dense) (*BaseModel, error) { - return &BaseModel{A: A, B: B, C: C, D: D, E: E}, nil -} - -// Propagate propagates internal state x of a falling ball to the next step -// given an input vector u and a disturbance input z. (wd is process noise, z not implemented yet) -func (b *BaseModel) Propagate(x, u, wd mat.Vector) (mat.Vector, error) { - nx, nu, _, _ := b.SystemDims() - if u != nil && u.Len() != nu { - return nil, fmt.Errorf("invalid input vector") - } - - if x.Len() != nx { - return nil, fmt.Errorf("invalid state vector") - } - - out := new(mat.Dense) - out.Mul(b.A, x) - - if u != nil && b.B != nil { - outU := new(mat.Dense) - outU.Mul(b.B, u) - - out.Add(out, outU) - } - - if wd != nil && wd.Len() == nx { // TODO change _nx to _nz when switching to z and disturbance matrix implementation - // outZ := new(mat.Dense) // TODO add E disturbance matrix - // outZ.Mul(b.E, z) - // out.Add(out, outZ) - out.Add(out, wd) - } - - return out.ColView(0), nil -} - -// Observe observes external state of falling ball given internal state x and input u. -// wn is added to the output as a noise vector. -func (b *BaseModel) Observe(x, u, wn mat.Vector) (mat.Vector, error) { - nx, nu, ny, _ := b.SystemDims() - if u != nil && u.Len() != nu { - return nil, fmt.Errorf("invalid input vector") - } - - if x.Len() != nx { - return nil, fmt.Errorf("invalid state vector") - } - - out := new(mat.Dense) - out.Mul(b.C, x) - - if u != nil && b.D != nil { - outU := new(mat.Dense) - outU.Mul(b.D, u) - - out.Add(out, outU) - } - - if wn != nil && wn.Len() == ny { - out.Add(out, wn) - } - - return out.ColView(0), nil -} - -// SystemDims returns internal state length (nx), input vector length (nu), -// external/observable/output state length (ny) and disturbance vector length (nz). -func (b *BaseModel) SystemDims() (nx, nu, ny, nz int) { - nx, _ = b.A.Dims() - if b.B != nil { - _, nu = b.B.Dims() - } - ny, _ = b.C.Dims() - if b.E != nil { - _, nz = b.E.Dims() - } - return nx, nu, ny, nz -} - -// SystemMatrix returns state propagation matrix -func (b *BaseModel) SystemMatrix() mat.Matrix { - m := &mat.Dense{} - m.CloneFrom(b.A) - - return m -} - -// ControlMatrix returns state propagation control matrix -func (b *BaseModel) ControlMatrix() mat.Matrix { - m := &mat.Dense{} - if b.B != nil { - m.CloneFrom(b.B) - } - - return m -} - -// OutputMatrix returns observation matrix -func (b *BaseModel) OutputMatrix() mat.Matrix { - m := &mat.Dense{} - m.CloneFrom(b.C) - - return m -} - -// FeedForwardMatrix returns observation control matrix -func (b *BaseModel) FeedForwardMatrix() mat.Matrix { - m := &mat.Dense{} - if b.D != nil { - m.CloneFrom(b.D) - } - return m -} diff --git a/sim/model_test.go b/sim/model_test.go index 0b71289..ec5a262 100644 --- a/sim/model_test.go +++ b/sim/model_test.go @@ -62,7 +62,7 @@ func TestInitCond(t *testing.T) { func TestBase(t *testing.T) { assert := assert.New(t) - f, err := NewBaseModel(A, B, C, D, E) + f, err := NewDiscrete(A, B, C, D, E) assert.NotNil(f) assert.NoError(err) } @@ -70,7 +70,7 @@ func TestBase(t *testing.T) { func TestBasePropagate(t *testing.T) { assert := assert.New(t) - f, err := NewBaseModel(A, B, C, D, E) + f, err := NewDiscrete(A, B, C, D, E) assert.NotNil(f) assert.NoError(err) @@ -96,7 +96,7 @@ func TestBasePropagate(t *testing.T) { func TestBaseObserve(t *testing.T) { assert := assert.New(t) - f, err := NewBaseModel(A, B, C, D, E) + f, err := NewDiscrete(A, B, C, D, E) assert.NotNil(f) assert.NoError(err) @@ -122,7 +122,7 @@ func TestBaseObserve(t *testing.T) { func TestBaseSystemMatrices(t *testing.T) { assert := assert.New(t) - f, err := NewBaseModel(A, B, C, D, E) + f, err := NewDiscrete(A, B, C, D, E) assert.NotNil(f) assert.NoError(err) @@ -142,7 +142,7 @@ func TestBaseSystemMatrices(t *testing.T) { func TestBaseDims(t *testing.T) { assert := assert.New(t) - f, err := NewBaseModel(A, B, C, D, E) + f, err := NewDiscrete(A, B, C, D, E) assert.NotNil(f) assert.NoError(err) diff --git a/sim/sim.go b/sim/sim.go new file mode 100644 index 0000000..fb7579e --- /dev/null +++ b/sim/sim.go @@ -0,0 +1,39 @@ +package sim + +import "gonum.org/v1/gonum/mat" + +// InitCond implements filter.InitCond +type InitCond struct { + state *mat.VecDense + cov *mat.SymDense +} + +// NewInitCond creates new InitCond and returns it +func NewInitCond(state mat.Vector, cov mat.Symmetric) *InitCond { + s := &mat.VecDense{} + s.CloneFromVec(state) + + c := mat.NewSymDense(cov.Symmetric(), nil) + c.CopySym(cov) + + return &InitCond{ + state: s, + cov: c, + } +} + +// State returns initial state +func (c *InitCond) State() mat.Vector { + state := mat.NewVecDense(c.state.Len(), nil) + state.CloneFromVec(c.state) + + return state +} + +// Cov returns initial covariance +func (c *InitCond) Cov() mat.Symmetric { + cov := mat.NewSymDense(c.cov.Symmetric(), nil) + cov.CopySym(c.cov) + + return cov +} diff --git a/sim/system.go b/sim/system.go new file mode 100644 index 0000000..4a4a53c --- /dev/null +++ b/sim/system.go @@ -0,0 +1,114 @@ +package sim + +import ( + "fmt" + + "gonum.org/v1/gonum/mat" +) + +// System defines a linear model of a plant using +// traditional matrices of modern control theory. +// +// It contains the System (A), input (B), Observation/Output (C) +// Feedthrough (D) and disturbance (E) matrices. +type System struct { + // System/State matrix A + A *mat.Dense + // Control/Input Matrix B + B *mat.Dense + // Observation/Output Matrix C + C *mat.Dense + // Feedthrough matrix D + D *mat.Dense + // Perturbation matrix (related to process noise wd) E + E *mat.Dense +} + +func newSystem(A, B, C, D, E mat.Matrix) System { + sys := System{A: mat.DenseCopyOf(A)} + if B != nil && B.(*mat.Dense) != nil { + sys.B = mat.DenseCopyOf(B) + } + if C != nil && C.(*mat.Dense) != nil { + sys.C = mat.DenseCopyOf(C) + } + if D != nil && D.(*mat.Dense) != nil { + sys.D = mat.DenseCopyOf(D) + } + if E != nil && E.(*mat.Dense) != nil { + sys.E = mat.DenseCopyOf(E) + } + return sys +} + +// SystemDims returns internal state length (nx), input vector length (nu), +// external/observable/output state length (ny) and disturbance vector length (nz). +func (s System) SystemDims() (nx, nu, ny, nz int) { + nx, _ = s.A.Dims() + if s.B != nil { + _, nu = s.B.Dims() + } + if s.C != nil { + ny, _ = s.C.Dims() + } + if s.E != nil { + _, nz = s.E.Dims() + } + return nx, nu, ny, nz +} + +// SystemMatrix returns state propagation matrix `A`. +func (s System) SystemMatrix() (A mat.Matrix) { return s.A } + +// ControlMatrix returns state propagation control matrix `B` +func (s System) ControlMatrix() (B mat.Matrix) { + if s.B == nil { + return nil + } + return s.B +} + +// OutputMatrix returns observation matrix `C` +func (s System) OutputMatrix() (C mat.Matrix) { + if s.C == nil { + return nil + } + return s.C +} + +// FeedForwardMatrix returns observation control matrix `D` +func (s System) FeedForwardMatrix() (D mat.Matrix) { + if s.D == nil { + return nil + } + return s.D +} + +// Observe returns external/observable state given internal state x and input u. +// wn is added to the output as a noise vector. +func (s System) Observe(x, u, wn mat.Vector) (y mat.Vector, err error) { + nx, nu, ny, _ := s.SystemDims() + if u != nil && u.Len() != nu { + return nil, fmt.Errorf("invalid input vector") + } + + if x.Len() != nx { + return nil, fmt.Errorf("invalid state vector") + } + + out := new(mat.Dense) + out.Mul(s.C, x) + + if u != nil && s.D != nil { + outU := new(mat.Dense) + outU.Mul(s.D, u) + + out.Add(out, outU) + } + + if wn != nil && wn.Len() == ny { + out.Add(out, wn) + } + + return out.ColView(0), nil +} diff --git a/smooth/erts/erts.go b/smooth/erts/erts.go index 6f5280e..2cca8e3 100644 --- a/smooth/erts/erts.go +++ b/smooth/erts/erts.go @@ -22,14 +22,14 @@ type ERTS struct { // f is EKF jacobian matrix f *mat.Dense // m is system model - m filter.Model + m filter.DiscreteModel // start is initial condition start filter.InitCond } // New creates new ERTS and returns it. // It returns error if it fails to create ERTS smoother. -func New(m filter.Model, init filter.InitCond, q filter.Noise) (*ERTS, error) { +func New(m filter.DiscreteModel, init filter.InitCond, q filter.Noise) (*ERTS, error) { in, _, out, _ := m.SystemDims() if in <= 0 || out <= 0 { return nil, fmt.Errorf("Invalid model dimensions: [%d x %d]", in, out) diff --git a/smooth/erts/erts_test.go b/smooth/erts/erts_test.go index 5ba5494..d0e2e13 100644 --- a/smooth/erts/erts_test.go +++ b/smooth/erts/erts_test.go @@ -13,7 +13,7 @@ import ( ) type invalidModel struct { - filter.DiscreteModel + filter.DiscreteControlSystem r int c int } @@ -23,7 +23,7 @@ func (m *invalidModel) SystemDims() (int, int, int, int) { } var ( - okModel *sim.BaseModel + okModel *sim.Discrete badModel *invalidModel ic *sim.InitCond q filter.Noise @@ -57,8 +57,8 @@ func setup() { ux = append(ux, u) } - okModel = &sim.BaseModel{A: A, B: B, C: C, D: D} - badModel = &invalidModel{DiscreteModel: okModel, r: 10, c: 10} + okModel, _ = sim.NewDiscrete(A, B, C, D, nil) + badModel = &invalidModel{DiscreteControlSystem: okModel, r: 10, c: 10} } func TestMain(m *testing.M) { diff --git a/smooth/rts/rts.go b/smooth/rts/rts.go index 8b9c3a6..0065d68 100644 --- a/smooth/rts/rts.go +++ b/smooth/rts/rts.go @@ -14,14 +14,14 @@ type RTS struct { // q is state noise a.k.a. process noise q filter.Noise // m is system model - m filter.DiscreteModel + m filter.DiscreteControlSystem // start is initial condition start filter.InitCond } // New creates new RTS and returns it. // It returns error if it fails to create RTS smoother. -func New(m filter.DiscreteModel, init filter.InitCond, q filter.Noise) (*RTS, error) { +func New(m filter.DiscreteControlSystem, init filter.InitCond, q filter.Noise) (*RTS, error) { in, _, out, _ := m.SystemDims() if in <= 0 || out <= 0 { return nil, fmt.Errorf("Invalid model dimensions: [%d x %d]", in, out) diff --git a/smooth/rts/rts_test.go b/smooth/rts/rts_test.go index f0a6dc8..1a24ed9 100644 --- a/smooth/rts/rts_test.go +++ b/smooth/rts/rts_test.go @@ -13,7 +13,7 @@ import ( ) type invalidModel struct { - filter.DiscreteModel + filter.DiscreteControlSystem r int c int } @@ -23,7 +23,7 @@ func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) { } var ( - okModel *sim.BaseModel + okModel *sim.Discrete badModel *invalidModel ic *sim.InitCond q filter.Noise @@ -57,8 +57,8 @@ func setup() { ux = append(ux, u) } - okModel = &sim.BaseModel{A: A, B: B, C: C, D: D} - badModel = &invalidModel{DiscreteModel: okModel, r: 10, c: 10} + okModel, _ = sim.NewDiscrete(A, B, C, D, nil) + badModel = &invalidModel{DiscreteControlSystem: okModel, r: 10, c: 10} } func TestMain(m *testing.M) {