controlsys

package module
v0.5.0 Latest Latest
Warning

This package is not in the latest version of its module.

Go to latest
Published: Mar 23, 2026 License: MIT Imports: 15 Imported by: 0

README

controlsys

Go library for linear state-space control systems.

Supports continuous and discrete LTI models with MIMO capability.

Install

go get github.com/jamestjsp/controlsys

Note: This package depends on a gonum fork for additional LAPACK routines. Add this to your go.mod:

replace gonum.org/v1/gonum => github.com/jamestjsp/gonum v0.17.2-fork

Features

  • Three representations: state-space, transfer function, zero-pole-gain (ZPK) with bidirectional conversion
  • Frequency response: Bode, Nyquist, Nichols, singular values
  • Stability analysis: gain/phase margins, disk margins, bandwidth, damping
  • Control design: LQR, LQE (Kalman), LQI, pole placement, Riccati solvers (CARE/DARE)
  • Model reduction: controllability/observability staircase, balanced realization, balanced truncation
  • System norms: H2 and H-infinity
  • Interconnection: series, parallel, feedback, append, sum blocks
  • Time-domain: step, impulse, initial condition, arbitrary input (lsim), discrete simulation
  • Discretization: ZOH, FOH, Tustin (bilinear), matched pole-zero, discrete-to-discrete resampling
  • Transport delays: input/output/internal delays, Pade and Thiran approximations, LFT representation
  • Transmission zeros & poles via staircase decomposition
  • Gramians: controllability and observability

Quick Start

package main

import (
	"fmt"

	"github.com/jamestjsp/controlsys"
	"gonum.org/v1/gonum/mat"
)

func main() {
	// Double integrator: x'' = u
	A := mat.NewDense(2, 2, []float64{0, 1, 0, 0})
	B := mat.NewDense(2, 1, []float64{0, 1})
	C := mat.NewDense(1, 2, []float64{1, 0})
	D := mat.NewDense(1, 1, []float64{0})

	sys, _ := controlsys.New(A, B, C, D, 0)

	poles, _ := sys.Poles()
	fmt.Println("Poles:", poles)
	stable, _ := sys.IsStable()
	fmt.Println("Stable:", stable)

	tf, _ := sys.TransferFunction(nil)
	fmt.Println("Transfer function:", tf.TF)
}

API Overview

System Construction
Function Description
New Create from A, B, C, D matrices
NewWithDelay Create with transport delays
NewGain Pure feedthrough (D only)
NewFromSlices Create from row-major flat arrays
NewZPK SISO zero-pole-gain model
NewZPKMIMO MIMO zero-pole-gain model
Frequency Response & Plotting
Method Description
FreqResponse H(jw) at given frequencies
Bode Magnitude (dB) and phase (deg) vs frequency
Nyquist Nyquist plot with encirclement counting
Nichols Nichols chart (magnitude vs phase)
Sigma Singular value frequency response
EvalFr Evaluate at arbitrary complex s
Stability & Margins
Function/Method Description
Poles Eigenvalues of A
Zeros Transmission zeros
IsStable Stability check
DCGain Steady-state (DC) gain
Damp Natural frequency, damping ratio, time constant
Margin Gain and phase margins (SISO)
AllMargin All gain/phase crossover points
DiskMargin Disk-based stability margin
Bandwidth -3 dB bandwidth
Control Design
Function Description
Lqr Continuous-time LQR regulator
Dlqr Discrete-time LQR regulator
Lqe Kalman filter (observer) gain
Lqi LQR with integral action
Pole Pole placement
Care Continuous algebraic Riccati equation
Dare Discrete algebraic Riccati equation
Model Reduction
Function/Method Description
Reduce Staircase reduction (controllability/observability)
MinimalRealization Shorthand for full reduction
Balreal Balanced realization
Balred Balanced truncation / singular perturbation
Ctrb Controllability matrix
Obsv Observability matrix
Gram Controllability/observability gramian
System Norms
Function Description
H2Norm H2 norm (RMS gain)
HinfNorm H-infinity norm (peak gain)
Norm General norm computation
Representation Conversion
Function/Method Description
TransferFunction State-space → transfer function
StateSpace Transfer function → state-space
ZPKModel State-space → zero-pole-gain
(ZPK).TransferFunction ZPK → transfer function
(TransferFunc).ZPK Transfer function → ZPK
Discretization
Method Description
Discretize Bilinear (Tustin) c2d
DiscretizeZOH Zero-order hold c2d
DiscretizeFOH First-order hold c2d
DiscretizeMatched Matched pole-zero c2d
DiscretizeD2D Discrete-to-discrete resampling
Undiscretize Bilinear d2c
Interconnection
Function Description
Series Cascade connection
Parallel Sum connection
Feedback Closed-loop with feedback
SafeFeedback Feedback with automatic delay handling
Append Block diagonal concatenation
Sumblk Sum block from string expression
Time-Domain Simulation
Function/Method Description
Step Unit step response
Impulse Unit impulse response
InitialCondition Free response to initial state
Lsim Response to arbitrary input signal
Simulate Discrete-time simulation
GenSig Generate test signals (step, sine, square, pulse)
Transport Delays
Function/Method Description
SetDelay Set MIMO delay matrix
SetInputDelay Set per-input delays
SetOutputDelay Set per-output delays
PadeDelay Pade rational approximation
ThiranDelay Thiran allpass (fractional discrete delays)
Pade Replace all delays with Pade approximations
AbsorbDelay Augment state for discrete delays

Core Algorithms

