Create distinction between discrete and continuous systems

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

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
 }
 

kalman/ekf/ekf_test.go

@@ -12,15 +12,15 @@ import (
 )
 
 type invalidModel struct {
-	filter.Model
+	filter.DiscreteModel
 }
 
 func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) {
 	return -10, 0, 8, 0 // a system may have 0 inputs, this is not "invalid". Negative dimension is invalid
 }
 
 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}
 }
 

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

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
 }
 

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

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
 }
 

kalman/ukf/ukf_test.go

@@ -12,15 +12,15 @@ import (
 )
 
 type invalidModel struct {
-	filter.Model
+	filter.DiscreteModel
 }
 
 func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) {
 	return -10, 0, 8, 0
 }
 
 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{

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)

particle/bf/bf_test.go

@@ -13,15 +13,15 @@ import (
 )
 
 type invalidModel struct {
-	filter.Model
+	filter.DiscreteModel
 }
 
 func (m *invalidModel) SystemDims() (nx, nu, ny, nz int) {
 	return -10, 0, 8, 0
 }
 
 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}
 }
 

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