Algorithm Purpose
Staircase decomposition Transmission zeros via rank-revealing factorization
Column-pivoting QR Rank determination with incremental condition estimation
Row-pivoting RQ Dual rank-revealing factorization
Controllability staircase Subspace decomposition for reduction and transfer functions
Balanced realization Gramian-based state transformation for model reduction
Schur decomposition Riccati equation solvers (CARE/DARE)

License

MIT

Documentation

Overview

Package controlsys provides linear state-space system operations for continuous and discrete LTI models. It supports MIMO systems with transport delays, state-space and transfer-function representations, frequency response analysis, discretization, simulation, and model reduction.

Index

Examples

Constants

This section is empty.

Variables

View Source
var (
	ErrDimensionMismatch = errors.New("controlsys: dimension mismatch")
	ErrSingularTransform = errors.New("controlsys: singular matrix in transformation")
	ErrWrongDomain       = errors.New("controlsys: wrong time domain for operation")
	ErrInvalidSampleTime = errors.New("controlsys: sample time must be positive")
	ErrSingularDenom     = errors.New("controlsys: zero or near-zero leading denominator")
	ErrOverflow          = errors.New("controlsys: coefficient overflow")

	ErrNegativeDelay        = errors.New("controlsys: delay must be non-negative")
	ErrFractionalDelay      = errors.New("controlsys: discrete delay must be non-negative integer")
	ErrNonUniformInputDelay = errors.New("controlsys: AbsorbDelay requires uniform delay per input column")

	ErrZeroInternalDelay = errors.New("controlsys: internal delay must be positive (tau=0 creates algebraic loop)")

	ErrAlgebraicLoop = errors.New("controlsys: algebraic loop: (I-D22) singular")

	ErrDomainMismatch  = errors.New("controlsys: systems must share the same time domain")
	ErrFeedbackDelay   = errors.New("controlsys: feedback with delays not supported")
	ErrMixedDelayTypes = errors.New("controlsys: InternalDelay and IODelay cannot coexist")

	ErrNotSymmetric     = errors.New("controlsys: matrix must be symmetric")
	ErrSchurFailed      = errors.New("controlsys: Schur decomposition failed to converge")
	ErrSingularEquation = errors.New("controlsys: matrix equation is singular or nearly singular")
	ErrNoStabilizing    = errors.New("controlsys: no stabilizing solution exists")
	ErrSingularR        = errors.New("controlsys: R matrix is singular or not positive definite")
	ErrUnstableGramian  = errors.New("controlsys: gramian undefined for unstable system")
	ErrUnstable         = errors.New("controlsys: system is unstable")
	ErrNotMinimal       = errors.New("controlsys: gramian not positive definite; system may not be minimal")
	ErrInvalidOrder     = errors.New("controlsys: reduction order out of range")
	ErrSingularA22      = errors.New("controlsys: A22 block singular; singular perturbation not applicable")

	ErrNotSISO           = errors.New("controlsys: system must be single-input")
	ErrConjugatePairs    = errors.New("controlsys: complex poles must appear in conjugate pairs")
	ErrPoleCount         = errors.New("controlsys: number of poles must equal state dimension")
	ErrUncontrollable    = errors.New("controlsys: uncontrollable mode cannot be assigned")
	ErrInsufficientData  = errors.New("controlsys: insufficient data for estimation")
	ErrInvalidExpression = errors.New("controlsys: invalid sumblk expression")
	ErrSignalNotFound    = errors.New("controlsys: signal name not found")
)

Functions

func Acker added in v0.4.0

func Acker(A, B *mat.Dense, poles []complex128) (*mat.Dense, error)

Acker computes SISO pole placement using Ackermann's formula. Given single-input system (A, B) and desired closed-loop poles, returns gain K (1×n) such that eig(A - B*K) = poles.

Only valid for single-input systems (m=1). Numerically fragile for n > 10.

func Bandwidth added in v0.4.0

func Bandwidth(sys *System, dbDrop float64) (float64, error)

func Ctrb added in v0.4.0

func Ctrb(A, B *mat.Dense) (*mat.Dense, error)

Ctrb returns the n×(n·m) controllability matrix [B, AB, A²B, …, Aⁿ⁻¹B].

func DLyap added in v0.4.0

func DLyap(A, Q *mat.Dense) (*mat.Dense, error)

DLyap solves the discrete Lyapunov equation

A*X*A' - X + Q = 0

A is n×n, Q is n×n symmetric. Returns X (n×n symmetric).

func DecomposeIODelay

func DecomposeIODelay(ioDelay *mat.Dense) (inputDelay, outputDelay []float64, residual *mat.Dense)

func GenSig added in v0.4.0

func GenSig(sigType string, period, dt float64) (t, u []float64, err error)

func H2Norm added in v0.4.0

func H2Norm(sys *System) (float64, error)

H2Norm computes the H2 norm of a stable LTI system.

For continuous systems with D ≠ 0, the H2 norm is infinite.

func HSV added in v0.4.0

func HSV(sys *System) ([]float64, error)

HSV computes the Hankel singular values of a stable LTI system in descending order.

func HinfNorm added in v0.4.0

func HinfNorm(sys *System) (norm float64, omega float64, err error)

HinfNorm computes the H∞ norm (peak gain) of a stable LTI system and the frequency at which it occurs.

func Lyap added in v0.4.0

func Lyap(A, Q *mat.Dense) (*mat.Dense, error)

Lyap solves the continuous Lyapunov equation

A*X + X*A' + Q = 0

A is n×n, Q is n×n symmetric. Returns X (n×n symmetric).

func Obsv added in v0.4.0

func Obsv(A, C *mat.Dense) (*mat.Dense, error)

Obsv returns the (n·p)×n observability matrix [C; CA; CA²; …; CAⁿ⁻¹].

func Place added in v0.4.0

func Place(A, B *mat.Dense, poles []complex128) (*mat.Dense, error)

Place computes state feedback gain F (m×n) via Schur-based pole placement such that eig(A - B*F) equals the desired poles.

Uses Varga's method: Schur decomposition with iterative minimum-norm eigenvalue assignment. Works for both SISO and MIMO systems.

Poles must come in conjugate pairs. len(poles) must equal n.

Types

type AbsorbScope

type AbsorbScope string
const (
	AbsorbInput    AbsorbScope = "input"
	AbsorbOutput   AbsorbScope = "output"
	AbsorbIO       AbsorbScope = "io"
	AbsorbInternal AbsorbScope = "internal"
	AbsorbAll      AbsorbScope = "all"

	DefaultPadeOrder = 5
)

type AllMarginResult added in v0.4.0

type AllMarginResult struct {
	GainMargins     []float64 // dB at each phase crossover
	PhaseMargins    []float64 // degrees at each gain crossover
	GainCrossFreqs  []float64 // omega where |G|=0dB
	PhaseCrossFreqs []float64 // omega where angle(G)=-180deg
}

func AllMargin added in v0.4.0

func AllMargin(sys *System) (*AllMarginResult, error)

type BalrealResult added in v0.4.0

type BalrealResult struct {
	Sys  *System
	HSV  []float64
	T    *mat.Dense
	Tinv *mat.Dense
}

func Balreal added in v0.4.0

func Balreal(sys *System) (*BalrealResult, error)

Balreal computes the balanced realization of a stable, minimal LTI system.

The returned system has equal controllability and observability gramians, both equal to diag(σ₁, σ₂, …, σₙ) where σᵢ are the Hankel singular values.

type BalredMethod added in v0.4.0

type BalredMethod int
const (
	Truncate BalredMethod = iota
	SingularPerturbation
)

type BodeResult

type BodeResult struct {
	Omega []float64

	InputName  []string
	OutputName []string
	// contains filtered or unexported fields
}

func (*BodeResult) MagDBAt

func (b *BodeResult) MagDBAt(freq, output, input int) float64

func (*BodeResult) PhaseAt

func (b *BodeResult) PhaseAt(freq, output, input int) float64

type C2DOptions

type C2DOptions struct {
	Method        string
	ThiranOrder   int
	DelayModeling string
}

type Connection added in v0.4.0

type Connection struct {
	From string
	To   string
	Gain float64
}

type DampInfo added in v0.4.0

type DampInfo struct {
	Pole complex128
	Wn   float64
	Zeta float64
	Tau  float64
}

func Damp added in v0.4.0

func Damp(sys *System) ([]DampInfo, error)
Example
package main

import (
	"fmt"

	"github.com/jamestjsp/controlsys"
	"gonum.org/v1/gonum/mat"
)

func main() {
	wn := 10.0
	zeta := 0.3
	sigma := -zeta * wn
	A := mat.NewDense(2, 2, []float64{0, 1, -(wn * wn), 2 * sigma})
	B := mat.NewDense(2, 1, []float64{0, 1})
	C := mat.NewDense(1, 2, []float64{1, 0})
	D := mat.NewDense(1, 1, []float64{0})
	sys, _ := controlsys.New(A, B, C, D, 0)

	info, _ := controlsys.Damp(sys)
	fmt.Printf("Wn:   %.1f rad/s\n", info[0].Wn)
	fmt.Printf("Zeta: %.1f\n", info[0].Zeta)

}
Output:
Wn:   10.0 rad/s
Zeta: 0.3

type DiskMarginResult added in v0.4.0

type DiskMarginResult struct {
	Alpha           float64    // disk margin = 1/Ms
	GainMargin      [2]float64 // [low, high] linear gain factors
	GainMarginDB    [2]float64 // [low, high] in dB
	PhaseMargin     float64    // +/- degrees
	PeakSensitivity float64    // Ms = ||S||_inf
	PeakFreq        float64    // omega where sensitivity peaks
}

func DiskMargin added in v0.4.0

func DiskMargin(sys *System) (*DiskMarginResult, error)

type FreqRespEstOpts added in v0.4.0

type FreqRespEstOpts struct {
	NFFT     int
	Window   func([]float64) []float64
	NOverlap int
	Method   string // "h1" (default), "h2", "fft"
}

type FreqRespEstResult added in v0.4.0

type FreqRespEstResult struct {
	H         *FreqResponseMatrix
	Omega     []float64
	Coherence []float64
}

func FreqRespEst added in v0.4.0

func FreqRespEst(input, output *mat.Dense, dt float64, opts *FreqRespEstOpts) (*FreqRespEstResult, error)

func (*FreqRespEstResult) CoherenceAt added in v0.4.0

func (r *FreqRespEstResult) CoherenceAt(freq, output, input int) float64

type FreqResponseMatrix

type FreqResponseMatrix struct {
	Data       []complex128
	NFreq      int
	P, M       int
	InputName  []string
	OutputName []string
}

func (*FreqResponseMatrix) At

func (f *FreqResponseMatrix) At(freq, output, input int) complex128

type GramResult added in v0.4.0

type GramResult struct {
	X *mat.Dense
	L *mat.Dense
}

func Gram added in v0.4.0

func Gram(sys *System, typ GramType) (*GramResult, error)

Gram computes the controllability or observability gramian of a stable LTI system.

Controllability gramian Wc satisfies:

Continuous: A·Wc + Wc·A' + B·B' = 0
Discrete:   A·Wc·A' - Wc + B·B' = 0

Observability gramian Wo satisfies:

Continuous: A'·Wo + Wo·A + C'·C = 0
Discrete:   A'·Wo·A - Wo + C'·C = 0

type GramType added in v0.4.0

type GramType int
const (
	GramControllability GramType = iota
	GramObservability
)

type LFTDelay added in v0.4.0

type LFTDelay struct {
	Tau                   []float64
	B2, C2, D12, D21, D22 *mat.Dense
}

LFTDelay holds the internal delay representation using a linear fractional transformation (LFT) structure.

type MarginResult added in v0.4.0

type MarginResult struct {
	GainMargin  float64 // dB; +Inf if no phase crossover
	PhaseMargin float64 // degrees; +Inf if no gain crossover
	WgFreq      float64 // gain crossover freq (0dB); NaN if none
	WpFreq      float64 // phase crossover freq (-180deg); NaN if none
}

func Margin added in v0.4.0

func Margin(sys *System) (*MarginResult, error)

type NicholsResult added in v0.4.0

type NicholsResult struct {
	Omega []float64

	InputName  []string
	OutputName []string
	// contains filtered or unexported fields
}

NicholsResult holds Nichols chart data: open-loop phase (degrees) vs magnitude (dB).

func (*NicholsResult) MagDBAt added in v0.4.0

func (r *NicholsResult) MagDBAt(freq, output, input int) float64

func (*NicholsResult) PhaseAt added in v0.4.0

func (r *NicholsResult) PhaseAt(freq, output, input int) float64

type NyquistResult added in v0.4.0

type NyquistResult struct {
	Omega         []float64
	Contour       []complex128
	ContourN      []complex128
	Encirclements int
	RHPPoles      int
	RHPZerosCL    int
}

type Poly

type Poly []float64

Poly represents a polynomial in descending power:

Poly{1, -3, 2} = s² - 3s + 2

func (Poly) Add

func (p Poly) Add(q Poly) Poly

func (Poly) AddTo

func (p Poly) AddTo(dst Poly, q Poly) Poly

func (Poly) Degree

func (p Poly) Degree() int

func (Poly) Equal

func (p Poly) Equal(q Poly, tol float64) bool

func (Poly) Eval

func (p Poly) Eval(s complex128) complex128

func (Poly) IsMonic

func (p Poly) IsMonic() bool

func (Poly) Monic

func (p Poly) Monic() (Poly, error)

func (Poly) Mul

func (p Poly) Mul(q Poly) Poly

func (Poly) MulTo

func (p Poly) MulTo(dst Poly, q Poly) Poly

func (Poly) Roots added in v0.4.0

func (p Poly) Roots() ([]complex128, error)

func (Poly) Scale

func (p Poly) Scale(s float64) Poly

func (Poly) ScaleTo

func (p Poly) ScaleTo(dst Poly, s float64) Poly

type ReduceMode

type ReduceMode int
const (
	ReduceAll ReduceMode = iota
	ReduceUncontrollable
	ReduceUnobservable
)

type ReduceOpts

type ReduceOpts struct {
	Mode     ReduceMode
	Tol      float64
	Equalize bool
}

type ReduceResult

type ReduceResult struct {
	Sys        *System
	Order      int
	BlockSizes []int
}

type Response

type Response struct {
	Y      *mat.Dense
	XFinal *mat.VecDense
}

type RiccatiOpts added in v0.4.0

type RiccatiOpts struct {
	S         *mat.Dense
	Workspace *RiccatiWorkspace
}

type RiccatiResult added in v0.4.0

type RiccatiResult struct {
	X    *mat.Dense
	K    *mat.Dense
	Eig  []complex128
	Rcnd float64
}

func Care added in v0.4.0

func Care(A, B, Q, R *mat.Dense, opts *RiccatiOpts) (*RiccatiResult, error)

Care solves the continuous algebraic Riccati equation:

A'X + XA - (XB+S)*R⁻¹*(B'X+S') + Q = 0

When opts is nil or opts.S is nil, the cross-term is zero:

A'X + XA - XB*R⁻¹*B'X + Q = 0

A is n×n, B is n×m, Q is n×n symmetric, R is m×m symmetric positive definite.

func Dare added in v0.4.0

func Dare(A, B, Q, R *mat.Dense, opts *RiccatiOpts) (*RiccatiResult, error)

Dare solves the discrete algebraic Riccati equation:

A'XA - X - (A'XB+S)*(R+B'XB)⁻¹*(B'XA+S') + Q = 0

When opts is nil or opts.S is nil, the cross-term is zero:

A'XA - X - A'XB*(R+B'XB)⁻¹*B'XA + Q = 0

A is n×n, B is n×m, Q is n×n symmetric, R is m×m symmetric positive definite.

func Dlqr added in v0.4.0

func Dlqr(A, B, Q, R *mat.Dense, opts *RiccatiOpts) (*RiccatiResult, error)

Dlqr solves the discrete-time linear-quadratic regulator problem. It computes the optimal gain K such that u[k] = -Kx[k] minimizes J = sum(x'Qx + u'Ru) for the system x[k+1] = Ax[k] + Bu[k].

A is n×n, B is n×m, Q is n×n symmetric PSD, R is m×m symmetric PD. Returns gain K (m×n), Riccati solution X, and closed-loop eigenvalues.

func Kalman added in v0.4.0

func Kalman(sys *System, Qn, Rn *mat.Dense, opts *RiccatiOpts) (*RiccatiResult, error)

Kalman computes the Kalman filter gain for a state-space system. Noise is assumed to enter through the input matrix B (G = B). Automatically handles continuous (CARE) and discrete (DARE) systems.

Qn is m×m (process noise covariance), Rn is p×p (measurement noise covariance).

func Kalmd added in v0.4.0

func Kalmd(sys *System, Qn, Rn *mat.Dense, dt float64, opts *RiccatiOpts) (*RiccatiResult, error)

Kalmd computes the discrete Kalman filter gain from a continuous plant using Van Loan's method for noise covariance discretization.

sys must be continuous. Qn is m×m, Rn is p×p, dt > 0.

func Lqe added in v0.4.0

func Lqe(A, G, C, Qn, Rn *mat.Dense, opts *RiccatiOpts) (*RiccatiResult, error)

Lqe computes the Kalman estimator gain via duality with LQR. It solves the continuous CARE for the dual system (A', C', G*Qn*G', Rn) and returns observer gain L (n×p) such that eig(A - L*C) is stable.

A is n×n, G is n×g (noise input), C is p×n, Qn is g×g, Rn is p×p.

func Lqi added in v0.4.0

func Lqi(A, B, C, Q, R *mat.Dense, opts *RiccatiOpts) (*RiccatiResult, error)

Lqi computes the linear-quadratic regulator with integral action. The system is augmented with integral states: xi = -C*x, giving Aaug = [A, 0; -C, 0] and Baug = [B; 0].

A is n×n, B is n×m, C is p×n. Q is (n+p)×(n+p), R is m×m. Returns gain K of size m×(n+p) where K = [Kx, Ki].

func Lqr added in v0.4.0

func Lqr(A, B, Q, R *mat.Dense, opts *RiccatiOpts) (*RiccatiResult, error)

Lqr solves the continuous-time linear-quadratic regulator problem. It computes the optimal gain K such that u = -Kx minimizes J = integral(x'Qx + u'Ru) for the system dx/dt = Ax + Bu.

A is n×n, B is n×m, Q is n×n symmetric PSD, R is m×m symmetric PD. Returns gain K (m×n), Riccati solution X, and closed-loop eigenvalues.

func Lqrd added in v0.4.0

func Lqrd(A, B, Q, R *mat.Dense, dt float64, opts *RiccatiOpts) (*RiccatiResult, error)

Lqrd computes the discrete-time LQR gain from a continuous-time plant. It discretizes (A, B) using zero-order hold with sample time dt, then solves the discrete LQR problem.

A is n×n, B is n×m, Q is n×n, R is m×m, dt > 0.

type RiccatiWorkspace added in v0.5.0

type RiccatiWorkspace struct {
	// contains filtered or unexported fields
}

func NewRiccatiWorkspace added in v0.5.0

func NewRiccatiWorkspace(n, m int) *RiccatiWorkspace

type SafeFeedbackOption

type SafeFeedbackOption func(*safeFeedbackConfig)

func WithPadeOrder

func WithPadeOrder(n int) SafeFeedbackOption

func WithThiranOrder

func WithThiranOrder(n int) SafeFeedbackOption

type SigmaResult added in v0.4.0

type SigmaResult struct {
	Omega []float64

	InputName  []string
	OutputName []string
	// contains filtered or unexported fields
}

SigmaResult holds singular value frequency response data.

func (*SigmaResult) At added in v0.4.0

func (r *SigmaResult) At(freq, svIndex int) float64

func (*SigmaResult) NSV added in v0.4.0

func (r *SigmaResult) NSV() int

type SimulateOpts

type SimulateOpts struct {
	Workspace *mat.VecDense
	// contains filtered or unexported fields
}

type StaircaseResult

type StaircaseResult struct {
	A          *mat.Dense
	B          *mat.Dense
	C          *mat.Dense
	NCont      int
	BlockSizes []int
}

func ControllabilityStaircase

func ControllabilityStaircase(A, B, C *mat.Dense, tol float64) *StaircaseResult

func CtrbF added in v0.4.0

func CtrbF(A, B, C *mat.Dense) (*StaircaseResult, error)

CtrbF computes the controllability staircase form. C may be nil if not needed in the transformed system.

func ObsvF added in v0.4.0

func ObsvF(A, B, C *mat.Dense) (*StaircaseResult, error)

ObsvF computes the observability staircase form via duality. B may be nil if not needed in the transformed system.

type StateSpaceOpts

type StateSpaceOpts struct {
	MinimalTol float64
}

type StateSpaceResult

type StateSpaceResult struct {
	Sys          *System
	MinimalOrder int
	BlockSizes   []int
}

type System

type System struct {
	A           *mat.Dense
	B           *mat.Dense
	C           *mat.Dense
	D           *mat.Dense
	Delay       *mat.Dense // p×m IODelay; nil = no delay
	InputDelay  []float64  // length m; nil = zeros
	OutputDelay []float64  // length p; nil = zeros
	LFT         *LFTDelay  // internal delays; nil = none

	Dt float64

	InputName  []string
	OutputName []string
	StateName  []string
	Notes      string
}

System represents a linear time-invariant (LTI) state-space model:

Continuous: dx/dt = Ax + Bu,  y = Cx + Du
Discrete:   x[k+1] = Ax[k] + Bu[k],  y[k] = Cx[k] + Du[k]

func Append

func Append(sys1, sys2 *System) (*System, error)

func Balred added in v0.4.0

func Balred(sys *System, order int, method BalredMethod) (*System, []float64, error)

Balred computes a reduced-order model via balanced truncation.

If order is 0, the truncation order is automatically selected based on the largest relative gap in the Hankel singular values. method selects between Truncate (default) and SingularPerturbation (DC-gain matching).

func BlkDiag added in v0.4.0

func BlkDiag(systems ...*System) (*System, error)

func Connect added in v0.4.0

func Connect(sys *System, Q *mat.Dense, inputs, outputs []int) (*System, error)

func ConnectByName added in v0.4.0

func ConnectByName(systems []*System, connections []Connection, inputs, outputs []string) (*System, error)

func Estim added in v0.4.0

func Estim(sys *System, L *mat.Dense) (*System, error)

Estim constructs an estimator system from a plant and observer gain L. The estimator takes [u; y] as input and produces [y_hat; x_hat] as output.

L is n×p. Returns system with n states, (m+p) inputs, (p+n) outputs.

func Feedback

func Feedback(plant, controller *System, sign float64) (*System, error)
Example
package main

import (
	"fmt"

	"github.com/jamestjsp/controlsys"
	"gonum.org/v1/gonum/mat"
)

func main() {
	// Plant: 1/(s+1)
	Ap := mat.NewDense(1, 1, []float64{-1})
	Bp := mat.NewDense(1, 1, []float64{1})
	Cp := mat.NewDense(1, 1, []float64{1})
	Dp := mat.NewDense(1, 1, []float64{0})
	plant, _ := controlsys.New(Ap, Bp, Cp, Dp, 0)

	// Unity negative feedback: T = P/(1+P) = 1/(s+2)
	cl, _ := controlsys.Feedback(plant, nil, -1)

	poles, _ := cl.Poles()
	fmt.Printf("Closed-loop pole: %.1f\n", real(poles[0]))

}
Output:
Closed-loop pole: -2.0

func LFT added in v0.4.0

func LFT(M, Delta *System, nu, ny int) (*System, error)

func Modred added in v0.4.0

func Modred(sys *System, elim []int, method BalredMethod) (*System, error)

Modred reduces a system by eliminating specified states.

elim contains 0-based indices of states to remove. method selects Truncate or SingularPerturbation (DC-gain matching).

func New

func New(A, B, C, D *mat.Dense, dt float64) (*System, error)

New validates dimension compatibility and returns a System. Matrices are copied to prevent aliasing bugs.

Example
package main

import (
	"fmt"

	"github.com/jamestjsp/controlsys"
	"gonum.org/v1/gonum/mat"
)

func main() {
	A := mat.NewDense(2, 2, []float64{0, 1, 0, 0})
	B := mat.NewDense(2, 1, []float64{0, 1})
	C := mat.NewDense(1, 2, []float64{1, 0})
	D := mat.NewDense(1, 1, []float64{0})

	sys, _ := controlsys.New(A, B, C, D, 0)

	poles, _ := sys.Poles()
	fmt.Println("Poles:", poles)

	stable, _ := sys.IsStable()
	fmt.Println("Stable:", stable)

}
Output:
Poles: [(0+0i) (0+0i)]
Stable: false

func NewFromSlices

func NewFromSlices(n, m, p int, a, b, c, d []float64, dt float64) (*System, error)

func NewGain

func NewGain(D *mat.Dense, dt float64) (*System, error)

func NewWithDelay

func NewWithDelay(A, B, C, D, delay *mat.Dense, dt float64) (*System, error)

func PadeDelay

func PadeDelay(tau float64, order int) (*System, error)

func Parallel

func Parallel(sys1, sys2 *System) (*System, error)

func Reg added in v0.4.0

func Reg(sys *System, K, L *mat.Dense) (*System, error)

Reg constructs a regulator (observer-based controller) from a plant, state-feedback gain K, and observer gain L. The controller takes y (p) as input and produces u (m) as output.

K is m×n, L is n×p. Returns system with n states, p inputs, m outputs.

func SafeFeedback

func SafeFeedback(plant, controller *System, sign float64, opts ...SafeFeedbackOption) (*System, error)

func Series

func Series(sys1, sys2 *System) (*System, error)

func SetDelayModel

func SetDelayModel(H *System, tau []float64) (*System, error)

func SumBlk added in v0.4.0

func SumBlk(expr string, widths ...int) (*System, error)

func ThiranDelay

func ThiranDelay(tau float64, order int, dt float64) (*System, error)

ThiranDelay returns a discrete-time allpass state-space system approximating a fractional delay of tau seconds at sample time dt. The order parameter controls the filter order (1-10). The delay must satisfy tau/dt >= order-0.5 for stability.

func (*System) AbsorbDelay

func (sys *System) AbsorbDelay(scopes ...AbsorbScope) (*System, error)

func (*System) Bode

func (sys *System) Bode(omega []float64, nPoints int) (*BodeResult, error)

func (*System) Copy

func (sys *System) Copy() *System

func (*System) D2D added in v0.4.0

func (sys *System) D2D(newDt float64, opts C2DOptions) (*System, error)

func (*System) DCGain added in v0.4.0

func (sys *System) DCGain() (*mat.Dense, error)
Example
package main

import (
	"fmt"

	"github.com/jamestjsp/controlsys"
	"gonum.org/v1/gonum/mat"
)

func main() {
	A := mat.NewDense(1, 1, []float64{-2})
	B := mat.NewDense(1, 1, []float64{1})
	C := mat.NewDense(1, 1, []float64{1})
	D := mat.NewDense(1, 1, []float64{0})
	sys, _ := controlsys.New(A, B, C, D, 0)

	g, _ := sys.DCGain()
	fmt.Printf("DC gain: %.2f\n", g.At(0, 0))

}
Output:
DC gain: 0.50

func (*System) Dims

func (sys *System) Dims() (n, m, p int)

func (*System) Discretize

func (sys *System) Discretize(dt float64) (*System, error)
Example
package main

import (
	"fmt"

	"github.com/jamestjsp/controlsys"
	"gonum.org/v1/gonum/mat"
)

func main() {
	A := mat.NewDense(1, 1, []float64{-1})
	B := mat.NewDense(1, 1, []float64{1})
	C := mat.NewDense(1, 1, []float64{1})
	D := mat.NewDense(1, 1, []float64{0})
	sys, _ := controlsys.New(A, B, C, D, 0)

	sd, _ := sys.Discretize(0.1)
	fmt.Printf("Discrete: dt=%.1f\n", sd.Dt)
	fmt.Printf("A[0,0]: %.4f\n", sd.A.At(0, 0))

}
Output:
Discrete: dt=0.1
A[0,0]: 0.9048

func (*System) DiscretizeFOH added in v0.4.0

func (sys *System) DiscretizeFOH(dt float64) (*System, error)

func (*System) DiscretizeImpulse added in v0.4.0

func (sys *System) DiscretizeImpulse(dt float64) (*System, error)

func (*System) DiscretizeMatched added in v0.4.0

func (sys *System) DiscretizeMatched(dt float64) (*System, error)

func (*System) DiscretizeWithOpts

func (sys *System) DiscretizeWithOpts(dt float64, opts C2DOptions) (*System, error)

func (*System) DiscretizeZOH

func (sys *System) DiscretizeZOH(dt float64) (*System, error)

func (*System) EvalFr

func (sys *System) EvalFr(s complex128) ([][]complex128, error)

func (*System) FreqResponse

func (sys *System) FreqResponse(omega []float64) (*FreqResponseMatrix, error)
Example
package main

import (
	"fmt"
	"math"
	"math/cmplx"

	"github.com/jamestjsp/controlsys"
	"gonum.org/v1/gonum/mat"
)

func main() {
	// First-order lowpass: H(s) = 1/(s+1), |H(j*1)| = 1/sqrt(2)
	A := mat.NewDense(1, 1, []float64{-1})
	B := mat.NewDense(1, 1, []float64{1})
	C := mat.NewDense(1, 1, []float64{1})
	D := mat.NewDense(1, 1, []float64{0})
	sys, _ := controlsys.New(A, B, C, D, 0)

	omega := []float64{1.0}
	fr, _ := sys.FreqResponse(omega)
	mag := 20 * math.Log10(cmplx.Abs(fr.At(0, 0, 0)))
	fmt.Printf("Gain at w=1: %.2f dB\n", mag)
	fmt.Printf("Expected:    %.2f dB\n", 20*math.Log10(1/math.Sqrt(2)))

}
Output:
Gain at w=1: -3.01 dB
Expected:    -3.01 dB

func (*System) GetDelayModel

func (sys *System) GetDelayModel() (H *System, tau []float64)

func (*System) HasDelay

func (sys *System) HasDelay() bool

func (*System) HasInternalDelay

func (sys *System) HasInternalDelay() bool

func (*System) IsContinuous

func (sys *System) IsContinuous() bool

func (*System) IsDiscrete

func (sys *System) IsDiscrete() bool

func (*System) IsStable

func (sys *System) IsStable() (bool, error)

func (*System) MinimalLFT

func (sys *System) MinimalLFT() (*System, error)

func (*System) MinimalRealization

func (sys *System) MinimalRealization() (*ReduceResult, error)

func (*System) Nichols added in v0.4.0

func (sys *System) Nichols(omega []float64, nPoints int) (*NicholsResult, error)

func (*System) Nyquist added in v0.4.0

func (sys *System) Nyquist(omega []float64, nPoints int) (*NyquistResult, error)

func (*System) Pade

func (sys *System) Pade(order int) (*System, error)

func (*System) Poles

func (sys *System) Poles() ([]complex128, error)

func (*System) PullDelaysToLFT

func (sys *System) PullDelaysToLFT() (*System, error)

PullDelaysToLFT returns a copy of the system where all I/O delays (InputDelay, OutputDelay, and IODelay) have been pulled into the InternalDelay (LFT) structure.

func (*System) Reduce

func (sys *System) Reduce(opts *ReduceOpts) (*ReduceResult, error)

func (*System) SelectByIndex added in v0.4.0

func (sys *System) SelectByIndex(inputs, outputs []int) (*System, error)

func (*System) SelectByName added in v0.4.0

func (sys *System) SelectByName(inputs, outputs []string) (*System, error)

func (*System) SetDelay

func (sys *System) SetDelay(delay *mat.Dense) error

func (*System) SetInputDelay

func (sys *System) SetInputDelay(delay []float64) error

func (*System) SetInputName added in v0.4.0

func (sys *System) SetInputName(names ...string) error

func (*System) SetInternalDelay

func (sys *System) SetInternalDelay(tau []float64, B2, C2, D12, D21, D22 *mat.Dense) error

func (*System) SetOutputDelay

func (sys *System) SetOutputDelay(delay []float64) error

func (*System) SetOutputName added in v0.4.0

func (sys *System) SetOutputName(names ...string) error

func (*System) SetStateName added in v0.4.0

func (sys *System) SetStateName(names ...string) error

func (*System) Sigma added in v0.4.0

func (sys *System) Sigma(omega []float64, nPoints int) (*SigmaResult, error)

func (*System) Simulate

func (sys *System) Simulate(u *mat.Dense, x0 *mat.VecDense, opts *SimulateOpts) (*Response, error)
Example
package main

import (
	"fmt"

	"github.com/jamestjsp/controlsys"
	"gonum.org/v1/gonum/mat"
)

func main() {
	// Discrete integrator: y[k] = y[k-1] + u[k]
	A := mat.NewDense(1, 1, []float64{1})
	B := mat.NewDense(1, 1, []float64{1})
	C := mat.NewDense(1, 1, []float64{1})
	D := mat.NewDense(1, 1, []float64{1})
	sys, _ := controlsys.New(A, B, C, D, 0.1)

	// u is (nInputs x nSteps)
	u := mat.NewDense(1, 5, []float64{1, 1, 1, 1, 1})
	resp, _ := sys.Simulate(u, nil, nil)

	_, cols := resp.Y.Dims()
	for i := 0; i < cols; i++ {
		fmt.Printf("y[%d] = %.0f\n", i, resp.Y.At(0, i))
	}

}
Output:
y[0] = 1
y[1] = 2
y[2] = 3
y[3] = 4
y[4] = 5

func (*System) String added in v0.4.0

func (sys *System) String() string

func (*System) TotalDelay

func (sys *System) TotalDelay() *mat.Dense

func (*System) TransferFunction

func (sys *System) TransferFunction(opts *TransferFuncOpts) (*TransferFuncResult, error)

func (*System) Undiscretize

func (sys *System) Undiscretize() (*System, error)

func (*System) ZPKModel added in v0.4.0

func (sys *System) ZPKModel(opts *TransferFuncOpts) (*ZPKResult, error)

func (*System) ZeroDelayApprox

func (sys *System) ZeroDelayApprox() (*System, error)

func (*System) Zeros

func (sys *System) Zeros() ([]complex128, error)

func (*System) ZerosDetail

func (sys *System) ZerosDetail() (*ZerosResult, error)

type TimeResponse added in v0.4.0

type TimeResponse struct {
	T          []float64
	Y          *mat.Dense
	OutputName []string
}

func Impulse added in v0.4.0

func Impulse(sys *System, tFinal float64) (*TimeResponse, error)

func Initial added in v0.4.0

func Initial(sys *System, x0 *mat.VecDense, tFinal float64) (*TimeResponse, error)

func Step added in v0.4.0

func Step(sys *System, tFinal float64) (*TimeResponse, error)
Example
package main

import (
	"fmt"

	"github.com/jamestjsp/controlsys"
	"gonum.org/v1/gonum/mat"
)

func main() {
	A := mat.NewDense(1, 1, []float64{-1})
	B := mat.NewDense(1, 1, []float64{1})
	C := mat.NewDense(1, 1, []float64{1})
	D := mat.NewDense(1, 1, []float64{0})
	sys, _ := controlsys.New(A, B, C, D, 0)

	resp, _ := controlsys.Step(sys, 7.0)
	last := len(resp.T) - 1
	fmt.Printf("Steady-state: %.2f\n", resp.Y.At(0, last))

}
Output:
Steady-state: 1.00

type TransferFunc

type TransferFunc struct {
	Num        [][][]float64
	Den        [][]float64
	Delay      [][]float64 // [p][m]; nil = no delay
	Dt         float64
	InputName  []string
	OutputName []string
}

func (*TransferFunc) Dims

func (tf *TransferFunc) Dims() (p, m int)

func (*TransferFunc) Eval

func (tf *TransferFunc) Eval(s complex128) [][]complex128

func (*TransferFunc) EvalMulti

func (tf *TransferFunc) EvalMulti(freqs []complex128) [][][]complex128

func (*TransferFunc) HasDelay

func (tf *TransferFunc) HasDelay() bool

func (*TransferFunc) StateSpace

func (tf *TransferFunc) StateSpace(opts *StateSpaceOpts) (*StateSpaceResult, error)

func (*TransferFunc) ZPK added in v0.4.0

func (tf *TransferFunc) ZPK() (*ZPK, error)

type TransferFuncOpts

type TransferFuncOpts struct {
	ControllabilityTol float64
	ObservabilityTol   float64
}

type TransferFuncResult

type TransferFuncResult struct {
	TF           *TransferFunc
	MinimalOrder int
	RowDegrees   []int
}

type ZPK added in v0.4.0

type ZPK struct {
	Zeros      [][][]complex128
	Poles      [][][]complex128
	Gain       [][]float64
	Dt         float64
	InputName  []string
	OutputName []string
}

func NewZPK added in v0.4.0

func NewZPK(zeros, poles []complex128, gain, dt float64) (*ZPK, error)

func NewZPKMIMO added in v0.4.0

func NewZPKMIMO(zeros, poles [][][]complex128, gain [][]float64, dt float64) (*ZPK, error)

func (*ZPK) Dims added in v0.4.0

func (z *ZPK) Dims() (p, m int)

func (*ZPK) Eval added in v0.4.0

func (z *ZPK) Eval(s complex128) [][]complex128

func (*ZPK) FreqResponse added in v0.4.0

func (z *ZPK) FreqResponse(omega []float64) (*FreqResponseMatrix, error)

func (*ZPK) IsContinuous added in v0.4.0

func (z *ZPK) IsContinuous() bool

func (*ZPK) IsDiscrete added in v0.4.0

func (z *ZPK) IsDiscrete() bool

func (*ZPK) StateSpace added in v0.4.0

func (z *ZPK) StateSpace(opts *StateSpaceOpts) (*StateSpaceResult, error)

func (*ZPK) TransferFunction added in v0.4.0

func (z *ZPK) TransferFunction() (*TransferFunc, error)

type ZPKResult added in v0.4.0

type ZPKResult struct {
	ZPK          *ZPK
	MinimalOrder int
	RowDegrees   []int
}

type ZerosResult

type ZerosResult struct {
	Zeros []complex128
	Rank  int
}

Jump to

Keyboard shortcuts

? : This menu
/ : Search site
f or F : Jump to
y or Y : Canonical URL