From 1578010909fc759651a04dbf6cfb6a4a24b8a609 Mon Sep 17 00:00:00 2001 From: James Joseph Date: Thu, 2 Apr 2026 20:51:01 -0500 Subject: [PATCH 1/8] feat: add 25 MATLAB Control System Toolbox parity functions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Phase 1 — Quick wins: Norm, Covar, Lsim, Augstate, SS2SS, Xperm, Inv, Pzmap, Isproper Phase 2 — PID controllers: NewPID (parallel form), NewPIDStd (standard form), PID2 (2-DOF), Pidtune (auto-tuning: P/I/PI/PD/PID/PIDF via open-loop shaping) Phase 3 — Analysis & canonical forms: Canon (modal + companion), Loopsens, Rss/Drss, Ssbal, Prescale Phase 4 — Frequency response data model: FRD type, FRDSeries/FRDParallel/FRDFeedback, FRD Nyquist/Sigma/Margin Phase 5 — Structural decomposition: Sminreal, Stabsep, Modsep (shared eigdecomp core with Schur fallback) Also fixes README inaccuracies (Norm, Lsim, InitialCondition). Co-Authored-By: Claude Opus 4.6 (1M context) --- README.md | 4 +- augstate.go | 73 ++++++ canon.go | 307 ++++++++++++++++++++++++ canon_test.go | 220 +++++++++++++++++ covar.go | 64 +++++ eigdecomp.go | 426 +++++++++++++++++++++++++++++++++ frd.go | 529 +++++++++++++++++++++++++++++++++++++++++ frd_test.go | 498 +++++++++++++++++++++++++++++++++++++++ inv.go | 69 ++++++ loopsens.go | 61 +++++ loopsens_test.go | 142 +++++++++++ modsep.go | 28 +++ modsep_test.go | 191 +++++++++++++++ norm_covar_test.go | 286 ++++++++++++++++++++++ norms.go | 14 ++ pid.go | 307 ++++++++++++++++++++++++ pid_test.go | 438 ++++++++++++++++++++++++++++++++++ pidtune.go | 298 +++++++++++++++++++++++ pidtune_test.go | 255 ++++++++++++++++++++ prescale.go | 117 +++++++++ prescale_test.go | 194 +++++++++++++++ pzmap.go | 18 ++ random.go | 110 +++++++++ random_test.go | 138 +++++++++++ response.go | 43 ++++ sminreal.go | 113 +++++++++ sminreal_test.go | 186 +++++++++++++++ ssbal.go | 116 +++++++++ ssbal_test.go | 116 +++++++++ stabsep.go | 24 ++ stabsep_test.go | 323 +++++++++++++++++++++++++ transfer.go | 18 ++ transform.go | 75 ++++++ utils_test.go | 573 +++++++++++++++++++++++++++++++++++++++++++++ 34 files changed, 6371 insertions(+), 3 deletions(-) create mode 100644 augstate.go create mode 100644 canon.go create mode 100644 canon_test.go create mode 100644 covar.go create mode 100644 eigdecomp.go create mode 100644 frd.go create mode 100644 frd_test.go create mode 100644 inv.go create mode 100644 loopsens.go create mode 100644 loopsens_test.go create mode 100644 modsep.go create mode 100644 modsep_test.go create mode 100644 norm_covar_test.go create mode 100644 pid.go create mode 100644 pid_test.go create mode 100644 pidtune.go create mode 100644 pidtune_test.go create mode 100644 prescale.go create mode 100644 prescale_test.go create mode 100644 pzmap.go create mode 100644 random.go create mode 100644 random_test.go create mode 100644 sminreal.go create mode 100644 sminreal_test.go create mode 100644 ssbal.go create mode 100644 ssbal_test.go create mode 100644 stabsep.go create mode 100644 stabsep_test.go create mode 100644 transform.go create mode 100644 utils_test.go diff --git a/README.md b/README.md index d601d61..71e0e2c 100644 --- a/README.md +++ b/README.md @@ -174,7 +174,6 @@ func main() { |----------|-------------| | `H2Norm` | H2 norm (RMS gain) | | `HinfNorm` | H-infinity norm (peak gain) | -| `Norm` | General norm computation | ### Lyapunov Equations @@ -222,8 +221,7 @@ func main() { |-----------------|-------------| | `Step` | Unit step response | | `Impulse` | Unit impulse response | -| `InitialCondition` | Free response to initial state | -| `Lsim` | Response to arbitrary input signal | +| `Initial` | Free response to initial state | | `Simulate` | Discrete-time simulation | | `GenSig` | Generate test signals (step, sine, square, pulse) | diff --git a/augstate.go b/augstate.go new file mode 100644 index 0000000..e27e836 --- /dev/null +++ b/augstate.go @@ -0,0 +1,73 @@ +package controlsys + +import "gonum.org/v1/gonum/mat" + +func Augstate(sys *System) (*System, error) { + n, m, p := sys.Dims() + if n == 0 { + return sys.Copy(), nil + } + + pNew := p + n + + cNew := mat.NewDense(pNew, n, nil) + cRaw := cNew.RawMatrix() + origC := sys.C.RawMatrix() + for i := 0; i < p; i++ { + copy(cRaw.Data[i*cRaw.Stride:i*cRaw.Stride+n], origC.Data[i*origC.Stride:i*origC.Stride+n]) + } + for i := 0; i < n; i++ { + cRaw.Data[(p+i)*cRaw.Stride+i] = 1 + } + + dNew := mat.NewDense(pNew, m, nil) + dRaw := dNew.RawMatrix() + origD := sys.D.RawMatrix() + for i := 0; i < p; i++ { + copy(dRaw.Data[i*dRaw.Stride:i*dRaw.Stride+m], origD.Data[i*origD.Stride:i*origD.Stride+m]) + } + + result := &System{ + A: denseCopy(sys.A), + B: denseCopy(sys.B), + C: cNew, + D: dNew, + Dt: sys.Dt, + } + + if sys.Delay != nil { + result.Delay = copyDelayOrNil(sys.Delay) + } + if sys.InputDelay != nil { + result.InputDelay = make([]float64, len(sys.InputDelay)) + copy(result.InputDelay, sys.InputDelay) + } + if sys.OutputDelay != nil { + result.OutputDelay = make([]float64, len(sys.OutputDelay)) + copy(result.OutputDelay, sys.OutputDelay) + } + if sys.LFT != nil { + result.LFT = &LFTDelay{ + Tau: append([]float64(nil), sys.LFT.Tau...), + B2: copyDelayOrNil(sys.LFT.B2), + C2: copyDelayOrNil(sys.LFT.C2), + D12: copyDelayOrNil(sys.LFT.D12), + D21: copyDelayOrNil(sys.LFT.D21), + D22: copyDelayOrNil(sys.LFT.D22), + } + } + + result.InputName = copyStringSlice(sys.InputName) + var outNames []string + if sys.OutputName != nil { + outNames = append(outNames, sys.OutputName...) + } else { + outNames = make([]string, p) + } + stNames := sys.stateLabels() + outNames = append(outNames, stNames...) + result.OutputName = outNames + result.StateName = copyStringSlice(sys.StateName) + + return result, nil +} diff --git a/canon.go b/canon.go new file mode 100644 index 0000000..7f46607 --- /dev/null +++ b/canon.go @@ -0,0 +1,307 @@ +package controlsys + +import ( + "fmt" + "math" + "sort" + + "gonum.org/v1/gonum/mat" +) + +type CanonForm string + +const ( + CanonModal CanonForm = "modal" + CanonCompanion CanonForm = "companion" +) + +type CanonResult struct { + Sys *System + T *mat.Dense +} + +func Canon(sys *System, form CanonForm) (*CanonResult, error) { + switch form { + case CanonModal: + return canonModal(sys) + case CanonCompanion: + return canonCompanion(sys) + default: + return nil, fmt.Errorf("controlsys: unknown canonical form %q", form) + } +} + +type eigBlock struct { + mag float64 + real1 float64 + imag1 float64 + idx int +} + +func canonModal(sys *System) (*CanonResult, error) { + n, m, p := sys.Dims() + if n == 0 { + cp := sys.Copy() + return &CanonResult{Sys: cp, T: &mat.Dense{}}, nil + } + + var eig mat.Eigen + ok := eig.Factorize(sys.A, mat.EigenRight) + if !ok { + return nil, fmt.Errorf("controlsys: eigendecomposition failed") + } + + vals := eig.Values(nil) + var vecs mat.CDense + eig.VectorsTo(&vecs) + + blocks := make([]eigBlock, 0, n) + used := make([]bool, n) + for i := 0; i < n; i++ { + if used[i] { + continue + } + if math.Abs(imag(vals[i])) < 1e-10 { + blocks = append(blocks, eigBlock{ + mag: math.Abs(real(vals[i])), + real1: real(vals[i]), + imag1: 0, + idx: i, + }) + used[i] = true + } else { + conj := -1 + for j := i + 1; j < n; j++ { + if !used[j] && math.Abs(real(vals[i])-real(vals[j])) < 1e-10 && + math.Abs(imag(vals[i])+imag(vals[j])) < 1e-10 { + conj = j + break + } + } + if conj < 0 { + return nil, fmt.Errorf("controlsys: complex eigenvalue without conjugate pair") + } + blocks = append(blocks, eigBlock{ + mag: math.Sqrt(real(vals[i])*real(vals[i]) + imag(vals[i])*imag(vals[i])), + real1: real(vals[i]), + imag1: math.Abs(imag(vals[i])), + idx: i, + }) + used[i] = true + used[conj] = true + } + } + + sort.Slice(blocks, func(i, j int) bool { + return blocks[i].mag < blocks[j].mag + }) + + tData := make([]float64, n*n) + col := 0 + for _, blk := range blocks { + if blk.imag1 == 0 { + for row := 0; row < n; row++ { + tData[row*n+col] = real(vecs.At(row, blk.idx)) + } + col++ + } else { + for row := 0; row < n; row++ { + v := vecs.At(row, blk.idx) + tData[row*n+col] = real(v) + tData[row*n+col+1] = imag(v) + } + col += 2 + } + } + + T := mat.NewDense(n, n, tData) + + var lu mat.LU + lu.Factorize(T) + if luNearSingular(&lu) { + return nil, fmt.Errorf("controlsys: transformation matrix is singular: %w", ErrSingularTransform) + } + + Tinv := mat.NewDense(n, n, nil) + eye := mat.NewDense(n, n, nil) + for i := 0; i < n; i++ { + eye.Set(i, i, 1) + } + if err := lu.SolveTo(Tinv, false, eye); err != nil { + return nil, fmt.Errorf("controlsys: inversion failed: %w", ErrSingularTransform) + } + + Anew := mat.NewDense(n, n, nil) + tmp := mat.NewDense(n, n, nil) + tmp.Mul(Tinv, sys.A) + Anew.Mul(tmp, T) + + Bnew := mat.NewDense(n, m, nil) + Bnew.Mul(Tinv, sys.B) + + Cnew := mat.NewDense(p, n, nil) + Cnew.Mul(sys.C, T) + + Dnew := denseCopy(sys.D) + + newSys, err := newNoCopy(Anew, Bnew, Cnew, Dnew, sys.Dt) + if err != nil { + return nil, err + } + propagateIONames(newSys, sys) + + return &CanonResult{Sys: newSys, T: T}, nil +} + +func canonCompanion(sys *System) (*CanonResult, error) { + n, m, p := sys.Dims() + if n == 0 { + cp := sys.Copy() + return &CanonResult{Sys: cp, T: &mat.Dense{}}, nil + } + if m != 1 || p != 1 { + return nil, fmt.Errorf("controlsys: companion form requires SISO system: %w", ErrNotSISO) + } + + // Observable companion form via similarity: T = Oc^{-1} * Ocomp + // where Oc = obsv(A,C) and Ocomp = obsv(Ac, Cc) with Cc = e1' + + charPoly := characteristicPoly(sys.A, n) + lead := charPoly[n] + for i := range charPoly { + charPoly[i] /= lead + } + + // Build companion A: superdiagonal 1s, last row = -coeffs + Ac := mat.NewDense(n, n, nil) + for i := 0; i < n-1; i++ { + Ac.Set(i, i+1, 1) + } + for j := 0; j < n; j++ { + Ac.Set(n-1, j, -charPoly[j]) + } + + // Cc = [1 0 ... 0] + Cc := mat.NewDense(1, n, nil) + Cc.Set(0, 0, 1) + + Oc := buildObsvMatrix(sys.A, sys.C, n, p) + Ocomp := buildObsvMatrix(Ac, Cc, n, 1) + + var luOc mat.LU + luOc.Factorize(Oc) + if luNearSingular(&luOc) { + return nil, fmt.Errorf("controlsys: system not observable, companion form undefined: %w", ErrSingularTransform) + } + + // T = Oc \ Ocomp (i.e. Oc * T = Ocomp => T = Oc^{-1} * Ocomp) + T := mat.NewDense(n, n, nil) + if err := luOc.SolveTo(T, false, Ocomp); err != nil { + return nil, fmt.Errorf("controlsys: transformation solve failed: %w", ErrSingularTransform) + } + + var luT mat.LU + luT.Factorize(T) + if luNearSingular(&luT) { + return nil, fmt.Errorf("controlsys: transformation matrix singular: %w", ErrSingularTransform) + } + + Tinv := mat.NewDense(n, n, nil) + eye := mat.NewDense(n, n, nil) + for i := 0; i < n; i++ { + eye.Set(i, i, 1) + } + if err := luT.SolveTo(Tinv, false, eye); err != nil { + return nil, fmt.Errorf("controlsys: inversion failed: %w", ErrSingularTransform) + } + + Anew := mat.NewDense(n, n, nil) + tmp := mat.NewDense(n, n, nil) + tmp.Mul(Tinv, sys.A) + Anew.Mul(tmp, T) + + Bnew := mat.NewDense(n, 1, nil) + Bnew.Mul(Tinv, sys.B) + + Cnew := mat.NewDense(1, n, nil) + Cnew.Mul(sys.C, T) + + Dnew := denseCopy(sys.D) + + newSys, err := newNoCopy(Anew, Bnew, Cnew, Dnew, sys.Dt) + if err != nil { + return nil, err + } + propagateIONames(newSys, sys) + + return &CanonResult{Sys: newSys, T: T}, nil +} + +func characteristicPoly(A *mat.Dense, n int) []float64 { + if n == 0 { + return []float64{1} + } + + var eig mat.Eigen + ok := eig.Factorize(A, mat.EigenNone) + if !ok { + coeffs := make([]float64, n+1) + coeffs[n] = 1 + return coeffs + } + vals := eig.Values(nil) + + // Build polynomial from roots incrementally: prod(s - lambda_i) + // coeffs in descending power: coeffs[0]*s^n + ... + coeffs[n] + coeffs := make([]float64, n+1) + coeffs[0] = 1.0 + cur := 1 + for _, lam := range vals { + if math.Abs(imag(lam)) < 1e-10 { + r := real(lam) + for j := cur; j >= 1; j-- { + coeffs[j] = coeffs[j-1] - r*coeffs[j] + } + coeffs[0] = -r * coeffs[0] + cur++ + } else if imag(lam) > 0 { + a := -2 * real(lam) + b := real(lam)*real(lam) + imag(lam)*imag(lam) + newCoeffs := make([]float64, n+1) + for j := 0; j <= cur; j++ { + newCoeffs[j+2] += coeffs[j] + newCoeffs[j+1] += a * coeffs[j] + newCoeffs[j] += b * coeffs[j] + } + copy(coeffs, newCoeffs) + cur += 2 + } + } + + // Result in ascending power order: result[i] = coefficient of s^i + result := make([]float64, n+1) + for i := range result { + result[i] = coeffs[n-i] + } + return result +} + +func buildObsvMatrix(A, C *mat.Dense, n, p int) *mat.Dense { + O := mat.NewDense(n*p, n, nil) + setBlock(O, 0, 0, C) + CA := mat.NewDense(p, n, nil) + CA.Mul(C, A) + prev := CA + setBlock(O, p, 0, CA) + for i := 2; i < n; i++ { + next := mat.NewDense(p, n, nil) + next.Mul(prev, A) + setBlock(O, i*p, 0, next) + prev = next + } + + if n*p == n { + return O + } + return extractSubmatrix(O, 0, n, 0, n) +} diff --git a/canon_test.go b/canon_test.go new file mode 100644 index 0000000..8f96a45 --- /dev/null +++ b/canon_test.go @@ -0,0 +1,220 @@ +package controlsys + +import ( + "math" + "math/cmplx" + "sort" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestCanonModal_RealEigenvalues(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -6, -5}), + mat.NewDense(2, 1, []float64{0, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Canon(sys, CanonModal) + if err != nil { + t.Fatal(err) + } + + Amod := res.Sys.A + poles, _ := res.Sys.Poles() + sortPolesByReal(poles) + + if math.Abs(real(poles[0])+3) > 1e-10 || math.Abs(real(poles[1])+2) > 1e-10 { + t.Errorf("poles = %v, want [-3, -2]", poles) + } + + for i := 0; i < 2; i++ { + for j := 0; j < 2; j++ { + if i != j && math.Abs(Amod.At(i, j)) > 1e-8 { + t.Errorf("A_modal[%d,%d] = %g, want 0 (should be diagonal)", i, j, Amod.At(i, j)) + } + } + } + + dc, err := res.Sys.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(dc.At(0, 0)-1.0/6.0) > 1e-8 { + t.Errorf("dcgain = %g, want 1/6", dc.At(0, 0)) + } + + checkEigsPreserved(t, sys, res.Sys, 1e-10) +} + +func TestCanonModal_ComplexEigenvalues(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -1, 0}), + mat.NewDense(2, 1, []float64{0, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Canon(sys, CanonModal) + if err != nil { + t.Fatal(err) + } + + Amod := res.Sys.A + if math.Abs(Amod.At(0, 0)) > 1e-8 || math.Abs(Amod.At(1, 1)) > 1e-8 { + t.Errorf("diagonal should be ~0, got [%g, %g]", Amod.At(0, 0), Amod.At(1, 1)) + } + if math.Abs(math.Abs(Amod.At(0, 1))-1) > 1e-8 || math.Abs(math.Abs(Amod.At(1, 0))-1) > 1e-8 { + t.Errorf("off-diagonal should be ±1, got [%g, %g]", Amod.At(0, 1), Amod.At(1, 0)) + } + if Amod.At(0, 1)*Amod.At(1, 0) > 0 { + t.Errorf("off-diagonal should have opposite signs") + } + + checkEigsPreserved(t, sys, res.Sys, 1e-10) +} + +func TestCanonModal_Mixed(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{-1, 0, 0, 0, 0, 1, 0, -4, 0}), + mat.NewDense(3, 1, []float64{1, 0, 1}), + mat.NewDense(1, 3, []float64{1, 1, 0}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Canon(sys, CanonModal) + if err != nil { + t.Fatal(err) + } + + poles, _ := res.Sys.Poles() + realCount := 0 + complexCount := 0 + for _, p := range poles { + if math.Abs(imag(p)) < 1e-8 { + realCount++ + } else { + complexCount++ + } + } + if realCount != 1 || complexCount != 2 { + t.Errorf("expected 1 real + 2 complex poles, got %d real + %d complex", realCount, complexCount) + } + + checkEigsPreserved(t, sys, res.Sys, 1e-10) + checkFreqPreserved(t, sys, res.Sys, 1e-6) +} + +func TestCanonModal_Empty(t *testing.T) { + sys, err := New(nil, nil, nil, mat.NewDense(1, 1, []float64{5}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Canon(sys, CanonModal) + if err != nil { + t.Fatal(err) + } + + dc, _ := res.Sys.DCGain() + if dc.At(0, 0) != 5 { + t.Errorf("dcgain = %g, want 5", dc.At(0, 0)) + } +} + +func TestCanonCompanion_SISO(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -6, -5}), + mat.NewDense(2, 1, []float64{0, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Canon(sys, CanonCompanion) + if err != nil { + t.Fatal(err) + } + + checkEigsPreserved(t, sys, res.Sys, 1e-10) + + dc, err := res.Sys.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(dc.At(0, 0)-1.0/6.0) > 1e-8 { + t.Errorf("dcgain = %g, want 1/6", dc.At(0, 0)) + } +} + +func TestCanonCompanion_MIMOError(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -6, -5}), + mat.NewDense(2, 2, []float64{0, 1, 1, 0}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 2, []float64{0, 0}), 0) + if err != nil { + t.Fatal(err) + } + + _, err = Canon(sys, CanonCompanion) + if err == nil { + t.Error("expected error for MIMO system") + } +} + +func TestCanonInvalidForm(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + + _, err := Canon(sys, "bogus") + if err == nil { + t.Error("expected error for unknown form") + } +} + +func sortPolesByReal(poles []complex128) { + sort.Slice(poles, func(i, j int) bool { + if real(poles[i]) != real(poles[j]) { + return real(poles[i]) < real(poles[j]) + } + return imag(poles[i]) < imag(poles[j]) + }) +} + +func TestCanonModal_FrequencyPreserved(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{-1, 2, 0, -3}), + mat.NewDense(2, 1, []float64{1, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Canon(sys, CanonModal) + if err != nil { + t.Fatal(err) + } + + for _, w := range []float64{0.01, 0.1, 1, 10, 100} { + g1, _ := sys.EvalFr(complex(0, w)) + g2, _ := res.Sys.EvalFr(complex(0, w)) + diff := cmplx.Abs(g1[0][0] - g2[0][0]) + if diff > 1e-6 { + t.Errorf("w=%g: freq response diff=%g", w, diff) + } + } +} diff --git a/covar.go b/covar.go new file mode 100644 index 0000000..e4a70aa --- /dev/null +++ b/covar.go @@ -0,0 +1,64 @@ +package controlsys + +import ( + "fmt" + + "gonum.org/v1/gonum/mat" +) + +func Covar(sys *System, W *mat.Dense) (*mat.Dense, error) { + n, m, p := sys.Dims() + + wr, wc := W.Dims() + if wr != m || wc != m { + return nil, fmt.Errorf("W must be %d×%d, got %d×%d: %w", m, m, wr, wc, ErrDimensionMismatch) + } + + stable, err := sys.IsStable() + if err != nil { + return nil, err + } + if !stable { + return nil, ErrUnstable + } + + // Q = B*W*B' + var BW mat.Dense + BW.Mul(sys.B, W) + Q := mat.NewDense(n, n, nil) + Q.Mul(&BW, sys.B.T()) + + // symmetrize Q for Lyapunov solver + qRaw := Q.RawMatrix() + for i := 0; i < n; i++ { + for j := i + 1; j < n; j++ { + avg := (qRaw.Data[i*qRaw.Stride+j] + qRaw.Data[j*qRaw.Stride+i]) / 2 + qRaw.Data[i*qRaw.Stride+j] = avg + qRaw.Data[j*qRaw.Stride+i] = avg + } + } + + var X *mat.Dense + if sys.IsContinuous() { + X, err = Lyap(sys.A, Q, nil) + } else { + X, err = DLyap(sys.A, Q, nil) + } + if err != nil { + return nil, err + } + + // P = C*X*C' + D*W*D' + var CX mat.Dense + CX.Mul(sys.C, X) + P := mat.NewDense(p, p, nil) + P.Mul(&CX, sys.C.T()) + + var DW mat.Dense + DW.Mul(sys.D, W) + var DWDt mat.Dense + DWDt.Mul(&DW, sys.D.T()) + P.Add(P, &DWDt) + + return P, nil +} diff --git a/eigdecomp.go b/eigdecomp.go new file mode 100644 index 0000000..223dc49 --- /dev/null +++ b/eigdecomp.go @@ -0,0 +1,426 @@ +package controlsys + +import ( + "fmt" + "math" + "math/cmplx" + "sort" + + "gonum.org/v1/gonum/blas" + "gonum.org/v1/gonum/blas/blas64" + "gonum.org/v1/gonum/lapack" + "gonum.org/v1/gonum/mat" +) + +func decomposeByEigenvalues(sys *System, isGroup1 func(complex128) bool) (group1, group2 *System, err error) { + n, m, p := sys.Dims() + if n == 0 { + cp := sys.Copy() + gain, _ := NewGain(mat.NewDense(p, m, nil), sys.Dt) + propagateIONames(gain, sys) + return cp, gain, nil + } + + var eig mat.Eigen + ok := eig.Factorize(sys.A, mat.EigenRight) + if !ok { + return decomposeBySchur(sys, isGroup1) + } + + vals := eig.Values(nil) + + var idx1, idx2 []int + assigned := make([]bool, n) + for i := range n { + if assigned[i] { + continue + } + if imag(vals[i]) != 0 { + j := -1 + for k := i + 1; k < n; k++ { + if !assigned[k] && isConjugate(vals[i], vals[k]) { + j = k + break + } + } + if j >= 0 { + if isGroup1(vals[i]) { + idx1 = append(idx1, i, j) + } else { + idx2 = append(idx2, i, j) + } + assigned[i] = true + assigned[j] = true + continue + } + } + if isGroup1(vals[i]) { + idx1 = append(idx1, i) + } else { + idx2 = append(idx2, i) + } + assigned[i] = true + } + + sort.Ints(idx1) + sort.Ints(idx2) + + n1 := len(idx1) + n2 := len(idx2) + + if n1 == 0 { + gain, _ := NewGain(mat.NewDense(p, m, nil), sys.Dt) + propagateIONames(gain, sys) + return gain, sys.Copy(), nil + } + if n2 == 0 { + gain, _ := NewGain(denseCopy(sys.D), sys.Dt) + propagateIONames(gain, sys) + cp := sys.Copy() + cp.D = mat.NewDense(p, m, nil) + return cp, gain, nil + } + + var vecC mat.CDense + eig.VectorsTo(&vecC) + + reordered := make([]int, 0, n) + reordered = append(reordered, idx1...) + reordered = append(reordered, idx2...) + + V := mat.NewCDense(n, n, nil) + for j, col := range reordered { + for i := range n { + V.Set(i, j, vecC.At(i, col)) + } + } + + Vinv, err := cinvert(V, n) + if err != nil { + return decomposeBySchur(sys, isGroup1) + } + + condNum := cmatNorm(V, n) * cmatNorm(Vinv, n) + if condNum > 1e12 { + return decomposeBySchur(sys, isGroup1) + } + + At := cmatMul3(Vinv, sys.A, V, n, n, n) + Bt := cmatMulDense(Vinv, sys.B, n, m) + Ct := cdenseMulCmat(sys.C, V, p, n) + + A1r := realPart(At, 0, n1, 0, n1) + B1r := realPart(Bt, 0, n1, 0, m) + C1r := realPart(Ct, 0, p, 0, n1) + + A2r := realPart(At, n1, n, n1, n) + B2r := realPart(Bt, n1, n, 0, m) + C2r := realPart(Ct, 0, p, n1, n) + + D1 := mat.NewDense(p, m, nil) + D2 := denseCopy(sys.D) + + sys1, err := newNoCopy(A1r, B1r, C1r, D1, sys.Dt) + if err != nil { + return nil, nil, err + } + propagateIONames(sys1, sys) + + sys2, err := newNoCopy(A2r, B2r, C2r, D2, sys.Dt) + if err != nil { + return nil, nil, err + } + propagateIONames(sys2, sys) + + return sys1, sys2, nil +} + +func decomposeBySchur(sys *System, isGroup1 func(complex128) bool) (group1, group2 *System, err error) { + n, m, p := sys.Dims() + + t := make([]float64, n*n) + aRaw := sys.A.RawMatrix() + copyStrided(t, n, aRaw.Data, aRaw.Stride, n, n) + + z := make([]float64, n*n) + wr := make([]float64, n) + wi := make([]float64, n) + bwork := make([]bool, n) + + workQuery := make([]float64, 1) + impl.Dgees(lapack.SchurHess, lapack.SortNone, nil, + n, t, n, wr, wi, z, n, workQuery, -1, bwork) + lwork := int(workQuery[0]) + work := make([]float64, lwork) + + _, ok := impl.Dgees(lapack.SchurHess, lapack.SortNone, nil, + n, t, n, wr, wi, z, n, work, lwork, bwork) + if !ok { + return nil, nil, ErrSchurFailed + } + + evals := schurEigenvaluesRaw(t, n) + + n1 := 0 + for i := range n { + if isGroup1(evals[i]) { + n1++ + } + } + + if n1 == 0 { + gain, _ := NewGain(mat.NewDense(p, m, nil), sys.Dt) + propagateIONames(gain, sys) + return gain, sys.Copy(), nil + } + if n1 == n { + gain, _ := NewGain(denseCopy(sys.D), sys.Dt) + propagateIONames(gain, sys) + cp := sys.Copy() + cp.D = mat.NewDense(p, m, nil) + return cp, gain, nil + } + + trexcWork := make([]float64, n) + nDone := 0 + i := 0 + for i < n { + blockSize := 1 + if i+1 < n && t[(i+1)*n+i] != 0 { + blockSize = 2 + } + + ev := evals[i] + if isGroup1(ev) { + if i != nDone { + _, _, swapOk := impl.Dtrexc(lapack.UpdateSchur, n, t, n, z, n, i, nDone, trexcWork) + if !swapOk { + return nil, nil, ErrSchurFailed + } + evals = schurEigenvaluesRaw(t, n) + } + if nDone+1 < n && t[(nDone+1)*n+nDone] != 0 { + nDone += 2 + } else { + nDone++ + } + i = nDone + } else { + i += blockSize + } + } + + n1 = nDone + + bRaw := sys.B.RawMatrix() + cRaw := sys.C.RawMatrix() + zGen := blas64.General{Rows: n, Cols: n, Stride: n, Data: z} + + bt := make([]float64, n*m) + blas64.Gemm(blas.Trans, blas.NoTrans, 1, zGen, + blas64.General{Rows: n, Cols: m, Stride: bRaw.Stride, Data: bRaw.Data}, + 0, blas64.General{Rows: n, Cols: m, Stride: m, Data: bt}) + + ct := make([]float64, p*n) + blas64.Gemm(blas.NoTrans, blas.NoTrans, 1, + blas64.General{Rows: p, Cols: n, Stride: cRaw.Stride, Data: cRaw.Data}, + zGen, + 0, blas64.General{Rows: p, Cols: n, Stride: n, Data: ct}) + + A1 := mat.NewDense(n1, n1, nil) + for i := range n1 { + for j := range n1 { + A1.Set(i, j, t[i*n+j]) + } + } + B1 := mat.NewDense(n1, m, nil) + for i := range n1 { + copy(B1.RawMatrix().Data[i*m:i*m+m], bt[i*m:i*m+m]) + } + C1 := mat.NewDense(p, n1, nil) + for i := range p { + for j := range n1 { + C1.Set(i, j, ct[i*n+j]) + } + } + + n2 := n - n1 + A2 := mat.NewDense(n2, n2, nil) + for i := range n2 { + for j := range n2 { + A2.Set(i, j, t[(n1+i)*n+(n1+j)]) + } + } + B2 := mat.NewDense(n2, m, nil) + for i := range n2 { + copy(B2.RawMatrix().Data[i*m:i*m+m], bt[(n1+i)*m:(n1+i)*m+m]) + } + C2 := mat.NewDense(p, n2, nil) + for i := range p { + for j := range n2 { + C2.Set(i, j, ct[i*n+(n1+j)]) + } + } + + D1 := mat.NewDense(p, m, nil) + D2 := denseCopy(sys.D) + + sys1, err := newNoCopy(A1, B1, C1, D1, sys.Dt) + if err != nil { + return nil, nil, err + } + propagateIONames(sys1, sys) + + sys2, err := newNoCopy(A2, B2, C2, D2, sys.Dt) + if err != nil { + return nil, nil, err + } + propagateIONames(sys2, sys) + + return sys1, sys2, nil +} + +func schurEigenvaluesRaw(t []float64, n int) []complex128 { + evals := make([]complex128, n) + i := 0 + for i < n { + if i+1 < n && t[(i+1)*n+i] != 0 { + a := t[i*n+i] + b := t[i*n+i+1] + c := t[(i+1)*n+i] + d := t[(i+1)*n+i+1] + tr := a + d + det := a*d - b*c + disc := tr*tr - 4*det + if disc < 0 { + re := tr / 2 + im := math.Sqrt(-disc) / 2 + evals[i] = complex(re, im) + evals[i+1] = complex(re, -im) + } else { + sq := math.Sqrt(disc) + evals[i] = complex((tr+sq)/2, 0) + evals[i+1] = complex((tr-sq)/2, 0) + } + i += 2 + } else { + evals[i] = complex(t[i*n+i], 0) + i++ + } + } + return evals +} + +func isConjugate(a, b complex128) bool { + return math.Abs(real(a)-real(b)) < 1e-10*math.Max(1, math.Abs(real(a))) && + math.Abs(imag(a)+imag(b)) < 1e-10*math.Max(1, math.Abs(imag(a))) +} + +func cinvert(A *mat.CDense, n int) (*mat.CDense, error) { + augR := mat.NewDense(2*n, 2*n, nil) + for i := range n { + for j := range n { + v := A.At(i, j) + augR.Set(i, j, real(v)) + augR.Set(i, j+n, -imag(v)) + augR.Set(i+n, j, imag(v)) + augR.Set(i+n, j+n, real(v)) + } + } + + augI := mat.NewDense(2*n, 2*n, nil) + for i := range 2 * n { + augI.Set(i, i, 1) + } + + var lu mat.LU + lu.Factorize(augR) + var sol mat.Dense + if err := lu.SolveTo(&sol, false, augI); err != nil { + return nil, fmt.Errorf("controlsys: eigenvector matrix singular: %w", err) + } + + result := mat.NewCDense(n, n, nil) + for i := range n { + for j := range n { + re := sol.At(i, j) + im := sol.At(i+n, j) + result.Set(i, j, complex(re, im)) + } + } + return result, nil +} + +func cmatNorm(A *mat.CDense, n int) float64 { + sum := 0.0 + for i := range n { + for j := range n { + sum += cmplx.Abs(A.At(i, j)) * cmplx.Abs(A.At(i, j)) + } + } + return math.Sqrt(sum) +} + +func cmatMul3(Vinv *mat.CDense, Areal *mat.Dense, V *mat.CDense, n1, n2, n3 int) *mat.CDense { + tmp := mat.NewCDense(n1, n2, nil) + for i := range n1 { + for j := range n2 { + var s complex128 + for k := range n2 { + s += Vinv.At(i, k) * complex(Areal.At(k, j), 0) + } + tmp.Set(i, j, s) + } + } + result := mat.NewCDense(n1, n3, nil) + for i := range n1 { + for j := range n3 { + var s complex128 + for k := range n2 { + s += tmp.At(i, k) * V.At(k, j) + } + result.Set(i, j, s) + } + } + return result +} + +func cmatMulDense(Vinv *mat.CDense, B *mat.Dense, n, m int) *mat.CDense { + result := mat.NewCDense(n, m, nil) + for i := range n { + for j := range m { + var s complex128 + for k := range n { + s += Vinv.At(i, k) * complex(B.At(k, j), 0) + } + result.Set(i, j, s) + } + } + return result +} + +func cdenseMulCmat(C *mat.Dense, V *mat.CDense, p, n int) *mat.CDense { + result := mat.NewCDense(p, n, nil) + for i := range p { + for j := range n { + var s complex128 + for k := range n { + s += complex(C.At(i, k), 0) * V.At(k, j) + } + result.Set(i, j, s) + } + } + return result +} + +func realPart(C *mat.CDense, r0, r1, c0, c1 int) *mat.Dense { + rows := r1 - r0 + cols := c1 - c0 + result := mat.NewDense(rows, cols, nil) + for i := range rows { + for j := range cols { + result.Set(i, j, real(C.At(r0+i, c0+j))) + } + } + return result +} diff --git a/frd.go b/frd.go new file mode 100644 index 0000000..4f55f85 --- /dev/null +++ b/frd.go @@ -0,0 +1,529 @@ +package controlsys + +import ( + "fmt" + "math" + "math/cmplx" + "sort" + + "gonum.org/v1/gonum/mat" +) + +// FRD represents a Frequency Response Data model — measured or computed +// complex response at discrete frequencies. +type FRD struct { + Response [][][]complex128 + Omega []float64 + Dt float64 + + InputName []string + OutputName []string +} + +// NewFRD creates an FRD model from response data and frequency vector. +// response[k] is the p*m complex response matrix at frequency omega[k]. +func NewFRD(response [][][]complex128, omega []float64, dt float64) (*FRD, error) { + if dt < 0 { + return nil, fmt.Errorf("FRD: negative sample time: %w", ErrInvalidSampleTime) + } + if len(response) != len(omega) { + return nil, fmt.Errorf("FRD: len(response)=%d != len(omega)=%d: %w", + len(response), len(omega), ErrDimensionMismatch) + } + if len(omega) == 0 { + return &FRD{Dt: dt}, nil + } + + for i := range omega { + if omega[i] < 0 { + return nil, fmt.Errorf("FRD: omega[%d]=%v is negative: %w", i, omega[i], ErrDimensionMismatch) + } + } + if !sort.Float64sAreSorted(omega) { + return nil, fmt.Errorf("FRD: omega must be sorted ascending: %w", ErrDimensionMismatch) + } + + if dt > 0 { + nyquist := math.Pi / dt + for i, w := range omega { + if w > nyquist*(1+1e-10) { + return nil, fmt.Errorf("FRD: omega[%d]=%v exceeds Nyquist frequency %v: %w", + i, w, nyquist, ErrDimensionMismatch) + } + } + } + + p := len(response[0]) + if p == 0 { + return nil, fmt.Errorf("FRD: response matrix has zero rows: %w", ErrDimensionMismatch) + } + m := len(response[0][0]) + if m == 0 { + return nil, fmt.Errorf("FRD: response matrix has zero columns: %w", ErrDimensionMismatch) + } + + for k := range response { + if len(response[k]) != p { + return nil, fmt.Errorf("FRD: response[%d] has %d rows, want %d: %w", + k, len(response[k]), p, ErrDimensionMismatch) + } + for i := range response[k] { + if len(response[k][i]) != m { + return nil, fmt.Errorf("FRD: response[%d][%d] has %d cols, want %d: %w", + k, i, len(response[k][i]), m, ErrDimensionMismatch) + } + } + } + + resp := make([][][]complex128, len(response)) + for k := range response { + resp[k] = make([][]complex128, p) + for i := range response[k] { + resp[k][i] = make([]complex128, m) + copy(resp[k][i], response[k][i]) + } + } + om := make([]float64, len(omega)) + copy(om, omega) + + return &FRD{ + Response: resp, + Omega: om, + Dt: dt, + }, nil +} + +// FRD computes the frequency response data model at the given frequencies. +func (sys *System) FRD(omega []float64) (*FRD, error) { + if len(omega) == 0 { + return &FRD{Dt: sys.Dt}, nil + } + + resp, err := sys.FreqResponse(omega) + if err != nil { + return nil, err + } + + _, m, p := sys.Dims() + nw := len(omega) + + response := make([][][]complex128, nw) + for k := 0; k < nw; k++ { + response[k] = make([][]complex128, p) + for i := 0; i < p; i++ { + response[k][i] = make([]complex128, m) + for j := 0; j < m; j++ { + response[k][i][j] = resp.At(k, i, j) + } + } + } + + om := make([]float64, nw) + copy(om, omega) + + return &FRD{ + Response: response, + Omega: om, + Dt: sys.Dt, + InputName: copyStringSlice(sys.InputName), + OutputName: copyStringSlice(sys.OutputName), + }, nil +} + +func (f *FRD) Dims() (p, m int) { + if len(f.Response) == 0 { + return 0, 0 + } + return len(f.Response[0]), len(f.Response[0][0]) +} + +func (f *FRD) NumFrequencies() int { + return len(f.Omega) +} + +func (f *FRD) IsContinuous() bool { + return f.Dt == 0 +} + +func (f *FRD) IsDiscrete() bool { + return f.Dt > 0 +} + +func (f *FRD) At(freqIdx, i, j int) complex128 { + return f.Response[freqIdx][i][j] +} + +// EvalFr returns the p*m complex response at frequency omega[freqIdx]. +func (f *FRD) EvalFr(freqIdx int) [][]complex128 { + p, m := f.Dims() + result := make([][]complex128, p) + for i := 0; i < p; i++ { + result[i] = make([]complex128, m) + copy(result[i], f.Response[freqIdx][i]) + } + return result +} + +// FreqResponse returns the FRD data as a FreqResponseMatrix for compatibility. +func (f *FRD) FreqResponse() *FreqResponseMatrix { + p, m := f.Dims() + nw := len(f.Omega) + pm := p * m + data := make([]complex128, nw*pm) + for k := 0; k < nw; k++ { + for i := 0; i < p; i++ { + for j := 0; j < m; j++ { + data[k*pm+i*m+j] = f.Response[k][i][j] + } + } + } + return &FreqResponseMatrix{ + Data: data, + NFreq: nw, + P: p, + M: m, + InputName: copyStringSlice(f.InputName), + OutputName: copyStringSlice(f.OutputName), + } +} + +// Bode computes magnitude (dB) and phase (degrees) from the FRD data. +func (f *FRD) Bode() *BodeResult { + p, m := f.Dims() + nw := len(f.Omega) + pm := p * m + magDB := make([]float64, nw*pm) + phase := make([]float64, nw*pm) + + for k := 0; k < nw; k++ { + for i := 0; i < p; i++ { + for j := 0; j < m; j++ { + off := k*pm + i*m + j + h := f.Response[k][i][j] + magDB[off] = 20 * math.Log10(cmplx.Abs(h)) + phase[off] = cmplx.Phase(h) * 180 / math.Pi + } + } + } + + for i := 0; i < p; i++ { + for j := 0; j < m; j++ { + for k := 1; k < nw; k++ { + cur := k*pm + i*m + j + prev := (k-1)*pm + i*m + j + diff := phase[cur] - phase[prev] + if diff > 180 { + phase[cur] -= 360 + } + if diff < -180 { + phase[cur] += 360 + } + } + } + } + + omega := make([]float64, nw) + copy(omega, f.Omega) + + return &BodeResult{ + Omega: omega, + magDB: magDB, + phase: phase, + p: p, + m: m, + InputName: copyStringSlice(f.InputName), + OutputName: copyStringSlice(f.OutputName), + } +} + +func (f *FRD) Nyquist() (*NyquistResult, error) { + p, m := f.Dims() + if p != 1 || m != 1 { + return nil, fmt.Errorf("FRD Nyquist: SISO only, got %dx%d", p, m) + } + nw := len(f.Omega) + contour := make([]complex128, nw) + contourN := make([]complex128, nw) + for k := 0; k < nw; k++ { + contour[k] = f.Response[k][0][0] + contourN[k] = cmplx.Conj(contour[k]) + } + enc := 0 + for k := 1; k < nw; k++ { + re0, im0 := real(contour[k-1])+1, imag(contour[k-1]) + re1, im1 := real(contour[k])+1, imag(contour[k]) + if (im0 >= 0) != (im1 >= 0) { + xCross := re0 - im0*(re1-re0)/(im1-im0) + if xCross < 0 { + if im1 > im0 { + enc++ + } else { + enc-- + } + } + } + } + omega := make([]float64, nw) + copy(omega, f.Omega) + return &NyquistResult{Omega: omega, Contour: contour, ContourN: contourN, Encirclements: enc}, nil +} + +func (f *FRD) Sigma() (*SigmaResult, error) { + p, m := f.Dims() + nw := len(f.Omega) + nsv := p + if m < p { + nsv = m + } + sv := make([]float64, nw*nsv) + for k := 0; k < nw; k++ { + H := f.Response[k] + svd := new(mat.SVD) + hMat := mat.NewDense(p, m, nil) + for i := 0; i < p; i++ { + for j := 0; j < m; j++ { + hMat.Set(i, j, cmplx.Abs(H[i][j])) + } + } + if p == 1 && m == 1 { + sv[k] = cmplx.Abs(H[0][0]) + continue + } + hReal := mat.NewDense(2*p, 2*m, nil) + for i := 0; i < p; i++ { + for j := 0; j < m; j++ { + hReal.Set(i, j, real(H[i][j])) + hReal.Set(i, m+j, -imag(H[i][j])) + hReal.Set(p+i, j, imag(H[i][j])) + hReal.Set(p+i, m+j, real(H[i][j])) + } + } + if !svd.Factorize(hReal, mat.SVDNone) { + return nil, fmt.Errorf("FRD Sigma: SVD failed at freq index %d", k) + } + vals := svd.Values(nil) + for s := 0; s < nsv; s++ { + sv[k*nsv+s] = vals[s] + } + } + omega := make([]float64, nw) + copy(omega, f.Omega) + return &SigmaResult{Omega: omega, sv: sv, nSV: nsv}, nil +} + +func FRDMargin(f *FRD) (*MarginResult, error) { + p, m := f.Dims() + if p != 1 || m != 1 { + return nil, fmt.Errorf("FRDMargin: SISO only, got %dx%d", p, m) + } + nw := len(f.Omega) + if nw < 2 { + return nil, fmt.Errorf("FRDMargin: need at least 2 frequency points") + } + + var gmDB, pm, wcg, wcp float64 + gmDB = math.Inf(1) + foundGM, foundPM := false, false + + for k := 1; k < nw; k++ { + h0 := f.Response[k-1][0][0] + h1 := f.Response[k][0][0] + ph0 := cmplx.Phase(h0) * 180 / math.Pi + ph1 := cmplx.Phase(h1) * 180 / math.Pi + mag0 := cmplx.Abs(h0) + mag1 := cmplx.Abs(h1) + magDB0 := 20 * math.Log10(mag0) + magDB1 := 20 * math.Log10(mag1) + + if (ph0+180)*(ph1+180) <= 0 || (math.Abs(ph0+180) < 1 && math.Abs(ph1+180) < 1) { + t := math.Abs(ph0+180) / (math.Abs(ph0+180) + math.Abs(ph1+180) + 1e-30) + magAtCross := magDB0 + t*(magDB1-magDB0) + gm := -magAtCross + if !foundGM || gm < gmDB { + gmDB = gm + wcg = f.Omega[k-1] + t*(f.Omega[k]-f.Omega[k-1]) + foundGM = true + } + } + + if (magDB0)*(magDB1) <= 0 || (math.Abs(magDB0) < 0.5 && math.Abs(magDB1) < 0.5) { + t := math.Abs(magDB0) / (math.Abs(magDB0) + math.Abs(magDB1) + 1e-30) + phAtCross := ph0 + t*(ph1-ph0) + pmCand := 180 + phAtCross + if !foundPM || f.Omega[k-1]+t*(f.Omega[k]-f.Omega[k-1]) < wcp { + pm = pmCand + wcp = f.Omega[k-1] + t*(f.Omega[k]-f.Omega[k-1]) + foundPM = true + } + } + } + + if !foundGM { + gmDB = math.Inf(1) + } + if !foundPM { + pm = math.Inf(1) + } + + return &MarginResult{ + GainMargin: gmDB, + PhaseMargin: pm, + WgFreq: wcp, + WpFreq: wcg, + }, nil +} + +func frdGridsMatch(f1, f2 *FRD) error { + if len(f1.Omega) != len(f2.Omega) { + return fmt.Errorf("frd: frequency grid lengths %d != %d", len(f1.Omega), len(f2.Omega)) + } + for i := range f1.Omega { + if math.Abs(f1.Omega[i]-f2.Omega[i]) > 1e-12*math.Max(1, math.Abs(f1.Omega[i])) { + return fmt.Errorf("frd: frequency mismatch at index %d: %g != %g", i, f1.Omega[i], f2.Omega[i]) + } + } + return nil +} + +func FRDSeries(f1, f2 *FRD) (*FRD, error) { + if err := frdGridsMatch(f1, f2); err != nil { + return nil, err + } + p1, m1 := f1.Dims() + p2, m2 := f2.Dims() + if p1 != m2 { + return nil, fmt.Errorf("frd series: f1 outputs %d != f2 inputs %d", p1, m2) + } + + nw := len(f1.Omega) + resp := make([][][]complex128, nw) + for w := 0; w < nw; w++ { + resp[w] = cMatMul(f2.Response[w], f1.Response[w], p2, p1, m1) + } + return &FRD{Response: resp, Omega: append([]float64(nil), f1.Omega...), Dt: f1.Dt}, nil +} + +func FRDParallel(f1, f2 *FRD) (*FRD, error) { + if err := frdGridsMatch(f1, f2); err != nil { + return nil, err + } + p1, m1 := f1.Dims() + p2, m2 := f2.Dims() + if p1 != p2 || m1 != m2 { + return nil, fmt.Errorf("frd parallel: dims (%d,%d) != (%d,%d)", p1, m1, p2, m2) + } + + nw := len(f1.Omega) + resp := make([][][]complex128, nw) + for w := 0; w < nw; w++ { + r := make([][]complex128, p1) + for i := 0; i < p1; i++ { + r[i] = make([]complex128, m1) + for j := 0; j < m1; j++ { + r[i][j] = f1.Response[w][i][j] + f2.Response[w][i][j] + } + } + resp[w] = r + } + return &FRD{Response: resp, Omega: append([]float64(nil), f1.Omega...), Dt: f1.Dt}, nil +} + +func FRDFeedback(plant, controller *FRD, sign float64) (*FRD, error) { + if err := frdGridsMatch(plant, controller); err != nil { + return nil, err + } + pp, pm := plant.Dims() + cp, cm := controller.Dims() + if pp != cm { + return nil, fmt.Errorf("frd feedback: plant outputs %d != controller inputs %d", pp, cm) + } + if pm != cp { + return nil, fmt.Errorf("frd feedback: plant inputs %d != controller outputs %d", pm, cp) + } + + nw := len(plant.Omega) + resp := make([][][]complex128, nw) + + for w := 0; w < nw; w++ { + G := plant.Response[w] + K := controller.Response[w] + KG := cMatMul(K, G, cp, pp, pm) + + n := pm + IpKG := make([][]complex128, n) + for i := 0; i < n; i++ { + IpKG[i] = make([]complex128, n) + for j := 0; j < n; j++ { + IpKG[i][j] = complex(-sign, 0) * KG[i][j] + if i == j { + IpKG[i][j] += 1 + } + } + } + + inv, err := cMatInv(IpKG, n) + if err != nil { + return nil, fmt.Errorf("frd feedback: singular at freq index %d", w) + } + resp[w] = cMatMul(G, inv, pp, n, pm) + } + + return &FRD{Response: resp, Omega: append([]float64(nil), plant.Omega...), Dt: plant.Dt}, nil +} + +func cMatMul(a, b [][]complex128, ra, ca, cb int) [][]complex128 { + r := make([][]complex128, ra) + for i := 0; i < ra; i++ { + r[i] = make([]complex128, cb) + for j := 0; j < cb; j++ { + var s complex128 + for k := 0; k < ca; k++ { + s += a[i][k] * b[k][j] + } + r[i][j] = s + } + } + return r +} + +func cMatInv(a [][]complex128, n int) ([][]complex128, error) { + aug := make([][]complex128, n) + for i := 0; i < n; i++ { + aug[i] = make([]complex128, 2*n) + copy(aug[i][:n], a[i]) + aug[i][n+i] = 1 + } + for col := 0; col < n; col++ { + pivot := -1 + var best float64 + for row := col; row < n; row++ { + if v := cmplx.Abs(aug[row][col]); v > best { + best = v + pivot = row + } + } + if best < 1e-15 { + return nil, fmt.Errorf("singular") + } + aug[col], aug[pivot] = aug[pivot], aug[col] + s := 1.0 / aug[col][col] + for j := col; j < 2*n; j++ { + aug[col][j] *= s + } + for row := 0; row < n; row++ { + if row == col { + continue + } + f := aug[row][col] + for j := col; j < 2*n; j++ { + aug[row][j] -= f * aug[col][j] + } + } + } + inv := make([][]complex128, n) + for i := 0; i < n; i++ { + inv[i] = make([]complex128, n) + copy(inv[i], aug[i][n:]) + } + return inv, nil +} diff --git a/frd_test.go b/frd_test.go new file mode 100644 index 0000000..d7b1ec5 --- /dev/null +++ b/frd_test.go @@ -0,0 +1,498 @@ +package controlsys + +import ( + "errors" + "math" + "math/cmplx" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestFRD_SISOFromSystem(t *testing.T) { + sys, err := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + omega := []float64{0.1, 1, 10} + frd, err := sys.FRD(omega) + if err != nil { + t.Fatal(err) + } + + p, m := frd.Dims() + if p != 1 || m != 1 { + t.Fatalf("Dims() = (%d,%d), want (1,1)", p, m) + } + if frd.NumFrequencies() != 3 { + t.Fatalf("NumFrequencies() = %d, want 3", frd.NumFrequencies()) + } + if !frd.IsContinuous() { + t.Fatal("expected continuous") + } + + wantH := []complex128{ + complex(1, 0) / complex(1, 0.1), + complex(1, 0) / complex(1, 1), + complex(1, 0) / complex(1, 10), + } + wantMag := []float64{ + 1.0 / math.Sqrt(1.01), + 1.0 / math.Sqrt(2), + 1.0 / math.Sqrt(101), + } + + for k := range omega { + h := frd.At(k, 0, 0) + if cmplx.Abs(h-wantH[k]) > 1e-10 { + t.Errorf("w=%v: H=%v, want %v", omega[k], h, wantH[k]) + } + mag := cmplx.Abs(h) + if math.Abs(mag-wantMag[k]) > 1e-10 { + t.Errorf("w=%v: |H|=%v, want %v", omega[k], mag, wantMag[k]) + } + } +} + +func TestFRD_MIMOFromSystem(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -2, -3}), + mat.NewDense(2, 2, []float64{1, 0, 0, 1}), + mat.NewDense(2, 2, []float64{1, 0, 0, 1}), + mat.NewDense(2, 2, []float64{0, 0, 0, 0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + omega := []float64{1.0} + frd, err := sys.FRD(omega) + if err != nil { + t.Fatal(err) + } + + p, m := frd.Dims() + if p != 2 || m != 2 { + t.Fatalf("Dims() = (%d,%d), want (2,2)", p, m) + } + + // H(j) = (jI - A)^{-1} since C=I, B=I, D=0 + // jI - A = [j, -1; 2, j+3] + // det = j(j+3) + 2 = -1+3j+2 = 1+3j + det := complex(1, 3) + want := [2][2]complex128{ + {complex(0, 1) + 3, 1}, + {-2, complex(0, 1)}, + } + for i := 0; i < 2; i++ { + for j := 0; j < 2; j++ { + want[i][j] /= det + } + } + + for i := 0; i < 2; i++ { + for j := 0; j < 2; j++ { + got := frd.At(0, i, j) + if cmplx.Abs(got-want[i][j]) > 1e-10 { + t.Errorf("H[%d][%d] = %v, want %v", i, j, got, want[i][j]) + } + } + } +} + +func TestFRD_DirectConstruction(t *testing.T) { + resp := [][][]complex128{ + {{0.5 - 0.5i}}, + {{0.01 - 0.1i}}, + } + omega := []float64{1, 10} + frd, err := NewFRD(resp, omega, 0) + if err != nil { + t.Fatal(err) + } + + p, m := frd.Dims() + if p != 1 || m != 1 { + t.Fatalf("Dims() = (%d,%d), want (1,1)", p, m) + } + if frd.NumFrequencies() != 2 { + t.Fatalf("NumFrequencies() = %d, want 2", frd.NumFrequencies()) + } + + if frd.At(0, 0, 0) != 0.5-0.5i { + t.Errorf("At(0,0,0) = %v, want 0.5-0.5i", frd.At(0, 0, 0)) + } + if frd.At(1, 0, 0) != 0.01-0.1i { + t.Errorf("At(1,0,0) = %v, want 0.01-0.1i", frd.At(1, 0, 0)) + } +} + +func TestFRD_Discrete(t *testing.T) { + sys, err := New( + mat.NewDense(1, 1, []float64{0.5}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), + 0.1, + ) + if err != nil { + t.Fatal(err) + } + + omega := []float64{0.1, 1, 5} + frd, err := sys.FRD(omega) + if err != nil { + t.Fatal(err) + } + + if !frd.IsDiscrete() { + t.Fatal("expected discrete") + } + if frd.IsContinuous() { + t.Fatal("expected not continuous") + } + + for k, w := range omega { + z := cmplx.Exp(complex(0, w*0.1)) + want := 1 / (z - 0.5) + got := frd.At(k, 0, 0) + if cmplx.Abs(got-want) > 1e-10 { + t.Errorf("w=%v: H=%v, want %v", w, got, want) + } + } +} + +func TestFRD_ValidationErrors(t *testing.T) { + t.Run("unsorted omega", func(t *testing.T) { + resp := [][][]complex128{{{1 + 0i}}, {{2 + 0i}}} + _, err := NewFRD(resp, []float64{10, 1}, 0) + if err == nil { + t.Fatal("expected error for unsorted omega") + } + }) + + t.Run("negative omega", func(t *testing.T) { + resp := [][][]complex128{{{1 + 0i}}, {{2 + 0i}}} + _, err := NewFRD(resp, []float64{-1, 1}, 0) + if err == nil { + t.Fatal("expected error for negative omega") + } + }) + + t.Run("mismatched response dimensions", func(t *testing.T) { + resp := [][][]complex128{ + {{1 + 0i}}, + {{2 + 0i, 3 + 0i}}, + } + _, err := NewFRD(resp, []float64{1, 2}, 0) + if !errors.Is(err, ErrDimensionMismatch) { + t.Fatalf("expected ErrDimensionMismatch, got %v", err) + } + }) + + t.Run("response/omega length mismatch", func(t *testing.T) { + resp := [][][]complex128{{{1 + 0i}}} + _, err := NewFRD(resp, []float64{1, 2}, 0) + if !errors.Is(err, ErrDimensionMismatch) { + t.Fatalf("expected ErrDimensionMismatch, got %v", err) + } + }) + + t.Run("negative sample time", func(t *testing.T) { + resp := [][][]complex128{{{1 + 0i}}} + _, err := NewFRD(resp, []float64{1}, -0.1) + if !errors.Is(err, ErrInvalidSampleTime) { + t.Fatalf("expected ErrInvalidSampleTime, got %v", err) + } + }) + + t.Run("exceeds Nyquist", func(t *testing.T) { + resp := [][][]complex128{{{1 + 0i}}} + dt := 0.1 + nyq := math.Pi / dt + _, err := NewFRD(resp, []float64{nyq + 1}, dt) + if !errors.Is(err, ErrDimensionMismatch) { + t.Fatalf("expected ErrDimensionMismatch, got %v", err) + } + }) +} + +func TestFRD_CrossValidateWithFreqResponse(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -2, -3}), + mat.NewDense(2, 1, []float64{0, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + omega := []float64{0.01, 0.1, 1, 10, 100} + resp, err := sys.FreqResponse(omega) + if err != nil { + t.Fatal(err) + } + frd, err := sys.FRD(omega) + if err != nil { + t.Fatal(err) + } + + _, m, p := sys.Dims() + for k := range omega { + for i := 0; i < p; i++ { + for j := 0; j < m; j++ { + got := frd.At(k, i, j) + want := resp.At(k, i, j) + if cmplx.Abs(got-want) > 1e-10 { + t.Errorf("w=%v [%d,%d]: FRD=%v, FreqResponse=%v", omega[k], i, j, got, want) + } + } + } + } +} + +func TestFRD_CrossValidateMIMO(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -2, -3}), + mat.NewDense(2, 2, []float64{1, 0, 0, 1}), + mat.NewDense(2, 2, []float64{1, 0, 0, 1}), + mat.NewDense(2, 2, []float64{0, 0, 0, 0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + omega := []float64{0.01, 0.1, 1, 10, 100} + resp, err := sys.FreqResponse(omega) + if err != nil { + t.Fatal(err) + } + frd, err := sys.FRD(omega) + if err != nil { + t.Fatal(err) + } + + _, m, p := sys.Dims() + for k := range omega { + for i := 0; i < p; i++ { + for j := 0; j < m; j++ { + got := frd.At(k, i, j) + want := resp.At(k, i, j) + if cmplx.Abs(got-want) > 1e-10 { + t.Errorf("w=%v [%d,%d]: FRD=%v, FreqResponse=%v", omega[k], i, j, got, want) + } + } + } + } +} + +func TestFRD_EmptyOmega(t *testing.T) { + sys, err := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + frd, err := sys.FRD(nil) + if err != nil { + t.Fatal(err) + } + if frd.NumFrequencies() != 0 { + t.Errorf("expected 0 frequencies, got %d", frd.NumFrequencies()) + } +} + +func TestFRD_EvalFr(t *testing.T) { + resp := [][][]complex128{ + {{1 + 2i, 3 + 4i}, {5 + 6i, 7 + 8i}}, + } + frd, err := NewFRD(resp, []float64{1.0}, 0) + if err != nil { + t.Fatal(err) + } + + grid := frd.EvalFr(0) + if len(grid) != 2 || len(grid[0]) != 2 { + t.Fatalf("EvalFr dims = %dx%d, want 2x2", len(grid), len(grid[0])) + } + if grid[0][0] != 1+2i || grid[0][1] != 3+4i || grid[1][0] != 5+6i || grid[1][1] != 7+8i { + t.Errorf("EvalFr mismatch: %v", grid) + } +} + +func TestFRD_FreqResponseMatrix(t *testing.T) { + resp := [][][]complex128{ + {{1 + 2i}}, + {{3 + 4i}}, + } + frd, err := NewFRD(resp, []float64{1, 10}, 0) + if err != nil { + t.Fatal(err) + } + + frm := frd.FreqResponse() + if frm.NFreq != 2 || frm.P != 1 || frm.M != 1 { + t.Fatalf("FreqResponseMatrix dims: NFreq=%d P=%d M=%d", frm.NFreq, frm.P, frm.M) + } + if frm.At(0, 0, 0) != 1+2i { + t.Errorf("At(0,0,0) = %v, want 1+2i", frm.At(0, 0, 0)) + } + if frm.At(1, 0, 0) != 3+4i { + t.Errorf("At(1,0,0) = %v, want 3+4i", frm.At(1, 0, 0)) + } +} + +func TestFRD_Bode(t *testing.T) { + sys, err := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + omega := []float64{0.1, 1, 10} + frd, err := sys.FRD(omega) + if err != nil { + t.Fatal(err) + } + + bode := frd.Bode() + wantMagDB := []float64{ + 20 * math.Log10(1.0/math.Sqrt(1.01)), + 20 * math.Log10(1.0/math.Sqrt(2)), + 20 * math.Log10(1.0/math.Sqrt(101)), + } + for k := range omega { + got := bode.MagDBAt(k, 0, 0) + if math.Abs(got-wantMagDB[k]) > 1e-8 { + t.Errorf("w=%v: magDB=%v, want %v", omega[k], got, wantMagDB[k]) + } + } +} + +func TestFRD_DataIsolation(t *testing.T) { + resp := [][][]complex128{{{1 + 0i}}} + omega := []float64{1.0} + frd, err := NewFRD(resp, omega, 0) + if err != nil { + t.Fatal(err) + } + + resp[0][0][0] = 999 + 0i + omega[0] = 999 + if frd.At(0, 0, 0) != 1+0i { + t.Error("FRD response mutated by external modification") + } + if frd.Omega[0] != 1.0 { + t.Error("FRD omega mutated by external modification") + } +} + +func TestFRD_NyquistBoundary(t *testing.T) { + dt := 0.1 + nyq := math.Pi / dt + resp := [][][]complex128{{{1 + 0i}}} + _, err := NewFRD(resp, []float64{nyq}, dt) + if err != nil { + t.Fatalf("Nyquist frequency should be allowed, got %v", err) + } +} + +func TestFRD_NyquistFromFRD(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + w := logspace(-1, 1, 50) + f, err := sys.FRD(w) + if err != nil { + t.Fatal(err) + } + res, err := f.Nyquist() + if err != nil { + t.Fatal(err) + } + if len(res.Contour) != 50 { + t.Fatalf("contour len = %d, want 50", len(res.Contour)) + } + if cmplx.Abs(res.Contour[0]-f.Response[0][0][0]) > 1e-12 { + t.Errorf("contour[0] mismatch") + } +} + +func TestFRD_Sigma(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + w := logspace(-1, 1, 20) + f, err := sys.FRD(w) + if err != nil { + t.Fatal(err) + } + sig, err := f.Sigma() + if err != nil { + t.Fatal(err) + } + for k, wk := range w { + want := 1 / math.Sqrt(1+wk*wk) + got := sig.sv[k] + if math.Abs(got-want)/want > 1e-4 { + t.Errorf("w=%g: sigma=%g, want %g", wk, got, want) + } + } +} + +func TestFRDMargin(t *testing.T) { + A := mat.NewDense(3, 3, []float64{0, 1, 0, 0, 0, 1, -6, -11, -6}) + B := mat.NewDense(3, 1, []float64{0, 0, 60}) + C := mat.NewDense(1, 3, []float64{1, 0, 0}) + D := mat.NewDense(1, 1, []float64{0}) + sys, _ := New(A, B, C, D, 0) + w := logspace(-2, 2, 2000) + f, err := sys.FRD(w) + if err != nil { + t.Fatal(err) + } + mr, err := FRDMargin(f) + if err != nil { + t.Fatal(err) + } + sysMr, err := Margin(sys) + if err != nil { + t.Fatal(err) + } + if math.IsInf(sysMr.PhaseMargin, 0) { + t.Skip("system has no gain crossover") + } + if math.Abs(mr.PhaseMargin-sysMr.PhaseMargin) > 3 { + t.Errorf("PM = %g, sys PM = %g", mr.PhaseMargin, sysMr.PhaseMargin) + } +} + diff --git a/inv.go b/inv.go new file mode 100644 index 0000000..f46c2b1 --- /dev/null +++ b/inv.go @@ -0,0 +1,69 @@ +package controlsys + +import ( + "fmt" + + "gonum.org/v1/gonum/mat" +) + +func Inv(sys *System) (*System, error) { + n, m, p := sys.Dims() + if m != p { + return nil, fmt.Errorf("Inv: system must be square (p=%d, m=%d): %w", p, m, ErrDimensionMismatch) + } + + if m == 0 { + return sys.Copy(), nil + } + + var luD mat.LU + luD.Factorize(sys.D) + if luNearSingular(&luD) { + return nil, fmt.Errorf("Inv: D matrix is singular: %w", ErrSingularTransform) + } + + Dinv := mat.NewDense(m, m, nil) + ones := make([]float64, m) + for i := range ones { + ones[i] = 1 + } + eye := mat.NewDiagDense(m, ones) + if err := luD.SolveTo(Dinv, false, eye); err != nil { + return nil, fmt.Errorf("Inv: %w", ErrSingularTransform) + } + + if n == 0 { + result, err := NewGain(Dinv, sys.Dt) + if err != nil { + return nil, err + } + propagateIONames(result, sys) + return result, nil + } + + var DinvC, BDinvC, Ainv, BDinv, DinvCneg mat.Dense + + DinvC.Mul(Dinv, sys.C) + BDinvC.Mul(sys.B, &DinvC) + + Ainv.Sub(sys.A, &BDinvC) + BDinv.Mul(sys.B, Dinv) + DinvCneg.Scale(-1, &DinvC) + + result, err := newNoCopy( + mat.DenseCopyOf(&Ainv), + mat.DenseCopyOf(&BDinv), + mat.DenseCopyOf(&DinvCneg), + Dinv, + sys.Dt, + ) + if err != nil { + return nil, err + } + + result.InputName = copyStringSlice(sys.OutputName) + result.OutputName = copyStringSlice(sys.InputName) + result.StateName = copyStringSlice(sys.StateName) + + return result, nil +} diff --git a/loopsens.go b/loopsens.go new file mode 100644 index 0000000..d0b6cfc --- /dev/null +++ b/loopsens.go @@ -0,0 +1,61 @@ +package controlsys + +import ( + "fmt" + + "gonum.org/v1/gonum/mat" +) + +type LoopsensResult struct { + So *System + To *System + Si *System + Ti *System +} + +func Loopsens(L *System) (*LoopsensResult, error) { + if L == nil { + return nil, fmt.Errorf("controlsys: loop transfer function cannot be nil") + } + + _, m, p := L.Dims() + if m != p { + return nil, fmt.Errorf("controlsys: loop transfer must be square (%d inputs, %d outputs): %w", m, p, ErrDimensionMismatch) + } + + eyeData := make([]float64, m*m) + for i := 0; i < m; i++ { + eyeData[i*(m+1)] = 1 + } + I, err := NewGain(mat.NewDense(m, m, eyeData), L.Dt) + if err != nil { + return nil, err + } + + So, err := Feedback(I, L, -1) + if err != nil { + return nil, fmt.Errorf("controlsys: loopsens So computation failed: %w", err) + } + + To, err := Feedback(L, I, -1) + if err != nil { + return nil, fmt.Errorf("controlsys: loopsens To computation failed: %w", err) + } + + Si, err := Feedback(I, L, -1) + if err != nil { + return nil, fmt.Errorf("controlsys: loopsens Si computation failed: %w", err) + } + + Ti, err := Feedback(L, I, -1) + if err != nil { + return nil, fmt.Errorf("controlsys: loopsens Ti computation failed: %w", err) + } + + return &LoopsensResult{ + So: So, + To: To, + Si: Si, + Ti: Ti, + }, nil +} diff --git a/loopsens_test.go b/loopsens_test.go new file mode 100644 index 0000000..c65fe92 --- /dev/null +++ b/loopsens_test.go @@ -0,0 +1,142 @@ +package controlsys + +import ( + "math" + "math/cmplx" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestLoopsens_FirstOrder(t *testing.T) { + L, err := New( + mat.NewDense(1, 1, []float64{-2}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Loopsens(L) + if err != nil { + t.Fatal(err) + } + + sDC, err := res.So.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(sDC.At(0, 0)-2.0/3.0) > 1e-8 { + t.Errorf("dcgain(S) = %g, want 2/3", sDC.At(0, 0)) + } + + tDC, err := res.To.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(tDC.At(0, 0)-1.0/3.0) > 1e-8 { + t.Errorf("dcgain(T) = %g, want 1/3", tDC.At(0, 0)) + } + + if math.Abs(sDC.At(0, 0)+tDC.At(0, 0)-1.0) > 1e-8 { + t.Errorf("S+T DC gain = %g, want 1", sDC.At(0, 0)+tDC.At(0, 0)) + } + + for _, w := range []float64{0.01, 0.1, 1, 10, 100} { + sResp, _ := res.So.EvalFr(complex(0, w)) + tResp, _ := res.To.EvalFr(complex(0, w)) + sum := sResp[0][0] + tResp[0][0] + if cmplx.Abs(sum-1) > 1e-6 { + t.Errorf("w=%g: S+T = %v, want 1", w, sum) + } + } +} + +func TestLoopsens_Integrator(t *testing.T) { + L, err := New( + mat.NewDense(1, 1, []float64{0}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Loopsens(L) + if err != nil { + t.Fatal(err) + } + + tDC, err := res.To.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(tDC.At(0, 0)-1.0) > 1e-8 { + t.Errorf("dcgain(T) = %g, want 1", tDC.At(0, 0)) + } + + for _, w := range []float64{0.1, 1, 10} { + sResp, _ := res.So.EvalFr(complex(0, w)) + tResp, _ := res.To.EvalFr(complex(0, w)) + sum := sResp[0][0] + tResp[0][0] + if cmplx.Abs(sum-1) > 1e-6 { + t.Errorf("w=%g: S+T = %v, want 1", w, sum) + } + } +} + +func TestLoopsens_NilError(t *testing.T) { + _, err := Loopsens(nil) + if err == nil { + t.Error("expected error for nil input") + } +} + +func TestLoopsens_NonSquareError(t *testing.T) { + L, err := New( + mat.NewDense(2, 2, []float64{-1, 0, 0, -2}), + mat.NewDense(2, 1, []float64{1, 1}), + mat.NewDense(2, 2, []float64{1, 0, 0, 1}), + mat.NewDense(2, 1, []float64{0, 0}), 0) + if err != nil { + t.Fatal(err) + } + + _, err = Loopsens(L) + if err == nil { + t.Error("expected error for non-square system") + } +} + +func TestLoopsens_StabilityCheck(t *testing.T) { + L, err := New( + mat.NewDense(1, 1, []float64{-2}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Loopsens(L) + if err != nil { + t.Fatal(err) + } + + stable, err := res.So.IsStable() + if err != nil { + t.Fatal(err) + } + if !stable { + t.Error("sensitivity should be stable") + } + + stable, err = res.To.IsStable() + if err != nil { + t.Fatal(err) + } + if !stable { + t.Error("complementary sensitivity should be stable") + } +} diff --git a/modsep.go b/modsep.go new file mode 100644 index 0000000..fbdb2de --- /dev/null +++ b/modsep.go @@ -0,0 +1,28 @@ +package controlsys + +import ( + "fmt" + "math/cmplx" +) + +type ModsepResult struct { + Slow *System + Fast *System +} + +func Modsep(sys *System, cutoff float64) (*ModsepResult, error) { + if cutoff <= 0 { + return nil, fmt.Errorf("controlsys: cutoff must be positive") + } + + isSlow := func(ev complex128) bool { + return cmplx.Abs(ev) < cutoff + } + + slow, fast, err := decomposeByEigenvalues(sys, isSlow) + if err != nil { + return nil, err + } + + return &ModsepResult{Slow: slow, Fast: fast}, nil +} diff --git a/modsep_test.go b/modsep_test.go new file mode 100644 index 0000000..9722cc6 --- /dev/null +++ b/modsep_test.go @@ -0,0 +1,191 @@ +package controlsys + +import ( + "math/cmplx" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestModsep_ClearSeparation(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{-1, 0, 0, 0, -10, 0, 0, 0, -100}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, nil), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Modsep(sys, 5) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Slow.Dims() + nf, _, _ := res.Fast.Dims() + + if ns != 1 { + t.Errorf("slow order = %d, want 1", ns) + } + if nf != 2 { + t.Errorf("fast order = %d, want 2", nf) + } + + slowPoles, _ := res.Slow.Poles() + for _, p := range slowPoles { + if cmplx.Abs(p) >= 5 { + t.Errorf("slow pole %v has |λ| >= 5", p) + } + } + + fastPoles, _ := res.Fast.Poles() + for _, p := range fastPoles { + if cmplx.Abs(p) < 5 { + t.Errorf("fast pole %v has |λ| < 5", p) + } + } + + checkAdditiveDecomposition(t, sys, res.Slow, res.Fast) +} + +func TestModsep_DifferentCutoff(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{-1, 0, 0, 0, -10, 0, 0, 0, -100}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, nil), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Modsep(sys, 50) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Slow.Dims() + nf, _, _ := res.Fast.Dims() + + if ns != 2 { + t.Errorf("slow order = %d, want 2", ns) + } + if nf != 1 { + t.Errorf("fast order = %d, want 1", nf) + } + + checkAdditiveDecomposition(t, sys, res.Slow, res.Fast) +} + +func TestModsep_AllFast(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{-1, 0, 0, 0, -10, 0, 0, 0, -100}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, nil), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Modsep(sys, 0.1) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Slow.Dims() + nf, _, _ := res.Fast.Dims() + + if ns != 0 { + t.Errorf("slow order = %d, want 0", ns) + } + if nf != 3 { + t.Errorf("fast order = %d, want 3", nf) + } +} + +func TestModsep_Discrete(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{0.1, 0, 0, 0, 0.5, 0, 0, 0, 0.99}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, nil), 0.1) + if err != nil { + t.Fatal(err) + } + + res, err := Modsep(sys, 0.3) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Slow.Dims() + nf, _, _ := res.Fast.Dims() + + if ns != 1 { + t.Errorf("slow order = %d, want 1", ns) + } + if nf != 2 { + t.Errorf("fast order = %d, want 2", nf) + } + + checkAdditiveDecomposition(t, sys, res.Slow, res.Fast) +} + +func TestModsep_InvalidCutoff(t *testing.T) { + sys, _ := New( + mat.NewDense(2, 2, []float64{-1, 0, 0, -2}), + mat.NewDense(2, 1, []float64{1, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), 0) + + _, err := Modsep(sys, -1) + if err == nil { + t.Error("expected error for negative cutoff") + } +} + +func TestModsep_Empty(t *testing.T) { + sys, _ := NewGain(mat.NewDense(1, 1, []float64{5}), 0) + + res, err := Modsep(sys, 1.0) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Slow.Dims() + if ns != 0 { + t.Errorf("slow order = %d, want 0", ns) + } +} + +func TestModsep_NonDiagonal(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{ + -1, 0.5, 0, + 0, -10, 0, + 0, 0, -100, + }), + mat.NewDense(3, 1, []float64{1, 1, 1}), + mat.NewDense(1, 3, []float64{1, 1, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Modsep(sys, 5) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Slow.Dims() + nf, _, _ := res.Fast.Dims() + + if ns != 1 { + t.Errorf("slow order = %d, want 1", ns) + } + if nf != 2 { + t.Errorf("fast order = %d, want 2", nf) + } + + checkAdditiveDecomposition(t, sys, res.Slow, res.Fast) +} diff --git a/norm_covar_test.go b/norm_covar_test.go new file mode 100644 index 0000000..a4f6c08 --- /dev/null +++ b/norm_covar_test.go @@ -0,0 +1,286 @@ +package controlsys + +import ( + "errors" + "math" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestNorm_H2_Continuous(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + + got, err := Norm(sys, 2) + if err != nil { + t.Fatal(err) + } + want := 0.707106781186548 + if math.Abs(got-want) > 1e-10 { + t.Errorf("Norm(sys,2) = %g, want %g", got, want) + } +} + +func TestNorm_Inf_Continuous(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + + got, err := Norm(sys, math.Inf(1)) + if err != nil { + t.Fatal(err) + } + want := 1.0 + if math.Abs(got-want) > 1e-6 { + t.Errorf("Norm(sys,Inf) = %g, want %g", got, want) + } +} + +func TestNorm_H2_Discrete(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{0.5}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0.1) + + got, err := Norm(sys, 2) + if err != nil { + t.Fatal(err) + } + want := 1.154700538379252 + if math.Abs(got-want)/want > 1e-6 { + t.Errorf("Norm(sys,2) = %g, want %g", got, want) + } +} + +func TestNorm_Inf_Discrete(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{0.5}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0.1) + + got, err := Norm(sys, math.Inf(1)) + if err != nil { + t.Fatal(err) + } + want := 2.0 + if math.Abs(got-want) > 1e-6 { + t.Errorf("Norm(sys,Inf) = %g, want %g", got, want) + } +} + +func TestNorm_InvalidType(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + + _, err := Norm(sys, 3) + if err == nil { + t.Fatal("expected error for invalid normType") + } +} + +func TestCovar_SISO_Continuous(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + + P, err := Covar(sys, mat.NewDense(1, 1, []float64{1})) + if err != nil { + t.Fatal(err) + } + want := 0.5 + if math.Abs(P.At(0, 0)-want) > 1e-10 { + t.Errorf("Covar = %g, want %g", P.At(0, 0), want) + } +} + +func TestCovar_SISO_Continuous_WithD(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0.5}), 0) + + P, err := Covar(sys, mat.NewDense(1, 1, []float64{1})) + if err != nil { + t.Fatal(err) + } + want := 0.75 + if math.Abs(P.At(0, 0)-want) > 1e-10 { + t.Errorf("Covar = %g, want %g", P.At(0, 0), want) + } +} + +func TestCovar_MIMO_Continuous(t *testing.T) { + sys, _ := New( + mat.NewDense(2, 2, []float64{0, 1, -2, -3}), + mat.NewDense(2, 1, []float64{0, 1}), + mat.NewDense(2, 2, []float64{1, 0, 0, 1}), + mat.NewDense(2, 1, []float64{0, 0}), 0) + + P, err := Covar(sys, mat.NewDense(1, 1, []float64{1})) + if err != nil { + t.Fatal(err) + } + r, c := P.Dims() + if r != 2 || c != 2 { + t.Fatalf("P dims = %d×%d, want 2×2", r, c) + } + want := mat.NewDense(2, 2, []float64{1.0 / 12, 0, 0, 1.0 / 6}) + if !matEqual(P, want, 1e-10) { + t.Errorf("Covar =\n%v\nwant\n%v", mat.Formatted(P), mat.Formatted(want)) + } +} + +func TestCovar_Discrete(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{0.5}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0.1) + + P, err := Covar(sys, mat.NewDense(1, 1, []float64{1})) + if err != nil { + t.Fatal(err) + } + want := 4.0 / 3.0 + if math.Abs(P.At(0, 0)-want) > 1e-10 { + t.Errorf("Covar = %g, want %g", P.At(0, 0), want) + } +} + +func TestCovar_Unstable(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + + _, err := Covar(sys, mat.NewDense(1, 1, []float64{1})) + if !errors.Is(err, ErrUnstable) { + t.Errorf("got %v, want ErrUnstable", err) + } +} + +func TestCovar_BadW(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + + _, err := Covar(sys, mat.NewDense(2, 2, nil)) + if !errors.Is(err, ErrDimensionMismatch) { + t.Errorf("got %v, want ErrDimensionMismatch", err) + } +} + +func TestLsim_Step_Continuous(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + + steps := 501 + tVec := make([]float64, steps) + uMat := mat.NewDense(steps, 1, nil) + for i := 0; i < steps; i++ { + tVec[i] = float64(i) * 0.01 + uMat.Set(i, 0, 1.0) + } + + resp, err := Lsim(sys, uMat, tVec, nil) + if err != nil { + t.Fatal(err) + } + + _, cols := resp.Y.Dims() + yEnd := resp.Y.At(0, cols-1) + want := 1 - math.Exp(-5) + if math.Abs(yEnd-want) > 1e-3 { + t.Errorf("y(end) = %g, want %g", yEnd, want) + } +} + +func TestLsim_InitialCondition_Continuous(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + + steps := 201 + tVec := make([]float64, steps) + uMat := mat.NewDense(steps, 1, nil) + for i := 0; i < steps; i++ { + tVec[i] = float64(i) * 0.01 + } + x0 := mat.NewVecDense(1, []float64{5}) + + resp, err := Lsim(sys, uMat, tVec, x0) + if err != nil { + t.Fatal(err) + } + + _, cols := resp.Y.Dims() + yEnd := resp.Y.At(0, cols-1) + want := 5 * math.Exp(-2) + if math.Abs(yEnd-want) > 1e-3 { + t.Errorf("y(end) = %g, want %g", yEnd, want) + } +} + +func TestLsim_Discrete(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{0.5}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0.1) + + steps := 10 + tVec := make([]float64, steps) + uMat := mat.NewDense(steps, 1, nil) + for i := 0; i < steps; i++ { + tVec[i] = float64(i) * 0.1 + uMat.Set(i, 0, 1.0) + } + + resp, err := Lsim(sys, uMat, tVec, nil) + if err != nil { + t.Fatal(err) + } + + if resp.Y.At(0, 0) != 0 { + t.Errorf("y(0) = %g, want 0 (no feedthrough)", resp.Y.At(0, 0)) + } + yEnd := resp.Y.At(0, steps-1) + if yEnd <= 0 { + t.Errorf("y(end) = %g, want positive", yEnd) + } +} + +func TestLsim_BadDims(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + + _, err := Lsim(sys, mat.NewDense(5, 2, nil), []float64{0, 1, 2, 3, 4}, nil) + if !errors.Is(err, ErrDimensionMismatch) { + t.Errorf("got %v, want ErrDimensionMismatch", err) + } +} diff --git a/norms.go b/norms.go index e1ca212..a1225bf 100644 --- a/norms.go +++ b/norms.go @@ -1,6 +1,7 @@ package controlsys import ( + "fmt" "math" "math/cmplx" "sort" @@ -11,6 +12,19 @@ import ( "gonum.org/v1/gonum/mat" ) +const NormH2 = 2 + +func Norm(sys *System, normType float64) (float64, error) { + if normType == 2 { + return H2Norm(sys) + } + if math.IsInf(normType, 1) { + norm, _, err := HinfNorm(sys) + return norm, err + } + return 0, fmt.Errorf("controlsys: normType must be 2 or Inf, got %g", normType) +} + // H2Norm computes the H2 norm of a stable LTI system. // // For continuous systems with D ≠ 0, the H2 norm is infinite. diff --git a/pid.go b/pid.go new file mode 100644 index 0000000..7a36769 --- /dev/null +++ b/pid.go @@ -0,0 +1,307 @@ +package controlsys + +import ( + "fmt" + + "gonum.org/v1/gonum/mat" +) + +type PIDForm int + +const ( + PIDParallel PIDForm = iota + PIDStandard +) + +type PID struct { + Kp float64 + Ki float64 + Kd float64 + Tf float64 + Dt float64 + Form PIDForm +} + +type PIDOption func(*PID) + +func WithFilter(Tf float64) PIDOption { + return func(p *PID) { p.Tf = Tf } +} + +func WithTs(dt float64) PIDOption { + return func(p *PID) { p.Dt = dt } +} + +func NewPID(Kp, Ki, Kd float64, opts ...PIDOption) *PID { + p := &PID{Kp: Kp, Ki: Ki, Kd: Kd, Form: PIDParallel} + for _, o := range opts { + o(p) + } + return p +} + +// NewPIDStd creates a PID in standard (ISA) form: +// +// C(s) = Kp * (1 + 1/(Ti*s) + Td*s/(Tf*s + 1)) +// +// Relation to parallel: Ki = Kp/Ti, Kd = Kp*Td. +func NewPIDStd(Kp, Ti, Td float64, opts ...PIDOption) (*PID, error) { + if Ti == 0 && Kp != 0 { + return nil, fmt.Errorf("controlsys: Ti must be nonzero in standard form") + } + var Ki, Kd float64 + if Ti != 0 { + Ki = Kp / Ti + } + Kd = Kp * Td + p := &PID{Kp: Kp, Ki: Ki, Kd: Kd, Form: PIDStandard} + for _, o := range opts { + o(p) + } + return p, nil +} + +// Parallel returns a copy in parallel form (Kp, Ki, Kd). +func (p *PID) Parallel() *PID { + return &PID{Kp: p.Kp, Ki: p.Ki, Kd: p.Kd, Tf: p.Tf, Dt: p.Dt, Form: PIDParallel} +} + +// Standard returns a copy in standard form (Kp, Ti, Td). +// Ti = Kp/Ki, Td = Kd/Kp. Requires Kp != 0 and Ki != 0. +func (p *PID) Standard() (*PID, error) { + if p.Kp == 0 { + return nil, fmt.Errorf("controlsys: Kp must be nonzero for standard form") + } + if p.Ki == 0 { + return nil, fmt.Errorf("controlsys: Ki must be nonzero for standard form (Ti would be Inf)") + } + return &PID{Kp: p.Kp, Ki: p.Ki, Kd: p.Kd, Tf: p.Tf, Dt: p.Dt, Form: PIDStandard}, nil +} + +// Ti returns the integral time constant (standard form). Returns 0 if Ki=0. +func (p *PID) Ti() float64 { + if p.Ki == 0 { + return 0 + } + return p.Kp / p.Ki +} + +// Td returns the derivative time constant (standard form). Returns 0 if Kp=0. +func (p *PID) Td() float64 { + if p.Kp == 0 { + return 0 + } + return p.Kd / p.Kp +} + +// PID2 represents a 2-DOF PID controller. +// +// u = Kp*(b*r - y) + Ki/s*(r - y) + Kd*s/(Tf*s+1)*(c*r - y) +// +// The System() method produces a 2-input (r, y) to 1-output (u) system. +type PID2 struct { + Kp float64 + Ki float64 + Kd float64 + Tf float64 + B float64 // setpoint weight on proportional + C float64 // setpoint weight on derivative + Dt float64 +} + +func NewPID2(Kp, Ki, Kd, Tf, b, c float64, opts ...PIDOption) *PID2 { + p2 := &PID2{Kp: Kp, Ki: Ki, Kd: Kd, Tf: Tf, B: b, C: c} + tmp := &PID{Dt: p2.Dt} + for _, o := range opts { + o(tmp) + } + p2.Dt = tmp.Dt + return p2 +} + +// System converts the 2-DOF PID to a 2-input (r,y) 1-output (u) state-space. +func (p *PID2) System() (*System, error) { + if p.Kd != 0 && p.Tf == 0 { + return nil, fmt.Errorf("controlsys: 2-DOF PD without filter (Tf=0) is improper; set Tf > 0") + } + + hasI := p.Ki != 0 + hasD := p.Kd != 0 && p.Tf != 0 + + n := 0 + if hasI { + n++ + } + if hasD { + n++ + } + + if n == 0 { + D := mat.NewDense(1, 2, []float64{p.Kp * p.B, -p.Kp}) + return NewGain(D, p.Dt) + } + + A := mat.NewDense(n, n, nil) + Bmat := mat.NewDense(n, 2, nil) + Cmat := mat.NewDense(1, n, nil) + Dmat := mat.NewDense(1, 2, nil) + + idx := 0 + dFeedR := p.Kp * p.B + dFeedY := -p.Kp + + if hasI { + A.Set(idx, idx, 0) + Bmat.Set(idx, 0, 1) + Bmat.Set(idx, 1, -1) + Cmat.Set(0, idx, p.Ki) + idx++ + } + + if hasD { + invTf := 1.0 / p.Tf + A.Set(idx, idx, -invTf) + Bmat.Set(idx, 0, p.C*invTf) + Bmat.Set(idx, 1, -invTf) + Cmat.Set(0, idx, -p.Kd*invTf) + dFeedR += p.Kd * invTf * p.C + dFeedY += -p.Kd * invTf + } + + Dmat.Set(0, 0, dFeedR) + Dmat.Set(0, 1, dFeedY) + + if p.Dt > 0 { + return p.discrete2DOF(n, hasI, hasD) + } + + return New(A, Bmat, Cmat, Dmat, 0) +} + +func (p *PID2) discrete2DOF(n int, hasI, hasD bool) (*System, error) { + dt := p.Dt + A := mat.NewDense(n, n, nil) + Bmat := mat.NewDense(n, 2, nil) + Cmat := mat.NewDense(1, n, nil) + Dmat := mat.NewDense(1, 2, nil) + + idx := 0 + dFeedR := p.Kp * p.B + dFeedY := -p.Kp + + if hasI { + A.Set(idx, idx, 1) + Bmat.Set(idx, 0, dt) + Bmat.Set(idx, 1, -dt) + Cmat.Set(0, idx, p.Ki) + idx++ + } + + if hasD { + alpha := 1.0 - dt/p.Tf + invTf := 1.0 / p.Tf + A.Set(idx, idx, alpha) + Bmat.Set(idx, 0, dt*invTf*p.C) + Bmat.Set(idx, 1, -dt*invTf) + Cmat.Set(0, idx, -p.Kd*invTf) + dFeedR += p.Kd * invTf * p.C + dFeedY += -p.Kd * invTf + } + + Dmat.Set(0, 0, dFeedR) + Dmat.Set(0, 1, dFeedY) + + return New(A, Bmat, Cmat, Dmat, dt) +} + +func (p *PID) System() (*System, error) { + if p.Kd != 0 && p.Tf == 0 && p.Ki == 0 { + return nil, fmt.Errorf("controlsys: PD without filter (Tf=0) is improper; set Tf > 0") + } + + if p.Dt > 0 { + return p.discreteSystem() + } + return p.continuousSystem() +} + +func (p *PID) continuousSystem() (*System, error) { + hasI := p.Ki != 0 + hasD := p.Kd != 0 && p.Tf != 0 + + switch { + case !hasI && !hasD: + return NewGain(mat.NewDense(1, 1, []float64{p.Kp}), 0) + + case hasI && !hasD: + return New( + mat.NewDense(1, 1, []float64{0}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{p.Ki}), + mat.NewDense(1, 1, []float64{p.Kp}), + 0, + ) + + case !hasI && hasD: + invTf := 1.0 / p.Tf + return New( + mat.NewDense(1, 1, []float64{-invTf}), + mat.NewDense(1, 1, []float64{invTf}), + mat.NewDense(1, 1, []float64{-p.Kd * invTf}), + mat.NewDense(1, 1, []float64{p.Kp + p.Kd*invTf}), + 0, + ) + + default: + invTf := 1.0 / p.Tf + return New( + mat.NewDense(2, 2, []float64{0, 0, 0, -invTf}), + mat.NewDense(2, 1, []float64{1, invTf}), + mat.NewDense(1, 2, []float64{p.Ki, -p.Kd * invTf}), + mat.NewDense(1, 1, []float64{p.Kp + p.Kd*invTf}), + 0, + ) + } +} + +func (p *PID) discreteSystem() (*System, error) { + hasI := p.Ki != 0 + hasD := p.Kd != 0 && p.Tf != 0 + dt := p.Dt + + switch { + case !hasI && !hasD: + return NewGain(mat.NewDense(1, 1, []float64{p.Kp}), dt) + + case hasI && !hasD: + return New( + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{dt}), + mat.NewDense(1, 1, []float64{p.Ki}), + mat.NewDense(1, 1, []float64{p.Kp}), + dt, + ) + + case !hasI && hasD: + alpha := 1.0 - dt/p.Tf + invTf := 1.0 / p.Tf + return New( + mat.NewDense(1, 1, []float64{alpha}), + mat.NewDense(1, 1, []float64{dt * invTf}), + mat.NewDense(1, 1, []float64{-p.Kd * invTf}), + mat.NewDense(1, 1, []float64{p.Kp + p.Kd*invTf}), + dt, + ) + + default: + alpha := 1.0 - dt/p.Tf + invTf := 1.0 / p.Tf + return New( + mat.NewDense(2, 2, []float64{1, 0, 0, alpha}), + mat.NewDense(2, 1, []float64{dt, dt * invTf}), + mat.NewDense(1, 2, []float64{p.Ki, -p.Kd * invTf}), + mat.NewDense(1, 1, []float64{p.Kp + p.Kd*invTf}), + dt, + ) + } +} diff --git a/pid_test.go b/pid_test.go new file mode 100644 index 0000000..1d6f4b8 --- /dev/null +++ b/pid_test.go @@ -0,0 +1,438 @@ +package controlsys + +import ( + "math" + "math/cmplx" + "sort" + "testing" +) + +const pidTol = 1e-10 + +func pidPoles(t *testing.T, sys *System) []complex128 { + t.Helper() + poles, err := sys.Poles() + if err != nil { + t.Fatal(err) + } + sort.Slice(poles, func(i, j int) bool { + if real(poles[i]) != real(poles[j]) { + return real(poles[i]) < real(poles[j]) + } + return imag(poles[i]) < imag(poles[j]) + }) + return poles +} + +func pidZeros(t *testing.T, sys *System) []complex128 { + t.Helper() + zeros, err := sys.Zeros() + if err != nil { + t.Fatal(err) + } + sort.Slice(zeros, func(i, j int) bool { + if real(zeros[i]) != real(zeros[j]) { + return real(zeros[i]) < real(zeros[j]) + } + return imag(zeros[i]) < imag(zeros[j]) + }) + return zeros +} + +func pidDCGain(t *testing.T, sys *System) float64 { + t.Helper() + g, err := sys.DCGain() + if err != nil { + t.Fatal(err) + } + return g.At(0, 0) +} + +func assertComplexSlice(t *testing.T, name string, got, want []complex128) { + t.Helper() + if len(got) != len(want) { + t.Fatalf("%s: len %d, want %d; got %v", name, len(got), len(want), got) + } + for i := range want { + if cmplx.Abs(got[i]-want[i]) > pidTol { + t.Errorf("%s[%d] = %v, want %v", name, i, got[i], want[i]) + } + } +} + +func TestPID_PureP(t *testing.T) { + c := NewPID(2, 0, 0) + sys, err := c.System() + if err != nil { + t.Fatal(err) + } + n, m, p := sys.Dims() + if n != 0 || m != 1 || p != 1 { + t.Fatalf("dims = (%d,%d,%d), want (0,1,1)", n, m, p) + } + if d := sys.D.At(0, 0); math.Abs(d-2) > pidTol { + t.Errorf("D = %v, want 2", d) + } +} + +func TestPID_PI(t *testing.T) { + c := NewPID(1, 2, 0) + sys, err := c.System() + if err != nil { + t.Fatal(err) + } + n, _, _ := sys.Dims() + if n != 1 { + t.Fatalf("states = %d, want 1", n) + } + if v := sys.A.At(0, 0); math.Abs(v) > pidTol { + t.Errorf("A = %v, want 0", v) + } + if v := sys.B.At(0, 0); math.Abs(v-1) > pidTol { + t.Errorf("B = %v, want 1", v) + } + if v := sys.C.At(0, 0); math.Abs(v-2) > pidTol { + t.Errorf("C = %v, want 2", v) + } + if v := sys.D.At(0, 0); math.Abs(v-1) > pidTol { + t.Errorf("D = %v, want 1", v) + } + poles := pidPoles(t, sys) + assertComplexSlice(t, "poles", poles, []complex128{0}) +} + +func TestPID_PDFiltered(t *testing.T) { + c := NewPID(1, 0, 3, WithFilter(0.5)) + sys, err := c.System() + if err != nil { + t.Fatal(err) + } + n, _, _ := sys.Dims() + if n != 1 { + t.Fatalf("states = %d, want 1", n) + } + if v := sys.A.At(0, 0); math.Abs(v-(-2)) > pidTol { + t.Errorf("A = %v, want -2", v) + } + if v := sys.B.At(0, 0); math.Abs(v-2) > pidTol { + t.Errorf("B = %v, want 2", v) + } + if v := sys.C.At(0, 0); math.Abs(v-(-6)) > pidTol { + t.Errorf("C = %v, want -6", v) + } + if v := sys.D.At(0, 0); math.Abs(v-7) > pidTol { + t.Errorf("D = %v, want 7", v) + } + poles := pidPoles(t, sys) + assertComplexSlice(t, "poles", poles, []complex128{-2}) + g := pidDCGain(t, sys) + if math.Abs(g-1) > pidTol { + t.Errorf("DCGain = %v, want 1 (Kp)", g) + } +} + +func TestPID_PIDFiltered(t *testing.T) { + c := NewPID(1, 2, 3, WithFilter(0.5)) + sys, err := c.System() + if err != nil { + t.Fatal(err) + } + n, _, _ := sys.Dims() + if n != 2 { + t.Fatalf("states = %d, want 2", n) + } + if v := sys.A.At(0, 0); math.Abs(v) > pidTol { + t.Errorf("A[0,0] = %v, want 0", v) + } + if v := sys.A.At(0, 1); math.Abs(v) > pidTol { + t.Errorf("A[0,1] = %v, want 0", v) + } + if v := sys.A.At(1, 0); math.Abs(v) > pidTol { + t.Errorf("A[1,0] = %v, want 0", v) + } + if v := sys.A.At(1, 1); math.Abs(v-(-2)) > pidTol { + t.Errorf("A[1,1] = %v, want -2", v) + } + if v := sys.B.At(0, 0); math.Abs(v-1) > pidTol { + t.Errorf("B[0] = %v, want 1", v) + } + if v := sys.B.At(1, 0); math.Abs(v-2) > pidTol { + t.Errorf("B[1] = %v, want 2", v) + } + if v := sys.C.At(0, 0); math.Abs(v-2) > pidTol { + t.Errorf("C[0] = %v, want 2", v) + } + if v := sys.C.At(0, 1); math.Abs(v-(-6)) > pidTol { + t.Errorf("C[1] = %v, want -6", v) + } + if v := sys.D.At(0, 0); math.Abs(v-7) > pidTol { + t.Errorf("D = %v, want 7", v) + } + poles := pidPoles(t, sys) + assertComplexSlice(t, "poles", poles, []complex128{-2, 0}) + + tfr, err := sys.TransferFunction(nil) + if err != nil { + t.Fatal(err) + } + s := complex(0, 1.0) + h := tfr.TF.Eval(s)[0][0] + want := complex(1, 0) + complex(2, 0)/s + complex(3, 0)*s/(complex(0.5, 0)*s+1) + if cmplx.Abs(h-want) > 1e-8 { + t.Errorf("TF at s=j = %v, want %v", h, want) + } +} + +func TestPID_PDNoFilter_Error(t *testing.T) { + c := NewPID(1, 0, 3) + _, err := c.System() + if err == nil { + t.Fatal("expected error for PD without filter") + } +} + +func TestPID_DiscretePI(t *testing.T) { + c := NewPID(1, 2, 0, WithTs(0.1)) + sys, err := c.System() + if err != nil { + t.Fatal(err) + } + n, _, _ := sys.Dims() + if n != 1 { + t.Fatalf("states = %d, want 1", n) + } + if !sys.IsDiscrete() { + t.Fatal("expected discrete system") + } + if v := sys.A.At(0, 0); math.Abs(v-1) > pidTol { + t.Errorf("A = %v, want 1", v) + } + if v := sys.B.At(0, 0); math.Abs(v-0.1) > pidTol { + t.Errorf("B = %v, want 0.1", v) + } + if v := sys.C.At(0, 0); math.Abs(v-2) > pidTol { + t.Errorf("C = %v, want 2", v) + } + if v := sys.D.At(0, 0); math.Abs(v-1) > pidTol { + t.Errorf("D = %v, want 1", v) + } + poles := pidPoles(t, sys) + assertComplexSlice(t, "poles", poles, []complex128{1}) + + zeros := pidZeros(t, sys) + assertComplexSlice(t, "zeros", zeros, []complex128{0.8}) +} + +func TestPID_DiscretePIDFiltered(t *testing.T) { + c := NewPID(1, 2, 3, WithFilter(0.5), WithTs(0.1)) + sys, err := c.System() + if err != nil { + t.Fatal(err) + } + n, _, _ := sys.Dims() + if n != 2 { + t.Fatalf("states = %d, want 2", n) + } + if !sys.IsDiscrete() { + t.Fatal("expected discrete system") + } + if v := sys.A.At(0, 0); math.Abs(v-1) > pidTol { + t.Errorf("A[0,0] = %v, want 1", v) + } + if v := sys.A.At(1, 1); math.Abs(v-0.8) > pidTol { + t.Errorf("A[1,1] = %v, want 0.8", v) + } + if v := sys.B.At(0, 0); math.Abs(v-0.1) > pidTol { + t.Errorf("B[0] = %v, want 0.1", v) + } + if v := sys.B.At(1, 0); math.Abs(v-0.2) > pidTol { + t.Errorf("B[1] = %v, want 0.2", v) + } + if v := sys.D.At(0, 0); math.Abs(v-7) > pidTol { + t.Errorf("D = %v, want 7", v) + } +} + +func TestPID_PIDFilteredTransferFunction(t *testing.T) { + c := NewPID(2, 5, 0.5, WithFilter(0.1)) + sys, err := c.System() + if err != nil { + t.Fatal(err) + } + tfr, err := sys.TransferFunction(nil) + if err != nil { + t.Fatal(err) + } + + freqs := []float64{0.01, 0.1, 1, 10, 100} + for _, w := range freqs { + s := complex(0, w) + got := tfr.TF.Eval(s)[0][0] + want := complex(2, 0) + complex(5, 0)/s + 0.5*s/(0.1*s+1) + if cmplx.Abs(got-want) > 1e-6*cmplx.Abs(want) { + t.Errorf("w=%g: got %v, want %v", w, got, want) + } + } +} + +func TestPID_StandardForm(t *testing.T) { + c := NewPID(4, 0, 0) + c.Form = PIDStandard + sys, err := c.System() + if err != nil { + t.Fatal(err) + } + n, _, _ := sys.Dims() + if n != 0 { + t.Fatalf("states = %d, want 0", n) + } + if v := sys.D.At(0, 0); math.Abs(v-4) > pidTol { + t.Errorf("D = %v, want 4", v) + } +} + +func TestPID_WithFilterAndTs(t *testing.T) { + c := NewPID(1, 0, 0, WithFilter(0.5), WithTs(0.01)) + if c.Tf != 0.5 { + t.Errorf("Tf = %v, want 0.5", c.Tf) + } + if c.Dt != 0.01 { + t.Errorf("Dt = %v, want 0.01", c.Dt) + } +} + +func TestPID_KdWithIButNoFilter_Works(t *testing.T) { + c := NewPID(1, 2, 3, WithFilter(0.5)) + _, err := c.System() + if err != nil { + t.Fatalf("unexpected error: %v", err) + } +} + +func TestPIDStd_PI(t *testing.T) { + c, err := NewPIDStd(2, 5, 0) + if err != nil { + t.Fatal(err) + } + if math.Abs(c.Kp-2) > pidTol { + t.Errorf("Kp = %v, want 2", c.Kp) + } + if math.Abs(c.Ki-0.4) > pidTol { + t.Errorf("Ki = Kp/Ti = %v, want 0.4", c.Ki) + } + if math.Abs(c.Kd) > pidTol { + t.Errorf("Kd = %v, want 0", c.Kd) + } + sys, err := c.System() + if err != nil { + t.Fatal(err) + } + poles := pidPoles(t, sys) + assertComplexSlice(t, "poles", poles, []complex128{0}) +} + +func TestPIDStd_PIDFiltered(t *testing.T) { + c, err := NewPIDStd(1, 2, 0.5, WithFilter(0.1)) + if err != nil { + t.Fatal(err) + } + if math.Abs(c.Ki-0.5) > pidTol { + t.Errorf("Ki = Kp/Ti = %v, want 0.5", c.Ki) + } + if math.Abs(c.Kd-0.5) > pidTol { + t.Errorf("Kd = Kp*Td = %v, want 0.5", c.Kd) + } + sys, err := c.System() + if err != nil { + t.Fatal(err) + } + poles := pidPoles(t, sys) + assertComplexSlice(t, "poles", poles, []complex128{-10, 0}) +} + +func TestPIDStd_ZeroTi_Error(t *testing.T) { + _, err := NewPIDStd(1, 0, 0.5) + if err == nil { + t.Fatal("expected error for Ti=0") + } +} + +func TestPIDStd_ConvertRoundTrip(t *testing.T) { + orig := NewPID(3, 1.5, 3, WithFilter(0.2)) + std, err := orig.Standard() + if err != nil { + t.Fatal(err) + } + if math.Abs(std.Ti()-2) > pidTol { + t.Errorf("Ti = %v, want 2", std.Ti()) + } + if math.Abs(std.Td()-1) > pidTol { + t.Errorf("Td = %v, want 1", std.Td()) + } + par := std.Parallel() + if math.Abs(par.Kp-3) > pidTol || math.Abs(par.Ki-1.5) > pidTol || math.Abs(par.Kd-3) > pidTol { + t.Errorf("round-trip failed: Kp=%v Ki=%v Kd=%v", par.Kp, par.Ki, par.Kd) + } +} + +func TestPID2_MatchesPID1DOF(t *testing.T) { + c1 := NewPID(1, 2, 3, WithFilter(0.5)) + sys1, err := c1.System() + if err != nil { + t.Fatal(err) + } + + c2 := NewPID2(1, 2, 3, 0.5, 1, 1) + sys2, err := c2.System() + if err != nil { + t.Fatal(err) + } + + _, m2, _ := sys2.Dims() + if m2 != 2 { + t.Fatalf("PID2 inputs = %d, want 2", m2) + } + + freqs := []float64{0.01, 0.1, 1, 10} + for _, w := range freqs { + s := complex(0, w) + h1 := pidEvalSS(sys1, s) + h2 := pidEvalSS(sys2, s) + hR := h2[0][0] + hY := h2[0][1] + if cmplx.Abs(hR-h1[0][0]) > 1e-8 { + t.Errorf("w=%g: H_r = %v, want PID1 = %v", w, hR, h1[0][0]) + } + if cmplx.Abs(hY+h1[0][0]) > 1e-8 { + t.Errorf("w=%g: H_y = %v, want -PID1 = %v", w, hY, -h1[0][0]) + } + } +} + +func TestPID2_SetpointWeight(t *testing.T) { + c := NewPID2(2, 1, 0, 0, 0.5, 0) + sys, err := c.System() + if err != nil { + t.Fatal(err) + } + _, m, _ := sys.Dims() + if m != 2 { + t.Fatalf("inputs = %d, want 2", m) + } + + s := complex(0, 1.0) + h := pidEvalSS(sys, s) + wantR := complex(2*0.5, 0) + complex(1, 0)/s + wantY := complex(-2, 0) + complex(-1, 0)/s + if cmplx.Abs(h[0][0]-wantR) > 1e-8 { + t.Errorf("H_r at s=j = %v, want %v", h[0][0], wantR) + } + if cmplx.Abs(h[0][1]-wantY) > 1e-8 { + t.Errorf("H_y at s=j = %v, want %v", h[0][1], wantY) + } +} + +func pidEvalSS(sys *System, s complex128) [][]complex128 { + h, _ := sys.EvalFr(s) + return h +} diff --git a/pidtune.go b/pidtune.go new file mode 100644 index 0000000..7d3d132 --- /dev/null +++ b/pidtune.go @@ -0,0 +1,298 @@ +package controlsys + +import ( + "fmt" + "math" + "math/cmplx" + "strings" +) + +type PidtuneOptions struct { + CrossoverFrequency float64 + PhaseMargin float64 +} + +func Pidtune(plant *System, pidType string, opts ...PidtuneOptions) (*PID, error) { + _, m, p := plant.Dims() + if p != 1 || m != 1 { + return nil, fmt.Errorf("pidtune: SISO plant required: %w", ErrDimensionMismatch) + } + + var opt PidtuneOptions + if len(opts) > 0 { + opt = opts[0] + } + if opt.PhaseMargin == 0 { + opt.PhaseMargin = 60 + } + + pidType = strings.ToUpper(pidType) + switch pidType { + case "P", "I", "PI", "PD", "PID", "PIDF": + default: + return nil, fmt.Errorf("pidtune: unsupported type %q", pidType) + } + + wc, err := findCrossoverFreq(plant, opt.CrossoverFrequency) + if err != nil { + return nil, err + } + + pid, err := computePIDGains(plant, pidType, wc, opt.PhaseMargin) + if err != nil { + return nil, err + } + pid.Dt = plant.Dt + + return pid, nil +} + +func findCrossoverFreq(plant *System, wcTarget float64) (float64, error) { + if wcTarget > 0 { + return wcTarget, nil + } + + omega, err := marginFreqs(plant, 500) + if err != nil { + return 0, err + } + if len(omega) == 0 { + return 1.0, nil + } + + eval, err := newSISOEval(plant) + if err != nil { + return 0, err + } + + nw := len(omega) + magDB := make([]float64, nw) + for k, w := range omega { + magDB[k] = 20 * math.Log10(cmplx.Abs(eval.at(w))) + } + + crossings := findCrossings(omega, magDB, 0.0) + if len(crossings) > 0 { + c := crossings[0] + return refineCrossing(omega[c.idx], omega[c.idx+1], func(w float64) float64 { + return 20 * math.Log10(cmplx.Abs(eval.at(w))) + }), nil + } + + bw, err := Bandwidth(plant, -3) + if err == nil && bw > 0 && !math.IsInf(bw, 0) { + return bw, nil + } + + poles, _ := plant.Poles() + if len(poles) > 0 { + minW := math.Inf(1) + for _, pole := range poles { + w := cmplx.Abs(pole) + if w > 0 && w < minW { + minW = w + } + } + if !math.IsInf(minW, 1) { + return minW * 0.5, nil + } + } + + return 1.0, nil +} + +func evalPlantAt(plant *System, w float64) complex128 { + resp, err := plant.FreqResponse([]float64{w}) + if err != nil { + return 0 + } + return resp.At(0, 0, 0) +} + +func computePIDGains(plant *System, pidType string, wc, pmDeg float64) (*PID, error) { + h := evalPlantAt(plant, wc) + magP := cmplx.Abs(h) + phaseP := cmplx.Phase(h) * 180 / math.Pi + + if magP == 0 || math.IsNaN(magP) || math.IsInf(magP, 0) { + return nil, fmt.Errorf("pidtune: plant has zero or infinite gain at wc=%g", wc) + } + + phiC := -180 + pmDeg - phaseP + + for phiC > 180 { + phiC -= 360 + } + for phiC < -180 { + phiC += 360 + } + + pid := &PID{} + + switch pidType { + case "P": + pid.Kp = 1.0 / magP + + case "I": + pid.Ki = wc / magP + + case "PI": + computePI(pid, wc, magP, phiC) + + case "PD": + computePD(pid, wc, magP, phiC) + + case "PID": + computePID(pid, wc, magP, phiC) + + case "PIDF": + computePIDF(pid, wc, magP, phiC) + } + + return pid, nil +} + +func computePI(pid *PID, wc, magP, phiC float64) { + // C(jw) = Kp*(1 + 1/(Ti*jw)), angle = -atan(1/(Ti*wc)) + // Ti = -1/(wc*tan(phiC_rad)) + phiCRad := phiC * math.Pi / 180 + + // PI phase range: (-90, 0) + if phiCRad >= 0 { + phiCRad = -0.05 + } + if phiCRad <= -math.Pi/2 { + phiCRad = -math.Pi/2 + 0.05 + } + + Ti := -1.0 / (wc * math.Tan(phiCRad)) + if Ti <= 0 { + Ti = 10.0 / wc + } + + magC := math.Sqrt(1 + 1.0/(Ti*Ti*wc*wc)) + pid.Kp = 1.0 / (magP * magC) + pid.Ki = pid.Kp / Ti +} + +func computePD(pid *PID, wc, magP, phiC float64) { + // C(jw) = Kp*(1 + Td*jw), angle = atan(Td*wc) + phiCRad := phiC * math.Pi / 180 + + // PD phase range: (0, 90) + if phiCRad <= 0 { + phiCRad = 0.05 + } + if phiCRad >= math.Pi/2 { + phiCRad = math.Pi/2 - 0.05 + } + + Td := math.Tan(phiCRad) / wc + if Td <= 0 { + Td = 0.1 / wc + } + + magC := math.Sqrt(1 + Td*Td*wc*wc) + pid.Kp = 1.0 / (magP * magC) + pid.Kd = pid.Kp * Td + pid.Tf = Td / 10 + if pid.Tf <= 0 { + pid.Tf = 1.0 / (10 * wc) + } +} + +func pidFromPhase(wc, magP, phiCRad, b float64) (Kp, Ki, Kd float64) { + Kp = math.Cos(phiCRad) / magP + imPart := math.Sin(phiCRad) / magP + + Ki = Kp * wc / b + Kd = (imPart + Ki/wc) / wc + + if Kd < 0 { + Ki = Kp * wc / (b * 2) + Kd = (imPart + Ki/wc) / wc + } + if Kd < 0 { + Kd = 0 + Ki = -imPart * wc + if Ki < 0 { + Ki = Kp * wc / (b * 4) + } + } + return +} + +func computePID(pid *PID, wc, magP, phiC float64) { + phiCRad := phiC * math.Pi / 180 + if phiCRad >= math.Pi/2 { + phiCRad = math.Pi/2 - 0.05 + } + if phiCRad <= -math.Pi/2 { + phiCRad = -math.Pi/2 + 0.05 + } + + pid.Kp, pid.Ki, pid.Kd = pidFromPhase(wc, magP, phiCRad, 5.0) +} + +func computePIDF(pid *PID, wc, magP, phiC float64) { + // Iteratively design PID then compensate for filter (Tf=Td/10) phase loss. + phiCRad := phiC * math.Pi / 180 + if phiCRad >= math.Pi/2 { + phiCRad = math.Pi/2 - 0.05 + } + if phiCRad <= -math.Pi/2 { + phiCRad = -math.Pi/2 + 0.05 + } + + Kp, Ki, Kd := pidFromPhase(wc, magP, phiCRad, 5.0) + + for range 20 { + Td := 0.0 + if Kp > 0 { + Td = Kd / Kp + } + Tf := Td / 10 + if Tf <= 0 { + Tf = 1.0 / (10 * wc) + } + + jw := complex(0, wc) + C := complex(Kp, 0) + complex(Ki, 0)/jw + complex(Kd, 0)*jw/(complex(Tf, 0)*jw+1) + cMag := cmplx.Abs(C) + cPhase := cmplx.Phase(C) + + if cMag == 0 { + break + } + scale := 1.0 / (cMag * magP) + Kp *= scale + Ki *= scale + Kd *= scale + + pErr := phiCRad - cPhase + if math.Abs(pErr) < 0.001 { + break + } + + phiAdj := phiCRad + pErr + if phiAdj >= math.Pi/2 { + phiAdj = math.Pi/2 - 0.05 + } + if phiAdj <= -math.Pi/2 { + phiAdj = -math.Pi/2 + 0.05 + } + Kp, Ki, Kd = pidFromPhase(wc, magP, phiAdj, 5.0) + } + + pid.Kp = Kp + pid.Ki = Ki + pid.Kd = Kd + Td := 0.0 + if Kp > 0 { + Td = Kd / Kp + } + pid.Tf = Td / 10 + if pid.Tf <= 0 { + pid.Tf = 1.0 / (10 * wc) + } +} diff --git a/pidtune_test.go b/pidtune_test.go new file mode 100644 index 0000000..4d79b8c --- /dev/null +++ b/pidtune_test.go @@ -0,0 +1,255 @@ +package controlsys + +import ( + "math" + "testing" +) + +func pidOpenLoop(t *testing.T, plant *System, pid *PID) *System { + t.Helper() + csys, err := pid.System() + if err != nil { + t.Fatalf("PID.System: %v", err) + } + ol, err := Series(csys, plant) + if err != nil { + t.Fatalf("Series: %v", err) + } + return ol +} + +func pidClosedLoop(t *testing.T, plant *System, pid *PID) *System { + t.Helper() + csys, err := pid.System() + if err != nil { + t.Fatalf("PID.System: %v", err) + } + ol, err := Series(csys, plant) + if err != nil { + t.Fatalf("Series: %v", err) + } + cl, err := Feedback(ol, nil, -1) + if err != nil { + t.Fatalf("Feedback: %v", err) + } + return cl +} + +func assertStable(t *testing.T, sys *System, label string) { + t.Helper() + stable, err := sys.IsStable() + if err != nil { + t.Fatalf("%s: IsStable error: %v", label, err) + } + if !stable { + poles, _ := sys.Poles() + t.Fatalf("%s: closed loop is not stable, poles: %v", label, poles) + } +} + +func assertPhaseMargin(t *testing.T, ol *System, target, tol float64, label string) { + t.Helper() + mr, err := Margin(ol) + if err != nil { + t.Fatalf("%s: Margin error: %v", label, err) + } + if math.IsInf(mr.PhaseMargin, 1) { + return + } + if math.Abs(mr.PhaseMargin-target) > tol { + t.Errorf("%s: phase margin = %.1f°, want %.0f° ± %.0f°", label, mr.PhaseMargin, target, tol) + } +} + +func assertFiniteGains(t *testing.T, pid *PID, label string) { + t.Helper() + if math.IsNaN(pid.Kp) || math.IsInf(pid.Kp, 0) { + t.Fatalf("%s: Kp = %g", label, pid.Kp) + } + if math.IsNaN(pid.Ki) || math.IsInf(pid.Ki, 0) { + t.Fatalf("%s: Ki = %g", label, pid.Ki) + } + if math.IsNaN(pid.Kd) || math.IsInf(pid.Kd, 0) { + t.Fatalf("%s: Kd = %g", label, pid.Kd) + } +} + +func makePlant(t *testing.T, num, den []float64) *System { + t.Helper() + tf := &TransferFunc{ + Num: [][][]float64{{num}}, + Den: [][]float64{den}, + } + res, err := tf.StateSpace(nil) + if err != nil { + t.Fatalf("StateSpace: %v", err) + } + return res.Sys +} + +func TestPidtune_PI_FirstOrder(t *testing.T) { + plant := makePlant(t, []float64{1}, []float64{1, 1}) + pid, err := Pidtune(plant, "PI") + if err != nil { + t.Fatal(err) + } + assertFiniteGains(t, pid, "PI/1st") + cl := pidClosedLoop(t, plant, pid) + assertStable(t, cl, "PI/1st") + ol := pidOpenLoop(t, plant, pid) + assertPhaseMargin(t, ol, 60, 5, "PI/1st") +} + +func TestPidtune_PID_SecondOrder(t *testing.T) { + plant := makePlant(t, []float64{1}, []float64{1, 2, 1}) + pid, err := Pidtune(plant, "PID") + if err != nil { + t.Fatal(err) + } + assertFiniteGains(t, pid, "PID/2nd") + cl := pidClosedLoop(t, plant, pid) + assertStable(t, cl, "PID/2nd") + ol := pidOpenLoop(t, plant, pid) + assertPhaseMargin(t, ol, 60, 5, "PID/2nd") +} + +func TestPidtune_PI_Integrator(t *testing.T) { + plant := makePlant(t, []float64{1}, []float64{1, 0}) + pid, err := Pidtune(plant, "PI") + if err != nil { + t.Fatal(err) + } + assertFiniteGains(t, pid, "PI/int") + cl := pidClosedLoop(t, plant, pid) + assertStable(t, cl, "PI/int") +} + +func TestPidtune_PI_Unstable(t *testing.T) { + plant := makePlant(t, []float64{1}, []float64{1, -1}) + pid, err := Pidtune(plant, "PI") + if err != nil { + t.Fatal(err) + } + assertFiniteGains(t, pid, "PI/unstable") + cl := pidClosedLoop(t, plant, pid) + assertStable(t, cl, "PI/unstable") +} + +func TestPidtune_PI_CustomCrossover(t *testing.T) { + plant := makePlant(t, []float64{1}, []float64{1, 1}) + pid, err := Pidtune(plant, "PI", PidtuneOptions{CrossoverFrequency: 10}) + if err != nil { + t.Fatal(err) + } + assertFiniteGains(t, pid, "PI/wc10") + + ol := pidOpenLoop(t, plant, pid) + mr, err := AllMargin(ol) + if err != nil { + t.Fatal(err) + } + if len(mr.GainCrossFreqs) == 0 { + t.Fatal("no gain crossover found") + } + gotWc := mr.GainCrossFreqs[0] + if math.Abs(gotWc-10)/10 > 0.1 { + t.Errorf("crossover = %.2f, want ≈10", gotWc) + } +} + +func TestPidtune_P(t *testing.T) { + plant := makePlant(t, []float64{1}, []float64{1, 1}) + pid, err := Pidtune(plant, "P") + if err != nil { + t.Fatal(err) + } + if pid.Kp <= 0 { + t.Fatalf("Kp = %g, want > 0", pid.Kp) + } + if pid.Ki != 0 || pid.Kd != 0 { + t.Fatalf("P type should have Ki=Kd=0, got Ki=%g Kd=%g", pid.Ki, pid.Kd) + } + cl := pidClosedLoop(t, plant, pid) + assertStable(t, cl, "P") +} + +func TestPidtune_I(t *testing.T) { + plant := makePlant(t, []float64{1}, []float64{1, 1}) + pid, err := Pidtune(plant, "I") + if err != nil { + t.Fatal(err) + } + if pid.Ki <= 0 { + t.Fatalf("Ki = %g, want > 0", pid.Ki) + } + if pid.Kp != 0 || pid.Kd != 0 { + t.Fatalf("I type should have Kp=Kd=0, got Kp=%g Kd=%g", pid.Kp, pid.Kd) + } + cl := pidClosedLoop(t, plant, pid) + assertStable(t, cl, "I") +} + +func TestPidtune_PD(t *testing.T) { + plant := makePlant(t, []float64{1}, []float64{1, 2, 1}) + pid, err := Pidtune(plant, "PD") + if err != nil { + t.Fatal(err) + } + assertFiniteGains(t, pid, "PD") + if pid.Ki != 0 { + t.Fatalf("PD type should have Ki=0, got %g", pid.Ki) + } + cl := pidClosedLoop(t, plant, pid) + assertStable(t, cl, "PD") +} + +func TestPidtune_PIDF(t *testing.T) { + plant := makePlant(t, []float64{1}, []float64{1, 2, 1}) + pid, err := Pidtune(plant, "PIDF") + if err != nil { + t.Fatal(err) + } + assertFiniteGains(t, pid, "PIDF") + if pid.Tf <= 0 { + t.Fatalf("PIDF should have Tf > 0, got %g", pid.Tf) + } + cl := pidClosedLoop(t, plant, pid) + assertStable(t, cl, "PIDF") + ol := pidOpenLoop(t, plant, pid) + assertPhaseMargin(t, ol, 60, 5, "PIDF") +} + +func TestPidtune_InvalidType(t *testing.T) { + plant := makePlant(t, []float64{1}, []float64{1, 1}) + _, err := Pidtune(plant, "FOO") + if err == nil { + t.Fatal("expected error for invalid type") + } +} + +func TestPidtune_MIMO_Rejected(t *testing.T) { + tf := &TransferFunc{ + Num: [][][]float64{{{1}, {0}}, {{0}, {1}}}, + Den: [][]float64{{1, 1}, {1, 1}}, + } + res, err := tf.StateSpace(nil) + if err != nil { + t.Fatal(err) + } + _, err = Pidtune(res.Sys, "PI") + if err == nil { + t.Fatal("expected error for MIMO plant") + } +} + +func TestPidtune_CustomPM(t *testing.T) { + plant := makePlant(t, []float64{1}, []float64{1, 1}) + pid, err := Pidtune(plant, "PI", PidtuneOptions{PhaseMargin: 45}) + if err != nil { + t.Fatal(err) + } + cl := pidClosedLoop(t, plant, pid) + assertStable(t, cl, "PI/pm45") + ol := pidOpenLoop(t, plant, pid) + assertPhaseMargin(t, ol, 45, 5, "PI/pm45") +} diff --git a/prescale.go b/prescale.go new file mode 100644 index 0000000..0536e63 --- /dev/null +++ b/prescale.go @@ -0,0 +1,117 @@ +package controlsys + +import ( + "math" + + "gonum.org/v1/gonum/lapack" + "gonum.org/v1/gonum/mat" +) + +type PrescaleResult struct { + Sys *System + Info struct { + StateScale []float64 + InputScale []float64 + OutputScale []float64 + } +} + +func Prescale(sys *System) (*PrescaleResult, error) { + n, m, p := sys.Dims() + + if n == 0 { + result := &PrescaleResult{Sys: sys.Copy()} + result.Info.InputScale = ones(m) + result.Info.OutputScale = ones(p) + return result, nil + } + + aRaw := sys.A.RawMatrix() + aData := make([]float64, n*n) + copyStrided(aData, n, aRaw.Data, aRaw.Stride, n, n) + + scale := make([]float64, n) + impl.Dgebal(lapack.PermuteScale, n, aData, n, scale) + + stateScale := make([]float64, n) + copy(stateScale, scale) + + dsData := make([]float64, n) + dsInvData := make([]float64, n) + for i := range n { + dsData[i] = scale[i] + dsInvData[i] = 1.0 / scale[i] + } + Ds := mat.NewDiagDense(n, dsData) + DsInv := mat.NewDiagDense(n, dsInvData) + + Ab := mat.NewDense(n, n, aData) + + Bb := mat.NewDense(n, m, nil) + Bb.Mul(DsInv, sys.B) + + Cb := mat.NewDense(p, n, nil) + Cb.Mul(sys.C, Ds) + + Db := denseCopy(sys.D) + + inputScale := make([]float64, m) + for j := range m { + maxVal := 0.0 + for i := range n { + if v := math.Abs(Bb.At(i, j)); v > maxVal { + maxVal = v + } + } + for i := range p { + if v := math.Abs(Db.At(i, j)); v > maxVal { + maxVal = v + } + } + if maxVal > 0 { + inputScale[j] = 1.0 / maxVal + } else { + inputScale[j] = 1.0 + } + } + + outputScale := make([]float64, p) + for i := range p { + maxVal := 0.0 + for j := range n { + if v := math.Abs(Cb.At(i, j)); v > maxVal { + maxVal = v + } + } + for j := range m { + if v := math.Abs(Db.At(i, j)); v > maxVal { + maxVal = v + } + } + if maxVal > 0 { + outputScale[i] = 1.0 / maxVal + } else { + outputScale[i] = 1.0 + } + } + + scaled, err := newNoCopy(Ab, Bb, Cb, Db, sys.Dt) + if err != nil { + return nil, err + } + propagateIONames(scaled, sys) + + result := &PrescaleResult{Sys: scaled} + result.Info.StateScale = stateScale + result.Info.InputScale = inputScale + result.Info.OutputScale = outputScale + return result, nil +} + +func ones(n int) []float64 { + s := make([]float64, n) + for i := range s { + s[i] = 1.0 + } + return s +} diff --git a/prescale_test.go b/prescale_test.go new file mode 100644 index 0000000..ebb2656 --- /dev/null +++ b/prescale_test.go @@ -0,0 +1,194 @@ +package controlsys + +import ( + "math" + "math/cmplx" + "sort" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestPrescale_Simple(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + + pr, err := Prescale(sys) + if err != nil { + t.Fatal(err) + } + + checkPrescalePolesPreserved(t, sys, pr.Sys, 1e-10) + checkPrescaleBodePreserved(t, sys, pr.Sys, 1e-6) + checkPrescaleDCGainPreserved(t, sys, pr.Sys, 1e-10) + + for i, s := range pr.Info.StateScale { + if math.Abs(s-1.0) > 1e-10 { + t.Errorf("StateScale[%d] = %g, want ~1", i, s) + } + } +} + +func TestPrescale_LargeGainSpread(t *testing.T) { + sys, _ := New( + mat.NewDense(2, 2, []float64{-1, 0.5, 0, -1000}), + mat.NewDense(2, 1, []float64{1, 1000}), + mat.NewDense(1, 2, []float64{1000, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + + pr, err := Prescale(sys) + if err != nil { + t.Fatal(err) + } + + checkPrescalePolesPreserved(t, sys, pr.Sys, 1e-10) + checkPrescaleDCGainPreserved(t, sys, pr.Sys, 1e-6) + checkPrescaleBodePreserved(t, sys, pr.Sys, 1e-6) + + if len(pr.Info.InputScale) != 1 { + t.Fatalf("InputScale length = %d, want 1", len(pr.Info.InputScale)) + } + if len(pr.Info.OutputScale) != 1 { + t.Fatalf("OutputScale length = %d, want 1", len(pr.Info.OutputScale)) + } +} + +func TestPrescale_MIMO(t *testing.T) { + sys, _ := New( + mat.NewDense(2, 2, []float64{-1, 100, 0, -2}), + mat.NewDense(2, 2, []float64{1, 0, 0, 100}), + mat.NewDense(2, 2, []float64{100, 0, 0, 1}), + mat.NewDense(2, 2, []float64{0, 0, 0, 0}), 0) + + pr, err := Prescale(sys) + if err != nil { + t.Fatal(err) + } + + checkPrescalePolesPreserved(t, sys, pr.Sys, 1e-10) + checkPrescaleDCGainPreserved(t, sys, pr.Sys, 1e-6) +} + +func TestPrescale_PureGain(t *testing.T) { + sys, _ := NewGain(mat.NewDense(1, 1, []float64{5}), 0) + + pr, err := Prescale(sys) + if err != nil { + t.Fatal(err) + } + + n, _, _ := pr.Sys.Dims() + if n != 0 { + t.Errorf("expected pure gain, got n=%d", n) + } +} + +func TestPrescale_Discrete(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{0.5}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0.1) + + pr, err := Prescale(sys) + if err != nil { + t.Fatal(err) + } + + checkPrescalePolesPreserved(t, sys, pr.Sys, 1e-10) + checkPrescaleDCGainPreserved(t, sys, pr.Sys, 1e-10) +} + +func checkPrescalePolesPreserved(t *testing.T, orig, scaled *System, tol float64) { + t.Helper() + p1, err := orig.Poles() + if err != nil { + t.Fatalf("orig poles: %v", err) + } + p2, err := scaled.Poles() + if err != nil { + t.Fatalf("scaled poles: %v", err) + } + if len(p1) != len(p2) { + t.Fatalf("pole count %d != %d", len(p1), len(p2)) + } + sort.Slice(p1, func(i, j int) bool { + if real(p1[i]) != real(p1[j]) { + return real(p1[i]) < real(p1[j]) + } + return imag(p1[i]) < imag(p1[j]) + }) + sort.Slice(p2, func(i, j int) bool { + if real(p2[i]) != real(p2[j]) { + return real(p2[i]) < real(p2[j]) + } + return imag(p2[i]) < imag(p2[j]) + }) + for i := range p1 { + if cmplx.Abs(p1[i]-p2[i]) > tol { + t.Errorf("pole[%d]: %v != %v", i, p1[i], p2[i]) + } + } +} + +func checkPrescaleBodePreserved(t *testing.T, orig, scaled *System, tol float64) { + t.Helper() + freqs := []float64{0.01, 0.1, 1, 10, 100} + for _, w := range freqs { + var s complex128 + if orig.IsContinuous() { + s = complex(0, w) + } else { + s = cmplx.Exp(complex(0, w*orig.Dt)) + } + g1, err1 := orig.EvalFr(s) + g2, err2 := scaled.EvalFr(s) + if err1 != nil || err2 != nil { + t.Errorf("EvalFr error at w=%g", w) + continue + } + _, m1, p1 := orig.Dims() + for i := range p1 { + for j := range m1 { + diff := cmplx.Abs(g1[i][j] - g2[i][j]) + mag := cmplx.Abs(g1[i][j]) + relTol := tol + if mag > 1e-10 { + relTol = tol * mag + } + if diff > relTol { + t.Errorf("w=%g: G[%d,%d] diff=%g > tol=%g", w, i, j, diff, relTol) + } + } + } + } +} + +func checkPrescaleDCGainPreserved(t *testing.T, orig, scaled *System, tol float64) { + t.Helper() + dc1, err := orig.DCGain() + if err != nil { + t.Fatalf("orig DCGain: %v", err) + } + dc2, err := scaled.DCGain() + if err != nil { + t.Fatalf("scaled DCGain: %v", err) + } + r, c := dc1.Dims() + for i := range r { + for j := range c { + diff := math.Abs(dc1.At(i, j) - dc2.At(i, j)) + mag := math.Abs(dc1.At(i, j)) + relTol := tol + if mag > 1e-10 { + relTol = tol * mag + } + if diff > relTol { + t.Errorf("DCGain[%d,%d]: %g != %g (diff=%g)", i, j, dc2.At(i, j), dc1.At(i, j), diff) + } + } + } +} diff --git a/pzmap.go b/pzmap.go new file mode 100644 index 0000000..ccf5f95 --- /dev/null +++ b/pzmap.go @@ -0,0 +1,18 @@ +package controlsys + +type PzmapResult struct { + Poles []complex128 + Zeros []complex128 +} + +func Pzmap(sys *System) (*PzmapResult, error) { + poles, err := sys.Poles() + if err != nil { + return nil, err + } + zeros, err := sys.Zeros() + if err != nil { + return nil, err + } + return &PzmapResult{Poles: poles, Zeros: zeros}, nil +} diff --git a/random.go b/random.go new file mode 100644 index 0000000..49f6d3b --- /dev/null +++ b/random.go @@ -0,0 +1,110 @@ +package controlsys + +import ( + "fmt" + "math" + "math/rand" + + "gonum.org/v1/gonum/mat" +) + +func Rss(n, p, m int) (*System, error) { + if n < 0 || p < 1 || m < 1 { + return nil, fmt.Errorf("controlsys: invalid dimensions n=%d p=%d m=%d", n, p, m) + } + return randomSS(n, p, m, 0, true) +} + +func Drss(n, p, m int, dt float64) (*System, error) { + if n < 0 || p < 1 || m < 1 { + return nil, fmt.Errorf("controlsys: invalid dimensions n=%d p=%d m=%d", n, p, m) + } + if dt <= 0 { + return nil, ErrInvalidSampleTime + } + return randomSS(n, p, m, dt, false) +} + +func randomSS(n, p, m int, dt float64, continuous bool) (*System, error) { + if n == 0 { + dData := make([]float64, p*m) + for i := range dData { + dData[i] = rand.NormFloat64() + } + return NewGain(mat.NewDense(p, m, dData), dt) + } + + A := randomStableA(n, continuous) + + bData := make([]float64, n*m) + for i := range bData { + bData[i] = rand.NormFloat64() + } + B := mat.NewDense(n, m, bData) + + cData := make([]float64, p*n) + for i := range cData { + cData[i] = rand.NormFloat64() + } + C := mat.NewDense(p, n, cData) + + D := mat.NewDense(p, m, nil) + + return newNoCopy(A, B, C, D, dt) +} + +func randomStableA(n int, continuous bool) *mat.Dense { + // Generate random eigenvalue structure with mix of real and complex pairs + diagData := make([]float64, n*n) + i := 0 + for i < n { + if i+1 < n && rand.Float64() < 0.5 { + if continuous { + sigma := -rand.Float64()*4 - 0.1 + omega := rand.Float64()*4 + 0.1 + diagData[i*n+i] = sigma + diagData[i*n+i+1] = omega + diagData[(i+1)*n+i] = -omega + diagData[(i+1)*n+i+1] = sigma + } else { + r := rand.Float64()*0.9 + 0.01 + theta := rand.Float64() * math.Pi + diagData[i*n+i] = r * math.Cos(theta) + diagData[i*n+i+1] = r * math.Sin(theta) + diagData[(i+1)*n+i] = -r * math.Sin(theta) + diagData[(i+1)*n+i+1] = r * math.Cos(theta) + } + i += 2 + } else { + if continuous { + diagData[i*n+i] = -rand.Float64()*4 - 0.1 + } else { + diagData[i*n+i] = rand.Float64()*1.8 - 0.9 + } + i++ + } + } + D := mat.NewDense(n, n, diagData) + + Q := randomOrthogonal(n) + + // A = Q * D * Q' + tmp := mat.NewDense(n, n, nil) + tmp.Mul(Q, D) + A := mat.NewDense(n, n, nil) + A.Mul(tmp, Q.T()) + return A +} + +func randomOrthogonal(n int) *mat.Dense { + data := make([]float64, n*n) + for i := range data { + data[i] = rand.NormFloat64() + } + M := mat.NewDense(n, n, data) + var qr mat.QR + qr.Factorize(M) + Q := mat.NewDense(n, n, nil) + qr.QTo(Q) + return Q +} diff --git a/random_test.go b/random_test.go new file mode 100644 index 0000000..189b0ce --- /dev/null +++ b/random_test.go @@ -0,0 +1,138 @@ +package controlsys + +import ( + "math/cmplx" + "testing" +) + +func TestRss_Basic(t *testing.T) { + sys, err := Rss(5, 2, 3) + if err != nil { + t.Fatal(err) + } + + n, m, p := sys.Dims() + if n != 5 || m != 3 || p != 2 { + t.Fatalf("dims = (%d,%d,%d), want (5,3,2)", n, m, p) + } + + if !sys.IsContinuous() { + t.Error("expected continuous system") + } + + stable, err := sys.IsStable() + if err != nil { + t.Fatal(err) + } + if !stable { + poles, _ := sys.Poles() + t.Errorf("system not stable, poles = %v", poles) + } +} + +func TestRss_StrictStability(t *testing.T) { + for i := 0; i < 20; i++ { + sys, err := Rss(8, 1, 1) + if err != nil { + t.Fatal(err) + } + poles, _ := sys.Poles() + for j, p := range poles { + if real(p) >= 0 { + t.Errorf("trial %d: pole[%d] = %v has non-negative real part", i, j, p) + } + } + } +} + +func TestRss_ZeroStates(t *testing.T) { + sys, err := Rss(0, 2, 3) + if err != nil { + t.Fatal(err) + } + + n, m, p := sys.Dims() + if n != 0 || m != 3 || p != 2 { + t.Fatalf("dims = (%d,%d,%d), want (0,3,2)", n, m, p) + } +} + +func TestDrss_Basic(t *testing.T) { + sys, err := Drss(4, 1, 1, 0.1) + if err != nil { + t.Fatal(err) + } + + n, m, p := sys.Dims() + if n != 4 || m != 1 || p != 1 { + t.Fatalf("dims = (%d,%d,%d), want (4,1,1)", n, m, p) + } + + if !sys.IsDiscrete() || sys.Dt != 0.1 { + t.Errorf("expected discrete with dt=0.1, got dt=%g", sys.Dt) + } + + stable, err := sys.IsStable() + if err != nil { + t.Fatal(err) + } + if !stable { + t.Error("discrete system not stable") + } +} + +func TestDrss_StrictStability(t *testing.T) { + for i := 0; i < 20; i++ { + sys, err := Drss(6, 2, 2, 0.05) + if err != nil { + t.Fatal(err) + } + poles, _ := sys.Poles() + for j, p := range poles { + if cmplx.Abs(p) >= 1 { + t.Errorf("trial %d: pole[%d] = %v has |p| >= 1", i, j, p) + } + } + } +} + +func TestDrss_ZeroStates(t *testing.T) { + sys, err := Drss(0, 1, 1, 0.5) + if err != nil { + t.Fatal(err) + } + + n, _, _ := sys.Dims() + if n != 0 { + t.Fatalf("n = %d, want 0", n) + } +} + +func TestDrss_InvalidDt(t *testing.T) { + _, err := Drss(4, 1, 1, 0) + if err == nil { + t.Error("expected error for dt=0") + } + + _, err = Drss(4, 1, 1, -1) + if err == nil { + t.Error("expected error for dt<0") + } +} + +func TestRss_InvalidDims(t *testing.T) { + _, err := Rss(-1, 1, 1) + if err == nil { + t.Error("expected error for n<0") + } + + _, err = Rss(1, 0, 1) + if err == nil { + t.Error("expected error for p=0") + } + + _, err = Rss(1, 1, 0) + if err == nil { + t.Error("expected error for m=0") + } +} diff --git a/response.go b/response.go index 9f15f82..ff1d1c7 100644 --- a/response.go +++ b/response.go @@ -293,3 +293,46 @@ func Initial(sys *System, x0 *mat.VecDense, tFinal float64) (*TimeResponse, erro return &TimeResponse{T: makeTimeVector(steps, dt), Y: resp.Y, OutputName: copyStringSlice(sys.OutputName)}, nil } + +func Lsim(sys *System, u *mat.Dense, t []float64, x0 *mat.VecDense) (*TimeResponse, error) { + if len(t) < 2 { + return nil, fmt.Errorf("Lsim: need at least 2 time points: %w", ErrDimensionMismatch) + } + + _, m, _ := sys.Dims() + ur, uc := u.Dims() + if ur != len(t) || uc != m { + return nil, fmt.Errorf("Lsim: u must be %d×%d, got %d×%d: %w", len(t), m, ur, uc, ErrDimensionMismatch) + } + + steps := len(t) + dt := t[1] - t[0] + + var dsys *System + var err error + if sys.IsContinuous() { + dsys, err = sys.DiscretizeZOH(dt) + if err != nil { + return nil, fmt.Errorf("Lsim: %w", err) + } + } else { + dsys = sys + } + + // Transpose u from len(t)×m to m×len(t) for Simulate + uSim := mat.NewDense(m, steps, nil) + uRaw := u.RawMatrix() + uSimRaw := uSim.RawMatrix() + for i := 0; i < steps; i++ { + for j := 0; j < m; j++ { + uSimRaw.Data[j*uSimRaw.Stride+i] = uRaw.Data[i*uRaw.Stride+j] + } + } + + resp, err := dsys.Simulate(uSim, x0, nil) + if err != nil { + return nil, fmt.Errorf("Lsim: %w", err) + } + + return &TimeResponse{T: t, Y: resp.Y, OutputName: copyStringSlice(sys.OutputName)}, nil +} diff --git a/sminreal.go b/sminreal.go new file mode 100644 index 0000000..1a9a7f3 --- /dev/null +++ b/sminreal.go @@ -0,0 +1,113 @@ +package controlsys + +import "gonum.org/v1/gonum/mat" + +func Sminreal(sys *System) (*System, error) { + n, m, p := sys.Dims() + if n == 0 { + return sys.Copy(), nil + } + + aRaw := sys.A.RawMatrix() + bRaw := sys.B.RawMatrix() + cRaw := sys.C.RawMatrix() + + reachable := make([]bool, n) + observable := make([]bool, n) + + for i := range n { + for j := range m { + if bRaw.Data[i*bRaw.Stride+j] != 0 { + reachable[i] = true + break + } + } + } + + for i := range n { + for j := range p { + if cRaw.Data[j*cRaw.Stride+i] != 0 { + observable[i] = true + break + } + } + } + + changed := true + for changed { + changed = false + for i := range n { + if reachable[i] { + continue + } + for j := range n { + if reachable[j] && aRaw.Data[i*aRaw.Stride+j] != 0 { + reachable[i] = true + changed = true + break + } + } + } + } + + changed = true + for changed { + changed = false + for i := range n { + if observable[i] { + continue + } + for j := range n { + if observable[j] && aRaw.Data[j*aRaw.Stride+i] != 0 { + observable[i] = true + changed = true + break + } + } + } + } + + var keep []int + for i := range n { + if reachable[i] && observable[i] { + keep = append(keep, i) + } + } + + if len(keep) == n { + return sys.Copy(), nil + } + + r := len(keep) + if r == 0 { + g, err := NewGain(denseCopy(sys.D), sys.Dt) + if err != nil { + return nil, err + } + propagateIONames(g, sys) + return g, nil + } + + ar := mat.NewDense(r, r, nil) + br := mat.NewDense(r, m, nil) + cr := mat.NewDense(p, r, nil) + + for ii, i := range keep { + for jj, j := range keep { + ar.Set(ii, jj, aRaw.Data[i*aRaw.Stride+j]) + } + for j := range m { + br.Set(ii, j, bRaw.Data[i*bRaw.Stride+j]) + } + for j := range p { + cr.Set(j, ii, cRaw.Data[j*cRaw.Stride+i]) + } + } + + result, err := newNoCopy(ar, br, cr, denseCopy(sys.D), sys.Dt) + if err != nil { + return nil, err + } + propagateIONames(result, sys) + return result, nil +} diff --git a/sminreal_test.go b/sminreal_test.go new file mode 100644 index 0000000..25626b5 --- /dev/null +++ b/sminreal_test.go @@ -0,0 +1,186 @@ +package controlsys + +import ( + "math" + "math/cmplx" + "sort" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestSminreal_DecoupledState(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{-1, 0, 0, 0, -2, 0, 0, 0, -3}), + mat.NewDense(3, 1, []float64{1, 0, 1}), + mat.NewDense(1, 3, []float64{1, 0, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + red, err := Sminreal(sys) + if err != nil { + t.Fatal(err) + } + + n, _, _ := red.Dims() + if n != 2 { + t.Fatalf("order = %d, want 2", n) + } + + poles, _ := red.Poles() + sortPolesDecomp(poles) + wantPoles := []complex128{-3, -1} + for i, p := range poles { + if cmplx.Abs(p-wantPoles[i]) > 1e-10 { + t.Errorf("pole[%d] = %v, want %v", i, p, wantPoles[i]) + } + } + + dc, _ := red.DCGain() + want := 1.0 + 1.0/3.0 + if math.Abs(dc.At(0, 0)-want) > 1e-10 { + t.Errorf("dcgain = %g, want %g", dc.At(0, 0), want) + } +} + +func TestSminreal_CoupledThroughA(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{-1, 1, 0, 0, -2, 0, 0, 0, -3}), + mat.NewDense(3, 1, []float64{0, 1, 1}), + mat.NewDense(1, 3, []float64{1, 0, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + red, err := Sminreal(sys) + if err != nil { + t.Fatal(err) + } + + n, _, _ := red.Dims() + if n != 3 { + t.Errorf("order = %d, want 3 (no reduction)", n) + } +} + +func TestSminreal_UncontrollableUncoupled(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{-1, 0, 0, 0, -2, 0, 0, 0, -3}), + mat.NewDense(3, 1, []float64{1, 1, 0}), + mat.NewDense(1, 3, []float64{1, 1, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + red, err := Sminreal(sys) + if err != nil { + t.Fatal(err) + } + + n, _, _ := red.Dims() + if n != 2 { + t.Fatalf("order = %d, want 2", n) + } + + poles, _ := red.Poles() + sortPolesDecomp(poles) + wantPoles := []complex128{-2, -1} + for i, p := range poles { + if cmplx.Abs(p-wantPoles[i]) > 1e-10 { + t.Errorf("pole[%d] = %v, want %v", i, p, wantPoles[i]) + } + } +} + +func TestSminreal_NoReduction(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -2, -3}), + mat.NewDense(2, 1, []float64{0, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + red, err := Sminreal(sys) + if err != nil { + t.Fatal(err) + } + + n, _, _ := red.Dims() + if n != 2 { + t.Errorf("order = %d, want 2", n) + } +} + +func TestSminreal_Empty(t *testing.T) { + sys, _ := NewGain(mat.NewDense(1, 1, []float64{5}), 0) + + red, err := Sminreal(sys) + if err != nil { + t.Fatal(err) + } + + n, _, _ := red.Dims() + if n != 0 { + t.Errorf("order = %d, want 0", n) + } +} + +func TestSminreal_AllRemoved(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{-1, 0, 0, -2}), + mat.NewDense(2, 1, []float64{0, 0}), + mat.NewDense(1, 2, []float64{1, 1}), + mat.NewDense(1, 1, []float64{3}), 0) + if err != nil { + t.Fatal(err) + } + + red, err := Sminreal(sys) + if err != nil { + t.Fatal(err) + } + + n, _, _ := red.Dims() + if n != 0 { + t.Errorf("order = %d, want 0", n) + } + if red.D.At(0, 0) != 3 { + t.Errorf("D = %g, want 3", red.D.At(0, 0)) + } +} + +func TestSminreal_MIMO(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{-1, 0, 0, 0, -2, 0, 0, 0, -3}), + mat.NewDense(3, 2, []float64{1, 0, 0, 0, 0, 1}), + mat.NewDense(2, 3, []float64{1, 0, 0, 0, 0, 1}), + mat.NewDense(2, 2, []float64{0, 0, 0, 0}), 0) + if err != nil { + t.Fatal(err) + } + + red, err := Sminreal(sys) + if err != nil { + t.Fatal(err) + } + + n, _, _ := red.Dims() + if n != 2 { + t.Errorf("order = %d, want 2 (state 1 removed)", n) + } +} + +func sortPolesDecomp(poles []complex128) { + sort.Slice(poles, func(i, j int) bool { + if real(poles[i]) != real(poles[j]) { + return real(poles[i]) < real(poles[j]) + } + return imag(poles[i]) < imag(poles[j]) + }) +} diff --git a/ssbal.go b/ssbal.go new file mode 100644 index 0000000..f1ffb3f --- /dev/null +++ b/ssbal.go @@ -0,0 +1,116 @@ +package controlsys + +import ( + "math" + + "gonum.org/v1/gonum/mat" +) + +type SsbalResult struct { + Sys *System + T *mat.Dense +} + +func Ssbal(sys *System) (*SsbalResult, error) { + n, m, p := sys.Dims() + if n == 0 { + cp := sys.Copy() + eye := &mat.Dense{} + return &SsbalResult{Sys: cp, T: eye}, nil + } + + aRaw := sys.A.RawMatrix() + bRaw := sys.B.RawMatrix() + cRaw := sys.C.RawMatrix() + + d := make([]float64, n) + for i := range d { + d[i] = 1.0 + } + + // Iterative diagonal balancing + const maxIter = 20 + const beta = 2.0 + for iter := 0; iter < maxIter; iter++ { + changed := false + for i := 0; i < n; i++ { + rowNorm := 0.0 + colNorm := 0.0 + + for j := 0; j < n; j++ { + rowNorm += math.Abs(aRaw.Data[i*aRaw.Stride+j]) * d[j] + colNorm += math.Abs(aRaw.Data[j*aRaw.Stride+i]) * d[j] + } + + for j := 0; j < m; j++ { + rowNorm += math.Abs(bRaw.Data[i*bRaw.Stride+j]) + } + + for j := 0; j < p; j++ { + colNorm += math.Abs(cRaw.Data[j*cRaw.Stride+i]) + } + + if rowNorm == 0 || colNorm == 0 { + continue + } + + f := 1.0 + s := rowNorm + colNorm + // Scale by powers of beta to find best balance + for rowNorm < colNorm/beta { + rowNorm *= beta + colNorm /= beta + f *= beta + } + for rowNorm > colNorm*beta { + rowNorm /= beta + colNorm *= beta + f /= beta + } + + if rowNorm+colNorm < 0.95*s { + changed = true + d[i] *= f + } + } + if !changed { + break + } + } + + Anew := mat.NewDense(n, n, nil) + for i := 0; i < n; i++ { + for j := 0; j < n; j++ { + Anew.Set(i, j, (d[i]/d[j])*aRaw.Data[i*aRaw.Stride+j]) + } + } + + Bnew := mat.NewDense(n, m, nil) + for i := 0; i < n; i++ { + for j := 0; j < m; j++ { + Bnew.Set(i, j, d[i]*bRaw.Data[i*bRaw.Stride+j]) + } + } + + Cnew := mat.NewDense(p, n, nil) + for i := 0; i < p; i++ { + for j := 0; j < n; j++ { + Cnew.Set(i, j, cRaw.Data[i*cRaw.Stride+j]/d[j]) + } + } + + Dnew := denseCopy(sys.D) + + T := mat.NewDense(n, n, nil) + for i := 0; i < n; i++ { + T.Set(i, i, d[i]) + } + + newSys, err := newNoCopy(Anew, Bnew, Cnew, Dnew, sys.Dt) + if err != nil { + return nil, err + } + propagateIONames(newSys, sys) + + return &SsbalResult{Sys: newSys, T: T}, nil +} diff --git a/ssbal_test.go b/ssbal_test.go new file mode 100644 index 0000000..4513bf6 --- /dev/null +++ b/ssbal_test.go @@ -0,0 +1,116 @@ +package controlsys + +import ( + "math" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestSsbal_PoorlyConditioned(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{1000, 1, 0, 0.001}), + mat.NewDense(2, 1, []float64{1000, 0.001}), + mat.NewDense(1, 2, []float64{1, 1000}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Ssbal(sys) + if err != nil { + t.Fatal(err) + } + + checkEigsPreserved(t, sys, res.Sys, 1e-8) + + for _, w := range []float64{0.01, 0.1, 1, 10} { + g1, _ := sys.EvalFr(complex(0, w)) + g2, _ := res.Sys.EvalFr(complex(0, w)) + diff := math.Abs(real(g1[0][0]) - real(g2[0][0])) + relDiff := diff + if math.Abs(real(g1[0][0])) > 1e-10 { + relDiff = diff / math.Abs(real(g1[0][0])) + } + if relDiff > 1e-6 { + t.Errorf("w=%g: freq response mismatch rel=%g", w, relDiff) + } + } +} + +func TestSsbal_AlreadyBalanced(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{-1, 0, 0, -2}), + mat.NewDense(2, 1, []float64{1, 1}), + mat.NewDense(1, 2, []float64{1, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Ssbal(sys) + if err != nil { + t.Fatal(err) + } + + n, _, _ := sys.Dims() + for i := 0; i < n; i++ { + d := res.T.At(i, i) + if math.Abs(d-1) > 1 { + t.Errorf("T[%d,%d] = %g, expected near 1 for already balanced system", i, i, d) + } + } + + checkEigsPreserved(t, sys, res.Sys, 1e-10) +} + +func TestSsbal_Empty(t *testing.T) { + sys, err := New(nil, nil, nil, mat.NewDense(1, 1, []float64{3}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Ssbal(sys) + if err != nil { + t.Fatal(err) + } + + dc, _ := res.Sys.DCGain() + if dc.At(0, 0) != 3 { + t.Errorf("dcgain = %g, want 3", dc.At(0, 0)) + } +} + +func TestSsbal_TransformIsDiagonal(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{ + -1, 100, 0, + 0.01, -2, 50, + 0, 0.02, -3, + }), + mat.NewDense(3, 1, []float64{100, 1, 0.01}), + mat.NewDense(1, 3, []float64{0.01, 1, 100}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Ssbal(sys) + if err != nil { + t.Fatal(err) + } + + n, _, _ := sys.Dims() + for i := 0; i < n; i++ { + for j := 0; j < n; j++ { + if i != j && res.T.At(i, j) != 0 { + t.Errorf("T[%d,%d] = %g, want 0 (T should be diagonal)", i, j, res.T.At(i, j)) + } + } + if res.T.At(i, i) <= 0 { + t.Errorf("T[%d,%d] = %g, want positive", i, i, res.T.At(i, i)) + } + } + + checkEigsPreserved(t, sys, res.Sys, 1e-8) +} diff --git a/stabsep.go b/stabsep.go new file mode 100644 index 0000000..4e4e7ca --- /dev/null +++ b/stabsep.go @@ -0,0 +1,24 @@ +package controlsys + +import "math/cmplx" + +type StabsepResult struct { + Stable *System + Unstable *System +} + +func Stabsep(sys *System) (*StabsepResult, error) { + isStable := func(ev complex128) bool { + if sys.IsContinuous() { + return real(ev) < 0 + } + return cmplx.Abs(ev) < 1 + } + + stable, unstable, err := decomposeByEigenvalues(sys, isStable) + if err != nil { + return nil, err + } + + return &StabsepResult{Stable: stable, Unstable: unstable}, nil +} diff --git a/stabsep_test.go b/stabsep_test.go new file mode 100644 index 0000000..ccc8641 --- /dev/null +++ b/stabsep_test.go @@ -0,0 +1,323 @@ +package controlsys + +import ( + "math" + "math/cmplx" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestStabsep_Diagonal(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{-1, 0, 0, 0, 2, 0, 0, 0, -3}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, nil), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Stabsep(sys) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Stable.Dims() + nu, _, _ := res.Unstable.Dims() + + if ns != 2 { + t.Errorf("stable order = %d, want 2", ns) + } + if nu != 1 { + t.Errorf("unstable order = %d, want 1", nu) + } + + stablePoles, _ := res.Stable.Poles() + for _, p := range stablePoles { + if real(p) >= 0 { + t.Errorf("stable pole %v has Re >= 0", p) + } + } + + unstablePoles, _ := res.Unstable.Poles() + for _, p := range unstablePoles { + if real(p) < 0 { + t.Errorf("unstable pole %v has Re < 0", p) + } + } + + checkAdditiveDecomposition(t, sys, res.Stable, res.Unstable) +} + +func TestStabsep_FullyStable(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{-1, 0, 0, -2}), + mat.NewDense(2, 1, []float64{1, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Stabsep(sys) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Stable.Dims() + nu, _, _ := res.Unstable.Dims() + + if ns != 2 { + t.Errorf("stable order = %d, want 2", ns) + } + if nu != 0 { + t.Errorf("unstable order = %d, want 0", nu) + } +} + +func TestStabsep_FullyUnstable(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{1, 0, 0, 2}), + mat.NewDense(2, 1, []float64{1, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Stabsep(sys) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Stable.Dims() + nu, _, _ := res.Unstable.Dims() + + if ns != 0 { + t.Errorf("stable order = %d, want 0", ns) + } + if nu != 2 { + t.Errorf("unstable order = %d, want 2", nu) + } +} + +func TestStabsep_ComplexEigenvalues(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{0, 1, 0, -1, 0, 0, 0, 0, 2}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, nil), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Stabsep(sys) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Stable.Dims() + nu, _, _ := res.Unstable.Dims() + + // ±j are marginal (Re=0), treated as unstable; pole 2 is unstable + if ns != 0 { + t.Errorf("stable order = %d, want 0", ns) + } + if nu != 3 { + t.Errorf("unstable order = %d, want 3", nu) + } +} + +func TestStabsep_Discrete(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{0.5, 0, 0, 0, 1.5, 0, 0, 0, 0.9}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, nil), 0.1) + if err != nil { + t.Fatal(err) + } + + res, err := Stabsep(sys) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Stable.Dims() + nu, _, _ := res.Unstable.Dims() + + if ns != 2 { + t.Errorf("stable order = %d, want 2", ns) + } + if nu != 1 { + t.Errorf("unstable order = %d, want 1", nu) + } + + checkAdditiveDecomposition(t, sys, res.Stable, res.Unstable) +} + +func TestStabsep_NonDiagonalStable(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{ + -1, 0.5, 0, + 0, -2, 0, + 0, 0, 3, + }), + mat.NewDense(3, 1, []float64{1, 1, 1}), + mat.NewDense(1, 3, []float64{1, 1, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Stabsep(sys) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Stable.Dims() + nu, _, _ := res.Unstable.Dims() + + if ns != 2 { + t.Errorf("stable order = %d, want 2", ns) + } + if nu != 1 { + t.Errorf("unstable order = %d, want 1", nu) + } + + checkAdditiveDecomposition(t, sys, res.Stable, res.Unstable) +} + +func TestStabsep_Empty(t *testing.T) { + sys, _ := NewGain(mat.NewDense(1, 1, []float64{5}), 0) + + res, err := Stabsep(sys) + if err != nil { + t.Fatal(err) + } + + ns, _, _ := res.Stable.Dims() + if ns != 0 { + t.Errorf("stable order = %d, want 0", ns) + } +} + +func TestStabsep_EigenvaluePreservation(t *testing.T) { + sys, err := New( + mat.NewDense(4, 4, []float64{ + -1, 0.5, 0, 0, + 0, -2, 0, 0, + 0, 0, 1, 0.3, + 0, 0, 0, 3, + }), + mat.NewDense(4, 4, []float64{1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}), + mat.NewDense(4, 4, []float64{1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}), + mat.NewDense(4, 4, nil), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Stabsep(sys) + if err != nil { + t.Fatal(err) + } + + origPoles, _ := sys.Poles() + stablePoles, _ := res.Stable.Poles() + unstablePoles, _ := res.Unstable.Poles() + + allDecomp := append(stablePoles, unstablePoles...) + sortPoles(origPoles) + sortPoles(allDecomp) + + if len(origPoles) != len(allDecomp) { + t.Fatalf("pole count: %d vs %d", len(origPoles), len(allDecomp)) + } + for i := range origPoles { + if cmplx.Abs(origPoles[i]-allDecomp[i]) > 1e-10 { + t.Errorf("pole[%d]: %v != %v", i, origPoles[i], allDecomp[i]) + } + } +} + +func checkAdditiveDecomposition(t *testing.T, sys, sys1, sys2 *System) { + t.Helper() + _, m, p := sys.Dims() + + freqs := []float64{0.01, 0.1, 1, 10, 100} + if sys.IsDiscrete() { + freqs = []float64{0.01, 0.1, 0.5, 1, 2} + } + + for _, w := range freqs { + var s complex128 + if sys.IsContinuous() { + s = complex(0, w) + } else { + s = complex(0, w*sys.Dt) + } + + g, err := sys.EvalFr(s) + if err != nil { + continue + } + g1, err := sys1.EvalFr(s) + if err != nil { + continue + } + g2, err := sys2.EvalFr(s) + if err != nil { + continue + } + + for i := range p { + for j := range m { + sum := g1[i][j] + g2[i][j] + diff := cmplx.Abs(sum - g[i][j]) + mag := cmplx.Abs(g[i][j]) + tol := 1e-8 + if mag > 1 { + tol = 1e-8 * mag + } + if diff > tol && mag > 1e-12 { + t.Errorf("w=%g: G[%d,%d] sum=%v, orig=%v, diff=%g", w, i, j, sum, g[i][j], diff) + } + } + } + } +} + +func TestStabsep_AdditiveDecompositionDCGain(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{-1, 0, 0, 0, 2, 0, 0, 0, -3}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}), + mat.NewDense(3, 3, nil), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Stabsep(sys) + if err != nil { + t.Fatal(err) + } + + stableDC, err := res.Stable.DCGain() + if err != nil { + t.Fatal(err) + } + + // Stable part DC gain: diag(-1/(−1), -1/(-3)) in transformed coords + // The trace of stable DC should be sum of -1/pole for stable poles + traceDC := 0.0 + ns, _, _ := res.Stable.Dims() + for i := range ns { + if i < 3 { + traceDC += stableDC.At(i, i) + } + } + if math.IsInf(traceDC, 0) || math.IsNaN(traceDC) { + t.Errorf("stable DC gain has inf/nan") + } +} diff --git a/transfer.go b/transfer.go index 3d31dea..57beef2 100644 --- a/transfer.go +++ b/transfer.go @@ -624,3 +624,21 @@ func (tf *TransferFunc) StateSpace(opts *StateSpaceOpts) (*StateSpaceResult, err BlockSizes: degrees, }, nil } + +func (sys *System) Isproper() bool { + return true +} + +func (tf *TransferFunc) Isproper() bool { + p, m := tf.Dims() + for i := 0; i < p; i++ { + denDeg := len(tf.Den[i]) - 1 + for j := 0; j < m; j++ { + numDeg := len(tf.Num[i][j]) - 1 + if numDeg > denDeg { + return false + } + } + } + return true +} diff --git a/transform.go b/transform.go new file mode 100644 index 0000000..88d95d0 --- /dev/null +++ b/transform.go @@ -0,0 +1,75 @@ +package controlsys + +import ( + "fmt" + + "gonum.org/v1/gonum/mat" +) + +func SS2SS(sys *System, T *mat.Dense) (*System, error) { + n, _, _ := sys.Dims() + if n == 0 { + return sys.Copy(), nil + } + + tr, tc := T.Dims() + if tr != n || tc != n { + return nil, fmt.Errorf("SS2SS: T must be %d×%d, got %d×%d: %w", n, n, tr, tc, ErrDimensionMismatch) + } + + var lu mat.LU + lu.Factorize(T) + if luNearSingular(&lu) { + return nil, fmt.Errorf("SS2SS: T is singular: %w", ErrSingularTransform) + } + + var Tinv mat.Dense + ones := make([]float64, n) + for i := range ones { + ones[i] = 1 + } + eye := mat.NewDiagDense(n, ones) + if err := lu.SolveTo(&Tinv, false, eye); err != nil { + return nil, fmt.Errorf("SS2SS: %w", ErrSingularTransform) + } + + var tmp, A2, B2, C2 mat.Dense + tmp.Mul(sys.A, &Tinv) + A2.Mul(T, &tmp) + B2.Mul(T, sys.B) + C2.Mul(sys.C, &Tinv) + + result := sys.Copy() + result.A = mat.DenseCopyOf(&A2) + result.B = mat.DenseCopyOf(&B2) + result.C = mat.DenseCopyOf(&C2) + return result, nil +} + +func Xperm(sys *System, perm []int) (*System, error) { + n, _, _ := sys.Dims() + if len(perm) != n { + return nil, fmt.Errorf("Xperm: perm length %d != state dim %d: %w", len(perm), n, ErrDimensionMismatch) + } + if n == 0 { + return sys.Copy(), nil + } + + seen := make([]bool, n) + for _, v := range perm { + if v < 0 || v >= n { + return nil, fmt.Errorf("Xperm: index %d out of range [0,%d): %w", v, n, ErrDimensionMismatch) + } + if seen[v] { + return nil, fmt.Errorf("Xperm: duplicate index %d: %w", v, ErrDimensionMismatch) + } + seen[v] = true + } + + P := mat.NewDense(n, n, nil) + for i, j := range perm { + P.Set(i, j, 1) + } + + return SS2SS(sys, P) +} diff --git a/utils_test.go b/utils_test.go new file mode 100644 index 0000000..27951f4 --- /dev/null +++ b/utils_test.go @@ -0,0 +1,573 @@ +package controlsys + +import ( + "errors" + "math" + "math/cmplx" + "sort" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func matClose(a, b *mat.Dense, tol float64) bool { + ar, ac := a.Dims() + br, bc := b.Dims() + if ar != br || ac != bc { + return false + } + aRaw := a.RawMatrix() + bRaw := b.RawMatrix() + for i := 0; i < ar; i++ { + for j := 0; j < ac; j++ { + if math.Abs(aRaw.Data[i*aRaw.Stride+j]-bRaw.Data[i*bRaw.Stride+j]) > tol { + return false + } + } + } + return true +} + +func sortPoles(p []complex128) { + sort.Slice(p, func(i, j int) bool { + ri, rj := real(p[i]), real(p[j]) + if math.Abs(ri-rj) > 1e-10 { + return ri < rj + } + return imag(p[i]) < imag(p[j]) + }) +} + +func polesClose(a, b []complex128, tol float64) bool { + if len(a) != len(b) { + return false + } + sa := make([]complex128, len(a)) + sb := make([]complex128, len(b)) + copy(sa, a) + copy(sb, b) + sortPoles(sa) + sortPoles(sb) + for i := range sa { + if cmplx.Abs(sa[i]-sb[i]) > tol { + return false + } + } + return true +} + +// --- Augstate tests --- + +func TestAugstate_Basic(t *testing.T) { + A := mat.NewDense(2, 2, []float64{1, 2, 3, 4}) + B := mat.NewDense(2, 1, []float64{5, 6}) + C := mat.NewDense(1, 2, []float64{1, 0}) + D := mat.NewDense(1, 1, []float64{0}) + + sys, err := New(A, B, C, D, 0) + if err != nil { + t.Fatal(err) + } + + aug, err := Augstate(sys) + if err != nil { + t.Fatal(err) + } + + n, m, p := aug.Dims() + if n != 2 || m != 1 || p != 3 { + t.Fatalf("dims = (%d,%d,%d), want (2,1,3)", n, m, p) + } + + wantC := mat.NewDense(3, 2, []float64{ + 1, 0, + 1, 0, + 0, 1, + }) + wantD := mat.NewDense(3, 1, []float64{0, 0, 0}) + + if !matClose(aug.C, wantC, 1e-12) { + t.Errorf("C mismatch:\ngot %v\nwant %v", mat.Formatted(aug.C), mat.Formatted(wantC)) + } + if !matClose(aug.D, wantD, 1e-12) { + t.Errorf("D mismatch:\ngot %v\nwant %v", mat.Formatted(aug.D), mat.Formatted(wantD)) + } +} + +func TestAugstate_StaticGain(t *testing.T) { + D := mat.NewDense(1, 1, []float64{5}) + sys, err := NewGain(D, 0) + if err != nil { + t.Fatal(err) + } + + aug, err := Augstate(sys) + if err != nil { + t.Fatal(err) + } + + n, m, p := aug.Dims() + if n != 0 || m != 1 || p != 1 { + t.Errorf("static gain augstate dims = (%d,%d,%d), want (0,1,1)", n, m, p) + } + if aug.D.At(0, 0) != 5 { + t.Errorf("D(0,0) = %v, want 5", aug.D.At(0, 0)) + } +} + +// --- SS2SS tests --- + +func TestSS2SS_Basic(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, -2, -3}) + B := mat.NewDense(2, 1, []float64{0, 1}) + C := mat.NewDense(1, 2, []float64{1, 0}) + D := mat.NewDense(1, 1, []float64{0}) + T := mat.NewDense(2, 2, []float64{1, 1, 0, 1}) + + sys, err := New(A, B, C, D, 0) + if err != nil { + t.Fatal(err) + } + + result, err := SS2SS(sys, T) + if err != nil { + t.Fatal(err) + } + + wantA := mat.NewDense(2, 2, []float64{-2, 0, -2, -1}) + wantB := mat.NewDense(2, 1, []float64{1, 1}) + wantC := mat.NewDense(1, 2, []float64{1, -1}) + wantD := mat.NewDense(1, 1, []float64{0}) + + if !matClose(result.A, wantA, 1e-12) { + t.Errorf("A2 mismatch:\ngot %v\nwant %v", mat.Formatted(result.A), mat.Formatted(wantA)) + } + if !matClose(result.B, wantB, 1e-12) { + t.Errorf("B2 mismatch:\ngot %v\nwant %v", mat.Formatted(result.B), mat.Formatted(wantB)) + } + if !matClose(result.C, wantC, 1e-12) { + t.Errorf("C2 mismatch:\ngot %v\nwant %v", mat.Formatted(result.C), mat.Formatted(wantC)) + } + if !matClose(result.D, wantD, 1e-12) { + t.Errorf("D2 mismatch:\ngot %v\nwant %v", mat.Formatted(result.D), mat.Formatted(wantD)) + } + + poles, err := result.Poles() + if err != nil { + t.Fatal(err) + } + wantPoles := []complex128{-1, -2} + if !polesClose(poles, wantPoles, 1e-10) { + t.Errorf("poles = %v, want %v", poles, wantPoles) + } +} + +func TestSS2SS_Identity(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, -2, -3}) + B := mat.NewDense(2, 1, []float64{0, 1}) + C := mat.NewDense(1, 2, []float64{1, 0}) + D := mat.NewDense(1, 1, []float64{0}) + + sys, err := New(A, B, C, D, 0) + if err != nil { + t.Fatal(err) + } + + I := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + result, err := SS2SS(sys, I) + if err != nil { + t.Fatal(err) + } + + if !matClose(result.A, sys.A, 1e-12) { + t.Error("A changed with identity T") + } + if !matClose(result.B, sys.B, 1e-12) { + t.Error("B changed with identity T") + } + if !matClose(result.C, sys.C, 1e-12) { + t.Error("C changed with identity T") + } +} + +func TestSS2SS_Singular(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, -2, -3}) + B := mat.NewDense(2, 1, []float64{0, 1}) + C := mat.NewDense(1, 2, []float64{1, 0}) + D := mat.NewDense(1, 1, []float64{0}) + + sys, err := New(A, B, C, D, 0) + if err != nil { + t.Fatal(err) + } + + Tsingular := mat.NewDense(2, 2, []float64{1, 2, 2, 4}) + _, err = SS2SS(sys, Tsingular) + if !errors.Is(err, ErrSingularTransform) { + t.Errorf("expected ErrSingularTransform, got %v", err) + } +} + +// --- Xperm tests --- + +func TestXperm_Diagonal(t *testing.T) { + A := mat.NewDense(3, 3, []float64{ + -1, 0, 0, + 0, -2, 0, + 0, 0, -3, + }) + B := mat.NewDense(3, 3, []float64{ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1, + }) + C := mat.NewDense(3, 3, []float64{ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1, + }) + D := mat.NewDense(3, 3, nil) + + sys, err := New(A, B, C, D, 0) + if err != nil { + t.Fatal(err) + } + + result, err := Xperm(sys, []int{2, 0, 1}) + if err != nil { + t.Fatal(err) + } + + wantA := mat.NewDense(3, 3, []float64{ + -3, 0, 0, + 0, -1, 0, + 0, 0, -2, + }) + wantB := mat.NewDense(3, 3, []float64{ + 0, 0, 1, + 1, 0, 0, + 0, 1, 0, + }) + wantC := mat.NewDense(3, 3, []float64{ + 0, 1, 0, + 0, 0, 1, + 1, 0, 0, + }) + + if !matClose(result.A, wantA, 1e-12) { + t.Errorf("A2 mismatch:\ngot %v\nwant %v", mat.Formatted(result.A), mat.Formatted(wantA)) + } + if !matClose(result.B, wantB, 1e-12) { + t.Errorf("B2 mismatch:\ngot %v\nwant %v", mat.Formatted(result.B), mat.Formatted(wantB)) + } + if !matClose(result.C, wantC, 1e-12) { + t.Errorf("C2 mismatch:\ngot %v\nwant %v", mat.Formatted(result.C), mat.Formatted(wantC)) + } +} + +func TestXperm_NonSymmetric(t *testing.T) { + A := mat.NewDense(2, 2, []float64{1, 2, 3, 4}) + B := mat.NewDense(2, 1, []float64{5, 6}) + C := mat.NewDense(1, 2, []float64{7, 8}) + D := mat.NewDense(1, 1, []float64{0}) + + sys, err := New(A, B, C, D, 0) + if err != nil { + t.Fatal(err) + } + + result, err := Xperm(sys, []int{1, 0}) + if err != nil { + t.Fatal(err) + } + + wantA := mat.NewDense(2, 2, []float64{4, 3, 2, 1}) + wantB := mat.NewDense(2, 1, []float64{6, 5}) + wantC := mat.NewDense(1, 2, []float64{8, 7}) + + if !matClose(result.A, wantA, 1e-12) { + t.Errorf("A2 mismatch:\ngot %v\nwant %v", mat.Formatted(result.A), mat.Formatted(wantA)) + } + if !matClose(result.B, wantB, 1e-12) { + t.Errorf("B2 mismatch:\ngot %v\nwant %v", mat.Formatted(result.B), mat.Formatted(wantB)) + } + if !matClose(result.C, wantC, 1e-12) { + t.Errorf("C2 mismatch:\ngot %v\nwant %v", mat.Formatted(result.C), mat.Formatted(wantC)) + } +} + +func TestXperm_Identity(t *testing.T) { + A := mat.NewDense(2, 2, []float64{1, 2, 3, 4}) + B := mat.NewDense(2, 1, []float64{5, 6}) + C := mat.NewDense(1, 2, []float64{7, 8}) + D := mat.NewDense(1, 1, []float64{0}) + + sys, err := New(A, B, C, D, 0) + if err != nil { + t.Fatal(err) + } + + result, err := Xperm(sys, []int{0, 1}) + if err != nil { + t.Fatal(err) + } + + if !matClose(result.A, sys.A, 1e-12) { + t.Error("A changed with identity perm") + } + if !matClose(result.B, sys.B, 1e-12) { + t.Error("B changed with identity perm") + } + if !matClose(result.C, sys.C, 1e-12) { + t.Error("C changed with identity perm") + } +} + +func TestXperm_Invalid(t *testing.T) { + A := mat.NewDense(2, 2, []float64{1, 2, 3, 4}) + B := mat.NewDense(2, 1, []float64{5, 6}) + C := mat.NewDense(1, 2, []float64{7, 8}) + D := mat.NewDense(1, 1, []float64{0}) + + sys, err := New(A, B, C, D, 0) + if err != nil { + t.Fatal(err) + } + + _, err = Xperm(sys, []int{0, 0}) + if err == nil { + t.Error("expected error for duplicate indices") + } + + _, err = Xperm(sys, []int{0, 2}) + if err == nil { + t.Error("expected error for out-of-range index") + } + + _, err = Xperm(sys, []int{0}) + if err == nil { + t.Error("expected error for wrong perm length") + } +} + +// --- Inv tests --- + +func TestInv_SISO(t *testing.T) { + sys, err := New( + mat.NewDense(1, 1, []float64{-2}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + inv, err := Inv(sys) + if err != nil { + t.Fatal(err) + } + + wantA := mat.NewDense(1, 1, []float64{-3}) + wantB := mat.NewDense(1, 1, []float64{1}) + wantC := mat.NewDense(1, 1, []float64{-1}) + wantD := mat.NewDense(1, 1, []float64{1}) + + if !matClose(inv.A, wantA, 1e-12) { + t.Errorf("A_inv = %v, want %v", mat.Formatted(inv.A), mat.Formatted(wantA)) + } + if !matClose(inv.B, wantB, 1e-12) { + t.Errorf("B_inv = %v, want %v", mat.Formatted(inv.B), mat.Formatted(wantB)) + } + if !matClose(inv.C, wantC, 1e-12) { + t.Errorf("C_inv = %v, want %v", mat.Formatted(inv.C), mat.Formatted(wantC)) + } + if !matClose(inv.D, wantD, 1e-12) { + t.Errorf("D_inv = %v, want %v", mat.Formatted(inv.D), mat.Formatted(wantD)) + } +} + +func TestInv_MIMO(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{-1, 0, 0, -2}), + mat.NewDense(2, 2, []float64{1, 0, 0, 1}), + mat.NewDense(2, 2, []float64{1, 0, 0, 1}), + mat.NewDense(2, 2, []float64{1, 0, 0, 2}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + inv, err := Inv(sys) + if err != nil { + t.Fatal(err) + } + + wantDinv := mat.NewDense(2, 2, []float64{1, 0, 0, 0.5}) + if !matClose(inv.D, wantDinv, 1e-12) { + t.Errorf("D_inv = %v, want %v", mat.Formatted(inv.D), mat.Formatted(wantDinv)) + } +} + +func TestInv_NonSquare(t *testing.T) { + sys, err := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 2, []float64{1, 0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + _, err = Inv(sys) + if !errors.Is(err, ErrDimensionMismatch) { + t.Errorf("expected ErrDimensionMismatch, got %v", err) + } +} + +func TestInv_SingularD(t *testing.T) { + sys, err := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + _, err = Inv(sys) + if !errors.Is(err, ErrSingularTransform) { + t.Errorf("expected ErrSingularTransform, got %v", err) + } +} + +func TestInv_DiscretePreservesDt(t *testing.T) { + sys, err := New( + mat.NewDense(1, 1, []float64{0.5}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + 0.01, + ) + if err != nil { + t.Fatal(err) + } + + inv, err := Inv(sys) + if err != nil { + t.Fatal(err) + } + if inv.Dt != 0.01 { + t.Errorf("Dt = %v, want 0.01", inv.Dt) + } +} + +// --- Pzmap tests --- + +func TestPzmap_PolesAndZeros(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -6, -5}), + mat.NewDense(2, 1, []float64{0, 1}), + mat.NewDense(1, 2, []float64{1, 1}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + result, err := Pzmap(sys) + if err != nil { + t.Fatal(err) + } + + wantPoles := []complex128{-2, -3} + wantZeros := []complex128{-1} + + if !polesClose(result.Poles, wantPoles, 1e-10) { + t.Errorf("poles = %v, want %v", result.Poles, wantPoles) + } + if !polesClose(result.Zeros, wantZeros, 1e-10) { + t.Errorf("zeros = %v, want %v", result.Zeros, wantZeros) + } +} + +func TestPzmap_NoZeros(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -2, -3}), + mat.NewDense(2, 1, []float64{0, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + result, err := Pzmap(sys) + if err != nil { + t.Fatal(err) + } + + wantPoles := []complex128{-1, -2} + if !polesClose(result.Poles, wantPoles, 1e-10) { + t.Errorf("poles = %v, want %v", result.Poles, wantPoles) + } + if len(result.Zeros) != 0 { + t.Errorf("zeros = %v, want empty", result.Zeros) + } +} + +// --- Isproper tests --- + +func TestIsproper_StateSpace(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -2, -3}), + mat.NewDense(2, 1, []float64{0, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + if !sys.Isproper() { + t.Error("state-space should always be proper") + } +} + +func TestIsproper_TFProper(t *testing.T) { + tf := &TransferFunc{ + Num: [][][]float64{{{1}}}, + Den: [][]float64{{1, 1}}, + } + if !tf.Isproper() { + t.Error("tf(1, [1 1]) should be proper") + } +} + +func TestIsproper_TFImproper(t *testing.T) { + tf := &TransferFunc{ + Num: [][][]float64{{{1, 0}}}, + Den: [][]float64{{1}}, + } + if tf.Isproper() { + t.Error("tf([1 0], [1]) should be improper") + } +} + +func TestIsproper_TFEqualDegree(t *testing.T) { + tf := &TransferFunc{ + Num: [][][]float64{{{1, 1}}}, + Den: [][]float64{{1, 2}}, + } + if !tf.Isproper() { + t.Error("tf([1 1], [1 2]) should be proper (equal degree)") + } +} From 2146cc31a81f81c3d0651e74783ef9c348baf844 Mon Sep 17 00:00:00 2001 From: James Joseph Date: Thu, 2 Apr 2026 21:18:29 -0500 Subject: [PATCH 2/8] fix: four correctness issues in new CST parity functions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1. FRD.Sigma() MIMO: real block form doubles each SV; take every-other value (vals[2*s]) instead of the first min(p,m). diag(2,1) now correctly returns [2,1] not [2,2]. 2. Delay metadata: Inv, Canon, Ssbal, Prescale, Sminreal, Stabsep, and Modsep now reject systems with delays (HasDelay) instead of silently dropping Delay/InputDelay/OutputDelay/LFT metadata. 3. Loopsens(P, C): changed from Loopsens(L) to accept plant and controller separately. Computes true input-side sensitivities Si=(I+CP)^{-1}, Ti=CP(I+CP)^{-1} via C*P loop, which differ from output-side So/To for non-commutative MIMO. Added regression test verifying Si != So. 4. FRDMargin: rewrote to use Bode()'s unwrapped phase instead of raw cmplx.Phase, fixing missed crossings at ±180° wrap. Fixed WgFreq/ WpFreq field assignment to match existing Margin() convention. Co-Authored-By: Claude Opus 4.6 (1M context) --- canon.go | 3 + eigdecomp.go | 3 + frd.go | 76 ++++++++++--------------- frd_test.go | 34 ++++++++++++ inv.go | 3 + loopsens.go | 71 ++++++++++++++---------- loopsens_test.go | 140 ++++++++++++++++++++++------------------------- prescale.go | 4 ++ sminreal.go | 9 ++- ssbal.go | 4 ++ 10 files changed, 196 insertions(+), 151 deletions(-) diff --git a/canon.go b/canon.go index 7f46607..c9e746c 100644 --- a/canon.go +++ b/canon.go @@ -21,6 +21,9 @@ type CanonResult struct { } func Canon(sys *System, form CanonForm) (*CanonResult, error) { + if sys.HasDelay() { + return nil, fmt.Errorf("controlsys: Canon does not support delayed systems; use Pade/AbsorbDelay first") + } switch form { case CanonModal: return canonModal(sys) diff --git a/eigdecomp.go b/eigdecomp.go index 223dc49..9bae020 100644 --- a/eigdecomp.go +++ b/eigdecomp.go @@ -13,6 +13,9 @@ import ( ) func decomposeByEigenvalues(sys *System, isGroup1 func(complex128) bool) (group1, group2 *System, err error) { + if sys.HasDelay() { + return nil, nil, fmt.Errorf("controlsys: decomposition does not support delayed systems; use Pade/AbsorbDelay first") + } n, m, p := sys.Dims() if n == 0 { cp := sys.Copy() diff --git a/frd.go b/frd.go index 4f55f85..967a920 100644 --- a/frd.go +++ b/frd.go @@ -279,12 +279,6 @@ func (f *FRD) Sigma() (*SigmaResult, error) { for k := 0; k < nw; k++ { H := f.Response[k] svd := new(mat.SVD) - hMat := mat.NewDense(p, m, nil) - for i := 0; i < p; i++ { - for j := 0; j < m; j++ { - hMat.Set(i, j, cmplx.Abs(H[i][j])) - } - } if p == 1 && m == 1 { sv[k] = cmplx.Abs(H[0][0]) continue @@ -303,7 +297,7 @@ func (f *FRD) Sigma() (*SigmaResult, error) { } vals := svd.Values(nil) for s := 0; s < nsv; s++ { - sv[k*nsv+s] = vals[s] + sv[k*nsv+s] = vals[2*s] } } omega := make([]float64, nw) @@ -321,56 +315,46 @@ func FRDMargin(f *FRD) (*MarginResult, error) { return nil, fmt.Errorf("FRDMargin: need at least 2 frequency points") } - var gmDB, pm, wcg, wcp float64 - gmDB = math.Inf(1) - foundGM, foundPM := false, false + bode := f.Bode() + magDB := bode.magDB + phase := bode.phase + + result := &MarginResult{ + GainMargin: math.Inf(1), + PhaseMargin: math.Inf(1), + WgFreq: math.NaN(), + WpFreq: math.NaN(), + } for k := 1; k < nw; k++ { - h0 := f.Response[k-1][0][0] - h1 := f.Response[k][0][0] - ph0 := cmplx.Phase(h0) * 180 / math.Pi - ph1 := cmplx.Phase(h1) * 180 / math.Pi - mag0 := cmplx.Abs(h0) - mag1 := cmplx.Abs(h1) - magDB0 := 20 * math.Log10(mag0) - magDB1 := 20 * math.Log10(mag1) - - if (ph0+180)*(ph1+180) <= 0 || (math.Abs(ph0+180) < 1 && math.Abs(ph1+180) < 1) { + ph0, ph1 := phase[k-1], phase[k] + + if (ph0+180)*(ph1+180) <= 0 && math.Abs(ph0-ph1) < 180 { t := math.Abs(ph0+180) / (math.Abs(ph0+180) + math.Abs(ph1+180) + 1e-30) - magAtCross := magDB0 + t*(magDB1-magDB0) + magAtCross := magDB[k-1] + t*(magDB[k]-magDB[k-1]) gm := -magAtCross - if !foundGM || gm < gmDB { - gmDB = gm - wcg = f.Omega[k-1] + t*(f.Omega[k]-f.Omega[k-1]) - foundGM = true + if gm > 0 && gm < result.GainMargin { + result.GainMargin = gm + result.WpFreq = bode.Omega[k-1] + t*(bode.Omega[k]-bode.Omega[k-1]) } } + } - if (magDB0)*(magDB1) <= 0 || (math.Abs(magDB0) < 0.5 && math.Abs(magDB1) < 0.5) { - t := math.Abs(magDB0) / (math.Abs(magDB0) + math.Abs(magDB1) + 1e-30) - phAtCross := ph0 + t*(ph1-ph0) - pmCand := 180 + phAtCross - if !foundPM || f.Omega[k-1]+t*(f.Omega[k]-f.Omega[k-1]) < wcp { - pm = pmCand - wcp = f.Omega[k-1] + t*(f.Omega[k]-f.Omega[k-1]) - foundPM = true + for k := 1; k < nw; k++ { + m0, m1 := magDB[k-1], magDB[k] + + if m0*m1 <= 0 && math.Abs(m0-m1) < 40 { + t := math.Abs(m0) / (math.Abs(m0) + math.Abs(m1) + 1e-30) + phAtCross := phase[k-1] + t*(phase[k]-phase[k-1]) + pm := 180 + phAtCross + if pm > 0 && pm < result.PhaseMargin { + result.PhaseMargin = pm + result.WgFreq = bode.Omega[k-1] + t*(bode.Omega[k]-bode.Omega[k-1]) } } } - if !foundGM { - gmDB = math.Inf(1) - } - if !foundPM { - pm = math.Inf(1) - } - - return &MarginResult{ - GainMargin: gmDB, - PhaseMargin: pm, - WgFreq: wcp, - WpFreq: wcg, - }, nil + return result, nil } func frdGridsMatch(f1, f2 *FRD) error { diff --git a/frd_test.go b/frd_test.go index d7b1ec5..bdf2abc 100644 --- a/frd_test.go +++ b/frd_test.go @@ -496,3 +496,37 @@ func TestFRDMargin(t *testing.T) { } } +func TestFRD_Sigma_MIMO_Regression(t *testing.T) { + // Bug: real block form doubles SVs; diag(2,1) was returning [2,2] not [2,1] + resp := [][][]complex128{ + {{2, 0}, {0, 1}}, + } + f, _ := NewFRD(resp, []float64{1.0}, 0) + sig, err := f.Sigma() + if err != nil { + t.Fatal(err) + } + if sig.nSV != 2 { + t.Fatalf("nSV = %d, want 2", sig.nSV) + } + if math.Abs(sig.sv[0]-2) > 1e-10 { + t.Errorf("sv[0] = %g, want 2", sig.sv[0]) + } + if math.Abs(sig.sv[1]-1) > 1e-10 { + t.Errorf("sv[1] = %g, want 1", sig.sv[1]) + } +} + +func TestInv_DelayRejected(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), 0) + sys.SetInputDelay([]float64{0.5}) + _, err := Inv(sys) + if err == nil { + t.Error("Inv should reject delayed system") + } +} + diff --git a/inv.go b/inv.go index f46c2b1..d861ce8 100644 --- a/inv.go +++ b/inv.go @@ -7,6 +7,9 @@ import ( ) func Inv(sys *System) (*System, error) { + if sys.HasDelay() { + return nil, fmt.Errorf("Inv: system with delays not supported; use Pade/AbsorbDelay first") + } n, m, p := sys.Dims() if m != p { return nil, fmt.Errorf("Inv: system must be square (p=%d, m=%d): %w", p, m, ErrDimensionMismatch) diff --git a/loopsens.go b/loopsens.go index d0b6cfc..5063671 100644 --- a/loopsens.go +++ b/loopsens.go @@ -7,55 +7,68 @@ import ( ) type LoopsensResult struct { - So *System - To *System - Si *System - Ti *System + So *System // output sensitivity: (I + P*C)^{-1} + To *System // output complementary sensitivity: P*C*(I + P*C)^{-1} + Si *System // input sensitivity: (I + C*P)^{-1} + Ti *System // input complementary sensitivity: C*P*(I + C*P)^{-1} } -func Loopsens(L *System) (*LoopsensResult, error) { - if L == nil { - return nil, fmt.Errorf("controlsys: loop transfer function cannot be nil") +func Loopsens(P, C *System) (*LoopsensResult, error) { + if P == nil || C == nil { + return nil, fmt.Errorf("controlsys: loopsens: P and C must not be nil") } - _, m, p := L.Dims() - if m != p { - return nil, fmt.Errorf("controlsys: loop transfer must be square (%d inputs, %d outputs): %w", m, p, ErrDimensionMismatch) + _, pm, pp := P.Dims() + _, cm, cp := C.Dims() + + if pp != cm { + return nil, fmt.Errorf("controlsys: loopsens: P outputs %d != C inputs %d: %w", pp, cm, ErrDimensionMismatch) + } + if cp != pm { + return nil, fmt.Errorf("controlsys: loopsens: C outputs %d != P inputs %d: %w", cp, pm, ErrDimensionMismatch) } - eyeData := make([]float64, m*m) - for i := 0; i < m; i++ { - eyeData[i*(m+1)] = 1 + Lo, err := Series(C, P) + if err != nil { + return nil, fmt.Errorf("controlsys: loopsens: cannot form output loop P*C: %w", err) } - I, err := NewGain(mat.NewDense(m, m, eyeData), L.Dt) + + Li, err := Series(P, C) if err != nil { - return nil, err + return nil, fmt.Errorf("controlsys: loopsens: cannot form input loop C*P: %w", err) } - So, err := Feedback(I, L, -1) + eyeO := makeIdentityGain(pp, Lo.Dt) + eyeI := makeIdentityGain(pm, Li.Dt) + + So, err := Feedback(eyeO, Lo, -1) if err != nil { - return nil, fmt.Errorf("controlsys: loopsens So computation failed: %w", err) + return nil, fmt.Errorf("controlsys: loopsens So: %w", err) } - To, err := Feedback(L, I, -1) + To, err := Feedback(Lo, eyeO, -1) if err != nil { - return nil, fmt.Errorf("controlsys: loopsens To computation failed: %w", err) + return nil, fmt.Errorf("controlsys: loopsens To: %w", err) } - Si, err := Feedback(I, L, -1) + Si, err := Feedback(eyeI, Li, -1) if err != nil { - return nil, fmt.Errorf("controlsys: loopsens Si computation failed: %w", err) + return nil, fmt.Errorf("controlsys: loopsens Si: %w", err) } - Ti, err := Feedback(L, I, -1) + Ti, err := Feedback(Li, eyeI, -1) if err != nil { - return nil, fmt.Errorf("controlsys: loopsens Ti computation failed: %w", err) + return nil, fmt.Errorf("controlsys: loopsens Ti: %w", err) } - return &LoopsensResult{ - So: So, - To: To, - Si: Si, - Ti: Ti, - }, nil + return &LoopsensResult{So: So, To: To, Si: Si, Ti: Ti}, nil +} + +func makeIdentityGain(n int, dt float64) *System { + data := make([]float64, n*n) + for i := 0; i < n; i++ { + data[i*(n+1)] = 1 + } + sys, _ := NewGain(mat.NewDense(n, n, data), dt) + return sys } diff --git a/loopsens_test.go b/loopsens_test.go index c65fe92..5056bc1 100644 --- a/loopsens_test.go +++ b/loopsens_test.go @@ -9,134 +9,124 @@ import ( ) func TestLoopsens_FirstOrder(t *testing.T) { - L, err := New( + P, _ := New( mat.NewDense(1, 1, []float64{-2}), mat.NewDense(1, 1, []float64{1}), mat.NewDense(1, 1, []float64{1}), mat.NewDense(1, 1, []float64{0}), 0) - if err != nil { - t.Fatal(err) - } + C, _ := NewGain(mat.NewDense(1, 1, []float64{1}), 0) - res, err := Loopsens(L) + res, err := Loopsens(P, C) if err != nil { t.Fatal(err) } - sDC, err := res.So.DCGain() - if err != nil { - t.Fatal(err) - } + sDC, _ := res.So.DCGain() if math.Abs(sDC.At(0, 0)-2.0/3.0) > 1e-8 { t.Errorf("dcgain(S) = %g, want 2/3", sDC.At(0, 0)) } - tDC, err := res.To.DCGain() - if err != nil { - t.Fatal(err) - } + tDC, _ := res.To.DCGain() if math.Abs(tDC.At(0, 0)-1.0/3.0) > 1e-8 { t.Errorf("dcgain(T) = %g, want 1/3", tDC.At(0, 0)) } - if math.Abs(sDC.At(0, 0)+tDC.At(0, 0)-1.0) > 1e-8 { - t.Errorf("S+T DC gain = %g, want 1", sDC.At(0, 0)+tDC.At(0, 0)) - } - for _, w := range []float64{0.01, 0.1, 1, 10, 100} { sResp, _ := res.So.EvalFr(complex(0, w)) tResp, _ := res.To.EvalFr(complex(0, w)) sum := sResp[0][0] + tResp[0][0] if cmplx.Abs(sum-1) > 1e-6 { - t.Errorf("w=%g: S+T = %v, want 1", w, sum) + t.Errorf("w=%g: So+To = %v, want 1", w, sum) + } + siResp, _ := res.Si.EvalFr(complex(0, w)) + tiResp, _ := res.Ti.EvalFr(complex(0, w)) + sumI := siResp[0][0] + tiResp[0][0] + if cmplx.Abs(sumI-1) > 1e-6 { + t.Errorf("w=%g: Si+Ti = %v, want 1", w, sumI) } } } func TestLoopsens_Integrator(t *testing.T) { - L, err := New( + P, _ := New( mat.NewDense(1, 1, []float64{0}), mat.NewDense(1, 1, []float64{1}), mat.NewDense(1, 1, []float64{1}), mat.NewDense(1, 1, []float64{0}), 0) - if err != nil { - t.Fatal(err) - } + C, _ := NewGain(mat.NewDense(1, 1, []float64{1}), 0) - res, err := Loopsens(L) + res, err := Loopsens(P, C) if err != nil { t.Fatal(err) } - tDC, err := res.To.DCGain() - if err != nil { - t.Fatal(err) - } + tDC, _ := res.To.DCGain() if math.Abs(tDC.At(0, 0)-1.0) > 1e-8 { t.Errorf("dcgain(T) = %g, want 1", tDC.At(0, 0)) } - - for _, w := range []float64{0.1, 1, 10} { - sResp, _ := res.So.EvalFr(complex(0, w)) - tResp, _ := res.To.EvalFr(complex(0, w)) - sum := sResp[0][0] + tResp[0][0] - if cmplx.Abs(sum-1) > 1e-6 { - t.Errorf("w=%g: S+T = %v, want 1", w, sum) - } - } } func TestLoopsens_NilError(t *testing.T) { - _, err := Loopsens(nil) + C, _ := NewGain(mat.NewDense(1, 1, []float64{1}), 0) + _, err := Loopsens(nil, C) if err == nil { - t.Error("expected error for nil input") + t.Error("expected error for nil P") } } -func TestLoopsens_NonSquareError(t *testing.T) { - L, err := New( +func TestLoopsens_MIMO_SiNotEqualSo(t *testing.T) { + // Non-symmetric MIMO: P and C such that P*C ≠ C*P + P, _ := New( mat.NewDense(2, 2, []float64{-1, 0, 0, -2}), - mat.NewDense(2, 1, []float64{1, 1}), mat.NewDense(2, 2, []float64{1, 0, 0, 1}), - mat.NewDense(2, 1, []float64{0, 0}), 0) + mat.NewDense(2, 2, []float64{1, 2, 0, 1}), + mat.NewDense(2, 2, []float64{0, 0, 0, 0}), 0) + C, _ := New( + mat.NewDense(1, 1, []float64{-3}), + mat.NewDense(1, 2, []float64{1, 1}), + mat.NewDense(2, 1, []float64{1, 0.5}), + mat.NewDense(2, 2, []float64{1, 0, 0, 1}), 0) + + res, err := Loopsens(P, C) if err != nil { t.Fatal(err) } - _, err = Loopsens(L) - if err == nil { - t.Error("expected error for non-square system") - } -} - -func TestLoopsens_StabilityCheck(t *testing.T) { - L, err := New( - mat.NewDense(1, 1, []float64{-2}), - mat.NewDense(1, 1, []float64{1}), - mat.NewDense(1, 1, []float64{1}), - mat.NewDense(1, 1, []float64{0}), 0) - if err != nil { - t.Fatal(err) - } - - res, err := Loopsens(L) - if err != nil { - t.Fatal(err) - } - - stable, err := res.So.IsStable() - if err != nil { - t.Fatal(err) - } - if !stable { - t.Error("sensitivity should be stable") - } + w := 1.0 + s := complex(0, w) + soResp, _ := res.So.EvalFr(s) + siResp, _ := res.Si.EvalFr(s) - stable, err = res.To.IsStable() - if err != nil { - t.Fatal(err) + differ := false + for i := range soResp { + for j := range soResp[i] { + if cmplx.Abs(soResp[i][j]-siResp[i][j]) > 1e-6 { + differ = true + } + } } - if !stable { - t.Error("complementary sensitivity should be stable") + if !differ { + t.Error("Si should differ from So for non-commutative P*C") + } + + // Still verify So+To=I and Si+Ti=I + toResp, _ := res.To.EvalFr(s) + tiResp, _ := res.Ti.EvalFr(s) + n := len(soResp) + for i := 0; i < n; i++ { + for j := 0; j < n; j++ { + eye := complex(0, 0) + if i == j { + eye = 1 + } + sumO := soResp[i][j] + toResp[i][j] + if cmplx.Abs(sumO-eye) > 1e-6 { + t.Errorf("(So+To)[%d][%d] = %v, want %v", i, j, sumO, eye) + } + sumI := siResp[i][j] + tiResp[i][j] + if cmplx.Abs(sumI-eye) > 1e-6 { + t.Errorf("(Si+Ti)[%d][%d] = %v, want %v", i, j, sumI, eye) + } + } } } diff --git a/prescale.go b/prescale.go index 0536e63..798b31e 100644 --- a/prescale.go +++ b/prescale.go @@ -1,6 +1,7 @@ package controlsys import ( + "fmt" "math" "gonum.org/v1/gonum/lapack" @@ -17,6 +18,9 @@ type PrescaleResult struct { } func Prescale(sys *System) (*PrescaleResult, error) { + if sys.HasDelay() { + return nil, fmt.Errorf("controlsys: Prescale does not support delayed systems; use Pade/AbsorbDelay first") + } n, m, p := sys.Dims() if n == 0 { diff --git a/sminreal.go b/sminreal.go index 1a9a7f3..26a8d68 100644 --- a/sminreal.go +++ b/sminreal.go @@ -1,8 +1,15 @@ package controlsys -import "gonum.org/v1/gonum/mat" +import ( + "fmt" + + "gonum.org/v1/gonum/mat" +) func Sminreal(sys *System) (*System, error) { + if sys.HasDelay() { + return nil, fmt.Errorf("controlsys: Sminreal does not support delayed systems; use Pade/AbsorbDelay first") + } n, m, p := sys.Dims() if n == 0 { return sys.Copy(), nil diff --git a/ssbal.go b/ssbal.go index f1ffb3f..41e25f2 100644 --- a/ssbal.go +++ b/ssbal.go @@ -1,6 +1,7 @@ package controlsys import ( + "fmt" "math" "gonum.org/v1/gonum/mat" @@ -12,6 +13,9 @@ type SsbalResult struct { } func Ssbal(sys *System) (*SsbalResult, error) { + if sys.HasDelay() { + return nil, fmt.Errorf("controlsys: Ssbal does not support delayed systems; use Pade/AbsorbDelay first") + } n, m, p := sys.Dims() if n == 0 { cp := sys.Copy() From ab3e8615eb71395b4fe06ae998a357abb531c1e0 Mon Sep 17 00:00:00 2001 From: James Joseph Date: Thu, 2 Apr 2026 21:28:14 -0500 Subject: [PATCH 3/8] fix: FRDMargin negative-margin fallback, Lsim non-uniform grid rejection MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit FRDMargin: collect all crossings then apply same two-pass selection as Margin() — prefer smallest positive margin, fall back to any finite value. K=10/(s+1)^3 now returns GM≈-1.94dB matching Margin(). Lsim: validate that t vector is uniformly spaced (tol 1e-6 relative) before discretizing with dt=t[1]-t[0]; reject non-uniform grids with a clear error instead of silently producing wrong output. Co-Authored-By: Claude Opus 4.6 (1M context) --- frd.go | 65 ++++++++++++++++++++++++++++++++++++++++------------- frd_test.go | 61 +++++++++++++++++++++++++++++++++++++++++++++++++ response.go | 9 ++++++++ 3 files changed, 119 insertions(+), 16 deletions(-) diff --git a/frd.go b/frd.go index 967a920..8f7ea9d 100644 --- a/frd.go +++ b/frd.go @@ -319,24 +319,21 @@ func FRDMargin(f *FRD) (*MarginResult, error) { magDB := bode.magDB phase := bode.phase - result := &MarginResult{ - GainMargin: math.Inf(1), - PhaseMargin: math.Inf(1), - WgFreq: math.NaN(), - WpFreq: math.NaN(), - } + var allGM []float64 + var allGMFreq []float64 + var allPM []float64 + var allPMFreq []float64 for k := 1; k < nw; k++ { ph0, ph1 := phase[k-1], phase[k] if (ph0+180)*(ph1+180) <= 0 && math.Abs(ph0-ph1) < 180 { - t := math.Abs(ph0+180) / (math.Abs(ph0+180) + math.Abs(ph1+180) + 1e-30) - magAtCross := magDB[k-1] + t*(magDB[k]-magDB[k-1]) + frac := math.Abs(ph0+180) / (math.Abs(ph0+180) + math.Abs(ph1+180) + 1e-30) + magAtCross := magDB[k-1] + frac*(magDB[k]-magDB[k-1]) gm := -magAtCross - if gm > 0 && gm < result.GainMargin { - result.GainMargin = gm - result.WpFreq = bode.Omega[k-1] + t*(bode.Omega[k]-bode.Omega[k-1]) - } + wCross := bode.Omega[k-1] + frac*(bode.Omega[k]-bode.Omega[k-1]) + allGM = append(allGM, gm) + allGMFreq = append(allGMFreq, wCross) } } @@ -344,12 +341,48 @@ func FRDMargin(f *FRD) (*MarginResult, error) { m0, m1 := magDB[k-1], magDB[k] if m0*m1 <= 0 && math.Abs(m0-m1) < 40 { - t := math.Abs(m0) / (math.Abs(m0) + math.Abs(m1) + 1e-30) - phAtCross := phase[k-1] + t*(phase[k]-phase[k-1]) + frac := math.Abs(m0) / (math.Abs(m0) + math.Abs(m1) + 1e-30) + phAtCross := phase[k-1] + frac*(phase[k]-phase[k-1]) pm := 180 + phAtCross - if pm > 0 && pm < result.PhaseMargin { + wCross := bode.Omega[k-1] + frac*(bode.Omega[k]-bode.Omega[k-1]) + allPM = append(allPM, pm) + allPMFreq = append(allPMFreq, wCross) + } + } + + result := &MarginResult{ + GainMargin: math.Inf(1), + PhaseMargin: math.Inf(1), + WgFreq: math.NaN(), + WpFreq: math.NaN(), + } + + for i, gm := range allGM { + if gm > 0 && gm < result.GainMargin { + result.GainMargin = gm + result.WpFreq = allGMFreq[i] + } + } + if math.IsInf(result.GainMargin, 1) { + for i, gm := range allGM { + if gm > result.GainMargin || math.IsInf(result.GainMargin, 1) { + result.GainMargin = gm + result.WpFreq = allGMFreq[i] + } + } + } + + for i, pm := range allPM { + if pm > 0 && pm < result.PhaseMargin { + result.PhaseMargin = pm + result.WgFreq = allPMFreq[i] + } + } + if math.IsInf(result.PhaseMargin, 1) { + for i, pm := range allPM { + if pm > result.PhaseMargin || math.IsInf(result.PhaseMargin, 1) { result.PhaseMargin = pm - result.WgFreq = bode.Omega[k-1] + t*(bode.Omega[k]-bode.Omega[k-1]) + result.WgFreq = allPMFreq[i] } } } diff --git a/frd_test.go b/frd_test.go index bdf2abc..0141c2d 100644 --- a/frd_test.go +++ b/frd_test.go @@ -517,6 +517,67 @@ func TestFRD_Sigma_MIMO_Regression(t *testing.T) { } } +func TestFRDMargin_NegativeMargins(t *testing.T) { + // K/(s+1)^3 with K=10: unstable closed-loop, negative margins + // Margin() returns GM≈-1.94dB, PM≈-7° + A := mat.NewDense(3, 3, []float64{-1, 1, 0, 0, -1, 1, 0, 0, -1}) + B := mat.NewDense(3, 1, []float64{0, 0, 10}) + C := mat.NewDense(1, 3, []float64{1, 0, 0}) + D := mat.NewDense(1, 1, []float64{0}) + sys, _ := New(A, B, C, D, 0) + + sysMr, err := Margin(sys) + if err != nil { + t.Fatal(err) + } + if sysMr.GainMargin >= 0 { + t.Skipf("expected negative GM, got %g", sysMr.GainMargin) + } + + w := logspace(-2, 2, 5000) + f, err := sys.FRD(w) + if err != nil { + t.Fatal(err) + } + mr, err := FRDMargin(f) + if err != nil { + t.Fatal(err) + } + + if math.IsInf(mr.GainMargin, 1) || math.IsNaN(mr.GainMargin) { + t.Errorf("FRDMargin GM = %g, want finite negative (sys GM = %g)", mr.GainMargin, sysMr.GainMargin) + } + if math.Abs(mr.GainMargin-sysMr.GainMargin) > 1 { + t.Errorf("FRDMargin GM = %g, sys GM = %g", mr.GainMargin, sysMr.GainMargin) + } + if math.IsInf(mr.PhaseMargin, 1) || math.IsNaN(mr.PhaseMargin) { + t.Errorf("FRDMargin PM = %g, want finite negative (sys PM = %g)", mr.PhaseMargin, sysMr.PhaseMargin) + } + if math.Abs(mr.PhaseMargin-sysMr.PhaseMargin) > 3 { + t.Errorf("FRDMargin PM = %g, sys PM = %g", mr.PhaseMargin, sysMr.PhaseMargin) + } + if math.IsNaN(mr.WgFreq) { + t.Errorf("FRDMargin WgFreq = NaN, want finite (sys = %g)", sysMr.WgFreq) + } + if math.IsNaN(mr.WpFreq) { + t.Errorf("FRDMargin WpFreq = NaN, want finite (sys = %g)", sysMr.WpFreq) + } +} + +func TestLsim_NonUniformGrid_Rejected(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0) + tNonUniform := []float64{0, 0.1, 0.3, 0.6} + u := mat.NewDense(4, 1, []float64{1, 1, 1, 1}) + _, err := Lsim(sys, u, tNonUniform, nil) + if err == nil { + t.Error("Lsim should reject non-uniform time grid") + } +} + func TestInv_DelayRejected(t *testing.T) { sys, _ := New( mat.NewDense(1, 1, []float64{-1}), diff --git a/response.go b/response.go index ff1d1c7..dd7f434 100644 --- a/response.go +++ b/response.go @@ -307,6 +307,15 @@ func Lsim(sys *System, u *mat.Dense, t []float64, x0 *mat.VecDense) (*TimeRespon steps := len(t) dt := t[1] - t[0] + if dt <= 0 { + return nil, fmt.Errorf("Lsim: time step must be positive, got %g: %w", dt, ErrDimensionMismatch) + } + for k := 2; k < steps; k++ { + dk := t[k] - t[k-1] + if math.Abs(dk-dt)/dt > 1e-6 { + return nil, fmt.Errorf("Lsim: non-uniform time grid at index %d (dt=%g, expected %g); uniform grid required: %w", k, dk, dt, ErrDimensionMismatch) + } + } var dsys *System var err error From 0ef7574739059aa1876418032f4655cc8bb2f854 Mon Sep 17 00:00:00 2001 From: James Joseph Date: Thu, 2 Apr 2026 21:47:09 -0500 Subject: [PATCH 4/8] fix: Lsim rejects discrete system when t spacing != sys.Dt Adds check in the discrete branch: abs(sys.Dt - dt)/sys.Dt <= 1e-6, so a discrete sys with Dt=0.1 fed a t grid at 0.05 spacing now errors instead of silently simulating with the wrong sample period. Co-Authored-By: Claude Opus 4.6 (1M context) --- frd_test.go | 14 ++++++++++++++ response.go | 3 +++ 2 files changed, 17 insertions(+) diff --git a/frd_test.go b/frd_test.go index 0141c2d..4bc0e3b 100644 --- a/frd_test.go +++ b/frd_test.go @@ -578,6 +578,20 @@ func TestLsim_NonUniformGrid_Rejected(t *testing.T) { } } +func TestLsim_DiscreteDtMismatch_Rejected(t *testing.T) { + sys, _ := New( + mat.NewDense(1, 1, []float64{0.9}), + mat.NewDense(1, 1, []float64{0.1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{0}), 0.1) + tGrid := []float64{0, 0.05, 0.10, 0.15} + u := mat.NewDense(4, 1, []float64{1, 1, 1, 1}) + _, err := Lsim(sys, u, tGrid, nil) + if err == nil { + t.Error("Lsim should reject discrete system when t spacing != sys.Dt") + } +} + func TestInv_DelayRejected(t *testing.T) { sys, _ := New( mat.NewDense(1, 1, []float64{-1}), diff --git a/response.go b/response.go index dd7f434..49b78db 100644 --- a/response.go +++ b/response.go @@ -325,6 +325,9 @@ func Lsim(sys *System, u *mat.Dense, t []float64, x0 *mat.VecDense) (*TimeRespon return nil, fmt.Errorf("Lsim: %w", err) } } else { + if math.Abs(sys.Dt-dt)/sys.Dt > 1e-6 { + return nil, fmt.Errorf("Lsim: time grid spacing %g does not match system Dt %g: %w", dt, sys.Dt, ErrDimensionMismatch) + } dsys = sys } From 6dd037c6140119a5154c99bbb9c186436ee74d82 Mon Sep 17 00:00:00 2001 From: James Joseph Date: Thu, 2 Apr 2026 22:29:18 -0500 Subject: [PATCH 5/8] bench: add benchmarks for all new CST parity functions FRD stack (System.FRD, Sigma, Margin, Series, Parallel, Feedback): SISO/MIMO nw=200/2000/10000, large n=50 MIMO, 5x5 MIMO, negative-margin FRDMargin case Time response (Step, Impulse, Lsim): SISO n=2/10/50, MIMO n=10, Lsim 1e4 steps, discrete Lsim Loopsens: SISO n=2/10, MIMO n=10 Transform/decomposition (Canon, Stabsep, Modsep, Ssbal, Prescale, Sminreal, Inv, Covar): n=2/10/50/100 sweeps, SISO + MIMO Pidtune: PI, PID, PIDF on 4-state plant All fixtures use non-symmetric A (sub+superdiagonal coupling). Co-Authored-By: Claude Opus 4.6 (1M context) --- bench_new_test.go | 407 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 407 insertions(+) create mode 100644 bench_new_test.go diff --git a/bench_new_test.go b/bench_new_test.go new file mode 100644 index 0000000..f286cc5 --- /dev/null +++ b/bench_new_test.go @@ -0,0 +1,407 @@ +package controlsys + +import ( + "math" + "testing" + + "gonum.org/v1/gonum/mat" +) + +// benchSysNonSym builds a stable system with non-symmetric A. +// A has sub/superdiagonal coupling so transposition bugs surface. +func benchSysNonSym(n, m, p int) *System { + A := mat.NewDense(n, n, nil) + for i := 0; i < n; i++ { + A.Set(i, i, -float64(i+1)*0.3) + if i > 0 { + A.Set(i, i-1, 1.0) + } + if i < n-1 { + A.Set(i, i+1, 0.1*float64(i+1)) + } + } + B := mat.NewDense(n, m, nil) + for i := 0; i < n && i < m; i++ { + B.Set(i, i, 1) + } + if n > m { + for i := m; i < n; i++ { + B.Set(i, i%m, 0.5) + } + } + C := mat.NewDense(p, n, nil) + for i := 0; i < p && i < n; i++ { + C.Set(i, i, 1) + } + if p > 1 && n > 1 { + C.Set(0, 1, 0.3) + } + D := mat.NewDense(p, m, nil) + sys, _ := New(A, B, C, D, 0) + return sys +} + +func benchFRDFromSys(sys *System, nw int) *FRD { + omega := logspace(-2, 3, nw) + f, _ := sys.FRD(omega) + return f +} + +// --------------- FRD stack --------------- + +func BenchmarkSystemFRD_SISO_200(b *testing.B) { benchSystemFRD(b, 2, 1, 1, 200) } +func BenchmarkSystemFRD_SISO_2000(b *testing.B) { benchSystemFRD(b, 2, 1, 1, 2000) } +func BenchmarkSystemFRD_SISO_10000(b *testing.B) { benchSystemFRD(b, 2, 1, 1, 10000) } +func BenchmarkSystemFRD_MIMO_200(b *testing.B) { benchSystemFRD(b, 10, 3, 4, 200) } +func BenchmarkSystemFRD_MIMO_2000(b *testing.B) { benchSystemFRD(b, 10, 3, 4, 2000) } +func BenchmarkSystemFRD_MIMO_10000(b *testing.B) { benchSystemFRD(b, 10, 3, 4, 10000) } +func BenchmarkSystemFRD_Large_2000(b *testing.B) { benchSystemFRD(b, 50, 4, 6, 2000) } + +func benchSystemFRD(b *testing.B, n, m, p, nw int) { + sys := benchSysNonSym(n, m, p) + omega := logspace(-2, 3, nw) + b.ResetTimer() + for i := 0; i < b.N; i++ { + sys.FRD(omega) + } +} + +func BenchmarkFRDSigma_SISO_200(b *testing.B) { benchFRDSigma(b, 2, 1, 1, 200) } +func BenchmarkFRDSigma_SISO_2000(b *testing.B) { benchFRDSigma(b, 2, 1, 1, 2000) } +func BenchmarkFRDSigma_MIMO_200(b *testing.B) { benchFRDSigma(b, 10, 3, 4, 200) } +func BenchmarkFRDSigma_MIMO_2000(b *testing.B) { benchFRDSigma(b, 10, 3, 4, 2000) } +func BenchmarkFRDSigma_MIMO_10000(b *testing.B) { benchFRDSigma(b, 10, 3, 4, 10000) } +func BenchmarkFRDSigma_Large_2000(b *testing.B) { benchFRDSigma(b, 50, 4, 6, 2000) } + +func benchFRDSigma(b *testing.B, n, m, p, nw int) { + sys := benchSysNonSym(n, m, p) + f := benchFRDFromSys(sys, nw) + b.ResetTimer() + for i := 0; i < b.N; i++ { + f.Sigma() + } +} + +func BenchmarkFRDMargin_200(b *testing.B) { benchFRDMargin(b, 4, 200) } +func BenchmarkFRDMargin_2000(b *testing.B) { benchFRDMargin(b, 4, 2000) } +func BenchmarkFRDMargin_10000(b *testing.B) { benchFRDMargin(b, 4, 10000) } + +func BenchmarkFRDMargin_NegGM_2000(b *testing.B) { + A := mat.NewDense(3, 3, []float64{-1, 1, 0, 0, -1, 1, 0, 0, -1}) + B := mat.NewDense(3, 1, []float64{0, 0, 10}) + C := mat.NewDense(1, 3, []float64{1, 0, 0}) + D := mat.NewDense(1, 1, []float64{0}) + sys, _ := New(A, B, C, D, 0) + f := benchFRDFromSys(sys, 2000) + b.ResetTimer() + for i := 0; i < b.N; i++ { + FRDMargin(f) + } +} + +func benchFRDMargin(b *testing.B, n, nw int) { + sys := benchSysNonSym(n, 1, 1) + f := benchFRDFromSys(sys, nw) + b.ResetTimer() + for i := 0; i < b.N; i++ { + FRDMargin(f) + } +} + +func BenchmarkFRDSigma_MIMO5x5_2000(b *testing.B) { benchFRDSigma(b, 20, 5, 5, 2000) } + +func BenchmarkFRDSeries_SISO_200(b *testing.B) { benchFRDSeries(b, 2, 1, 1, 200) } +func BenchmarkFRDSeries_SISO_2000(b *testing.B) { benchFRDSeries(b, 2, 1, 1, 2000) } +func BenchmarkFRDSeries_MIMO_200(b *testing.B) { benchFRDSeries(b, 10, 3, 3, 200) } +func BenchmarkFRDSeries_MIMO_2000(b *testing.B) { benchFRDSeries(b, 10, 3, 3, 2000) } +func BenchmarkFRDSeries_MIMO_10000(b *testing.B) { benchFRDSeries(b, 10, 3, 3, 10000) } + +func benchFRDSeries(b *testing.B, n, m, p, nw int) { + s1 := benchSysNonSym(n, m, p) + s2 := benchSysNonSym(n, p, m) + f1 := benchFRDFromSys(s1, nw) + f2 := benchFRDFromSys(s2, nw) + b.ResetTimer() + for i := 0; i < b.N; i++ { + FRDSeries(f1, f2) + } +} + +func BenchmarkFRDParallel_SISO_2000(b *testing.B) { benchFRDParallel(b, 2, 1, 1, 2000) } +func BenchmarkFRDParallel_MIMO_2000(b *testing.B) { benchFRDParallel(b, 10, 3, 3, 2000) } + +func benchFRDParallel(b *testing.B, n, m, p, nw int) { + sys := benchSysNonSym(n, m, p) + f1 := benchFRDFromSys(sys, nw) + f2 := benchFRDFromSys(sys, nw) + b.ResetTimer() + for i := 0; i < b.N; i++ { + FRDParallel(f1, f2) + } +} + +func BenchmarkFRDFeedback_SISO_200(b *testing.B) { benchFRDFeedback(b, 2, 1, 1, 200) } +func BenchmarkFRDFeedback_SISO_2000(b *testing.B) { benchFRDFeedback(b, 2, 1, 1, 2000) } +func BenchmarkFRDFeedback_MIMO_200(b *testing.B) { benchFRDFeedback(b, 10, 3, 3, 200) } +func BenchmarkFRDFeedback_MIMO_2000(b *testing.B) { benchFRDFeedback(b, 10, 3, 3, 2000) } +func BenchmarkFRDFeedback_MIMO_10000(b *testing.B) { benchFRDFeedback(b, 10, 3, 3, 10000) } + +func benchFRDFeedback(b *testing.B, n, m, p, nw int) { + plant := benchSysNonSym(n, m, p) + ctrl := benchSysNonSym(n/2+1, p, m) + fp := benchFRDFromSys(plant, nw) + fc := benchFRDFromSys(ctrl, nw) + b.ResetTimer() + for i := 0; i < b.N; i++ { + FRDFeedback(fp, fc, -1) + } +} + +// --------------- Time-response wrappers --------------- + +func BenchmarkStep_SISO_N2(b *testing.B) { benchStep(b, 2, 1, 1) } +func BenchmarkStep_SISO_N10(b *testing.B) { benchStep(b, 10, 1, 1) } +func BenchmarkStep_SISO_N50(b *testing.B) { benchStep(b, 50, 1, 1) } +func BenchmarkStep_MIMO_N10(b *testing.B) { benchStep(b, 10, 3, 4) } + +func benchStep(b *testing.B, n, m, p int) { + sys := benchSysNonSym(n, m, p) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Step(sys, 10) + } +} + +func BenchmarkImpulse_SISO_N2(b *testing.B) { benchImpulse(b, 2, 1, 1) } +func BenchmarkImpulse_SISO_N10(b *testing.B) { benchImpulse(b, 10, 1, 1) } +func BenchmarkImpulse_SISO_N50(b *testing.B) { benchImpulse(b, 50, 1, 1) } +func BenchmarkImpulse_MIMO_N10(b *testing.B) { benchImpulse(b, 10, 3, 4) } + +func benchImpulse(b *testing.B, n, m, p int) { + sys := benchSysNonSym(n, m, p) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Impulse(sys, 10) + } +} + +func BenchmarkLsim_SISO_N2(b *testing.B) { benchLsimB(b, 2, 1, 1, 500) } +func BenchmarkLsim_SISO_N10(b *testing.B) { benchLsimB(b, 10, 1, 1, 500) } +func BenchmarkLsim_SISO_N50(b *testing.B) { benchLsimB(b, 50, 1, 1, 500) } +func BenchmarkLsim_MIMO_N10(b *testing.B) { benchLsimB(b, 10, 3, 4, 500) } +func BenchmarkLsim_SISO_1e4(b *testing.B) { benchLsimB(b, 4, 1, 1, 10000) } +func BenchmarkLsim_MIMO_1e4(b *testing.B) { benchLsimB(b, 10, 4, 4, 10000) } + +func BenchmarkLsim_Discrete_SISO(b *testing.B) { + sys := benchSysNonSym(4, 1, 1) + dsys, _ := sys.DiscretizeZOH(0.01) + steps := 1000 + t := make([]float64, steps) + for k := range t { + t[k] = float64(k) * 0.01 + } + u := mat.NewDense(steps, 1, nil) + for k := range steps { + u.Set(k, 0, 1) + } + b.ResetTimer() + for i := 0; i < b.N; i++ { + Lsim(dsys, u, t, nil) + } +} + +func benchLsimB(b *testing.B, n, m, p, steps int) { + sys := benchSysNonSym(n, m, p) + dt := 0.01 + t := make([]float64, steps) + for k := range t { + t[k] = float64(k) * dt + } + u := mat.NewDense(steps, m, nil) + for k := 0; k < steps; k++ { + for j := 0; j < m; j++ { + u.Set(k, j, math.Sin(float64(k)*dt*float64(j+1))) + } + } + b.ResetTimer() + for i := 0; i < b.N; i++ { + Lsim(sys, u, t, nil) + } +} + +// --------------- Loopsens --------------- + +func BenchmarkLoopsens_SISO_N2(b *testing.B) { benchLoopsens(b, 2, 1, 1) } +func BenchmarkLoopsens_SISO_N10(b *testing.B) { benchLoopsens(b, 10, 1, 1) } +func BenchmarkLoopsens_MIMO_N10(b *testing.B) { benchLoopsens(b, 10, 3, 3) } + +func benchLoopsens(b *testing.B, n, m, p int) { + P := benchSysNonSym(n, m, p) + C := benchSysNonSym(n/2+1, p, m) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Loopsens(P, C) + } +} + +// --------------- Transform / decomposition --------------- + +func BenchmarkCanon_Modal_N2(b *testing.B) { benchCanon(b, CanonModal, 2) } +func BenchmarkCanon_Modal_N10(b *testing.B) { benchCanon(b, CanonModal, 10) } +func BenchmarkCanon_Modal_N50(b *testing.B) { benchCanon(b, CanonModal, 50) } +func BenchmarkCanon_Modal_N100(b *testing.B) { benchCanon(b, CanonModal, 100) } +func BenchmarkCanon_Comp_N2(b *testing.B) { benchCanon(b, CanonCompanion, 2) } +func BenchmarkCanon_Comp_N10(b *testing.B) { benchCanon(b, CanonCompanion, 10) } + +func benchCanon(b *testing.B, form CanonForm, n int) { + sys := benchSysNonSym(n, 1, 1) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Canon(sys, form) + } +} + +func BenchmarkStabsep_N2(b *testing.B) { benchStabsep(b, 2) } +func BenchmarkStabsep_N10(b *testing.B) { benchStabsep(b, 10) } +func BenchmarkStabsep_N50(b *testing.B) { benchStabsep(b, 50) } +func BenchmarkStabsep_N100(b *testing.B) { benchStabsep(b, 100) } + +func benchStabsep(b *testing.B, n int) { + A := mat.NewDense(n, n, nil) + for i := 0; i < n; i++ { + if i%2 == 0 { + A.Set(i, i, -float64(i+1)*0.3) + } else { + A.Set(i, i, float64(i)*0.2) + } + if i > 0 { + A.Set(i, i-1, 0.5) + } + if i < n-1 { + A.Set(i, i+1, 0.1) + } + } + B := mat.NewDense(n, 2, nil) + B.Set(0, 0, 1) + if n > 1 { + B.Set(1, 1, 1) + } + C := mat.NewDense(2, n, nil) + C.Set(0, 0, 1) + if n > 1 { + C.Set(1, 1, 1) + } + D := mat.NewDense(2, 2, nil) + sys, _ := New(A, B, C, D, 0) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Stabsep(sys) + } +} + +func BenchmarkModsep_N10(b *testing.B) { benchModsep(b, 10) } +func BenchmarkModsep_N50(b *testing.B) { benchModsep(b, 50) } + +func benchModsep(b *testing.B, n int) { + sys := benchSysNonSym(n, 2, 2) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Modsep(sys, 1.0) + } +} + +func BenchmarkSsbal_N2(b *testing.B) { benchSsbalB(b, 2) } +func BenchmarkSsbal_N10(b *testing.B) { benchSsbalB(b, 10) } +func BenchmarkSsbal_N50(b *testing.B) { benchSsbalB(b, 50) } + +func benchSsbalB(b *testing.B, n int) { + sys := benchSysNonSym(n, 2, 2) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Ssbal(sys) + } +} + +func BenchmarkPrescale_N2(b *testing.B) { benchPrescaleB(b, 2) } +func BenchmarkPrescale_N10(b *testing.B) { benchPrescaleB(b, 10) } +func BenchmarkPrescale_N50(b *testing.B) { benchPrescaleB(b, 50) } + +func benchPrescaleB(b *testing.B, n int) { + sys := benchSysNonSym(n, 2, 2) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Prescale(sys) + } +} + +func BenchmarkSminreal_N2(b *testing.B) { benchSminrealB(b, 2) } +func BenchmarkSminreal_N10(b *testing.B) { benchSminrealB(b, 10) } +func BenchmarkSminreal_N50(b *testing.B) { benchSminrealB(b, 50) } + +func benchSminrealB(b *testing.B, n int) { + sys := benchSysNonSym(n, 2, 2) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Sminreal(sys) + } +} + +func BenchmarkInv_SISO_N2(b *testing.B) { benchInvB(b, 2, 1, 1) } +func BenchmarkInv_SISO_N10(b *testing.B) { benchInvB(b, 10, 1, 1) } +func BenchmarkInv_MIMO_N10(b *testing.B) { benchInvB(b, 10, 3, 3) } + +func benchInvB(b *testing.B, n, m, p int) { + sys := benchSysNonSym(n, m, p) + sys.D.Set(0, 0, 1) + for i := 1; i < m && i < p; i++ { + sys.D.Set(i, i, 1) + } + b.ResetTimer() + for i := 0; i < b.N; i++ { + Inv(sys) + } +} + +func BenchmarkCovar_SISO_N2(b *testing.B) { benchCovarB(b, 2, 1, 1) } +func BenchmarkCovar_SISO_N10(b *testing.B) { benchCovarB(b, 10, 1, 1) } +func BenchmarkCovar_SISO_N50(b *testing.B) { benchCovarB(b, 50, 1, 1) } +func BenchmarkCovar_SISO_N100(b *testing.B) { benchCovarB(b, 100, 1, 1) } +func BenchmarkCovar_MIMO_N10(b *testing.B) { benchCovarB(b, 10, 3, 4) } + +func benchCovarB(b *testing.B, n, m, p int) { + sys := benchSysNonSym(n, m, p) + W := mat.NewDense(m, m, nil) + for i := 0; i < m; i++ { + W.Set(i, i, 1) + } + b.ResetTimer() + for i := 0; i < b.N; i++ { + Covar(sys, W) + } +} + +// --------------- Pidtune --------------- + +func BenchmarkPidtune_PI(b *testing.B) { + sys := benchSysNonSym(4, 1, 1) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Pidtune(sys, "PI") + } +} + +func BenchmarkPidtune_PID(b *testing.B) { + sys := benchSysNonSym(4, 1, 1) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Pidtune(sys, "PID") + } +} + +func BenchmarkPidtune_PIDF(b *testing.B) { + sys := benchSysNonSym(4, 1, 1) + b.ResetTimer() + for i := 0; i < b.N; i++ { + Pidtune(sys, "PIDF") + } +} From bed9ac23777d9f311f9df6d841b60a5c90aa49dc Mon Sep 17 00:00:00 2001 From: James Joseph Date: Fri, 3 Apr 2026 08:48:50 -0500 Subject: [PATCH 6/8] =?UTF-8?q?perf:=20optimize=20FRD=20stack=20=E2=80=94?= =?UTF-8?q?=20workspace=20reuse,=20flat=20storage,=20LAPACK=20direct?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit System.FRD: single-alloc contiguous response storage via newFRDResponseStorage; bulk copy from FreqResponse.Data. FRD.Sigma: pre-allocate SVD workspace (block matrix, work array, singular values) once via newFRDSVDWorkspace, reuse across all frequency points; call impl.Dgesvd directly instead of mat.SVD. FRDSeries/Parallel: write into pre-allocated flat data slice via cMulNestedInto/cAddNestedInto instead of per-frequency [][]complex128. FRDFeedback: pre-allocate all scratch buffers (G, K, KG, I+KG, inv, aug) in newFRDFeedbackWorkspace; flat complex128 multiply/invert via cMulInto/cInvertInto; zero per-iteration allocations. benchstat (geomean): -14% time, -22% memory SystemFRD SISO nw=10k: -50% time, -99.7% allocs FRDSigma MIMO nw=10k: -14% time, -93% memory FRDFeedback SISO nw=2k: -74% time, -58% memory FRDFeedback MIMO nw=10k: -54% time, -81% memory FRDSeries MIMO nw=10k: -39% time, -99.9% allocs Co-Authored-By: Claude Opus 4.6 (1M context) --- frd.go | 266 +++++++++++++++++++++++++++++----------------------- frd_test.go | 118 ++++++++++++++++++++++- 2 files changed, 265 insertions(+), 119 deletions(-) diff --git a/frd.go b/frd.go index 8f7ea9d..ee808ca 100644 --- a/frd.go +++ b/frd.go @@ -6,7 +6,7 @@ import ( "math/cmplx" "sort" - "gonum.org/v1/gonum/mat" + "gonum.org/v1/gonum/lapack" ) // FRD represents a Frequency Response Data model — measured or computed @@ -75,14 +75,8 @@ func NewFRD(response [][][]complex128, omega []float64, dt float64) (*FRD, error } } - resp := make([][][]complex128, len(response)) - for k := range response { - resp[k] = make([][]complex128, p) - for i := range response[k] { - resp[k][i] = make([]complex128, m) - copy(resp[k][i], response[k][i]) - } - } + resp, data := newFRDResponseStorage(len(response), p, m) + copyComplexGridInto(data, response, p, m) om := make([]float64, len(omega)) copy(om, omega) @@ -107,16 +101,8 @@ func (sys *System) FRD(omega []float64) (*FRD, error) { _, m, p := sys.Dims() nw := len(omega) - response := make([][][]complex128, nw) - for k := 0; k < nw; k++ { - response[k] = make([][]complex128, p) - for i := 0; i < p; i++ { - response[k][i] = make([]complex128, m) - for j := 0; j < m; j++ { - response[k][i][j] = resp.At(k, i, j) - } - } - } + response, data := newFRDResponseStorage(nw, p, m) + copy(data, resp.Data) om := make([]float64, nw) copy(om, omega) @@ -130,6 +116,62 @@ func (sys *System) FRD(omega []float64) (*FRD, error) { }, nil } +func newFRDResponseStorage(nw, p, m int) ([][][]complex128, []complex128) { + response := make([][][]complex128, nw) + if nw == 0 || p == 0 || m == 0 { + return response, nil + } + rows := make([][]complex128, nw*p) + data := make([]complex128, nw*p*m) + for k := 0; k < nw; k++ { + response[k] = rows[k*p : (k+1)*p] + block := data[k*p*m : (k+1)*p*m] + for i := 0; i < p; i++ { + start := i * m + response[k][i] = block[start : start+m : start+m] + } + } + return response, data +} + +func copyComplexGridInto(dst []complex128, src [][][]complex128, p, m int) { + pm := p * m + for k := range src { + base := k * pm + for i := 0; i < p; i++ { + copy(dst[base+i*m:base+(i+1)*m], src[k][i]) + } + } +} + +func copyComplexMatrixInto(dst []complex128, src [][]complex128, rows, cols int) { + for i := 0; i < rows; i++ { + copy(dst[i*cols:(i+1)*cols], src[i]) + } +} + +func cMulNestedInto(dst []complex128, a, b [][]complex128, ra, ca, cb int) { + for i := 0; i < ra; i++ { + row := dst[i*cb : (i+1)*cb] + for j := 0; j < cb; j++ { + var sum complex128 + for k := 0; k < ca; k++ { + sum += a[i][k] * b[k][j] + } + row[j] = sum + } + } +} + +func cAddNestedInto(dst []complex128, a, b [][]complex128, rows, cols int) { + for i := 0; i < rows; i++ { + row := dst[i*cols : (i+1)*cols] + for j := 0; j < cols; j++ { + row[j] = a[i][j] + b[i][j] + } + } +} + func (f *FRD) Dims() (p, m int) { if len(f.Response) == 0 { return 0, 0 @@ -271,33 +313,40 @@ func (f *FRD) Nyquist() (*NyquistResult, error) { func (f *FRD) Sigma() (*SigmaResult, error) { p, m := f.Dims() nw := len(f.Omega) - nsv := p - if m < p { - nsv = m + nsv := min(p, m) + if nsv == 0 { + omega := make([]float64, nw) + copy(omega, f.Omega) + return &SigmaResult{Omega: omega, nSV: 0}, nil } sv := make([]float64, nw*nsv) + if p == 1 && m == 1 { + for k := 0; k < nw; k++ { + sv[k] = cmplx.Abs(f.Response[k][0][0]) + } + omega := make([]float64, nw) + copy(omega, f.Omega) + return &SigmaResult{Omega: omega, sv: sv, nSV: nsv}, nil + } + + ws := newFRDSVDWorkspace(p, m) for k := 0; k < nw; k++ { H := f.Response[k] - svd := new(mat.SVD) - if p == 1 && m == 1 { - sv[k] = cmplx.Abs(H[0][0]) - continue - } - hReal := mat.NewDense(2*p, 2*m, nil) for i := 0; i < p; i++ { + top := i * ws.cols + bottom := (p + i) * ws.cols for j := 0; j < m; j++ { - hReal.Set(i, j, real(H[i][j])) - hReal.Set(i, m+j, -imag(H[i][j])) - hReal.Set(p+i, j, imag(H[i][j])) - hReal.Set(p+i, m+j, real(H[i][j])) + h := H[i][j] + ws.block[top+j] = real(h) + ws.block[top+m+j] = -imag(h) + ws.block[bottom+j] = imag(h) + ws.block[bottom+m+j] = real(h) } } - if !svd.Factorize(hReal, mat.SVDNone) { - return nil, fmt.Errorf("FRD Sigma: SVD failed at freq index %d", k) - } - vals := svd.Values(nil) - for s := 0; s < nsv; s++ { - sv[k*nsv+s] = vals[2*s] + impl.Dgesvd(lapack.SVDNone, lapack.SVDNone, ws.rows, ws.cols, ws.block, ws.cols, + ws.fullSV, nil, 1, nil, 1, ws.work, len(ws.work)) + for i := 0; i < nsv; i++ { + sv[k*nsv+i] = ws.fullSV[2*i] } } omega := make([]float64, nw) @@ -305,6 +354,32 @@ func (f *FRD) Sigma() (*SigmaResult, error) { return &SigmaResult{Omega: omega, sv: sv, nSV: nsv}, nil } +type frdSVDWorkspace struct { + block []float64 + fullSV []float64 + work []float64 + rows int + cols int +} + +func newFRDSVDWorkspace(p, m int) *frdSVDWorkspace { + rows := 2 * p + cols := 2 * m + fullSV := make([]float64, min(rows, cols)) + block := make([]float64, rows*cols) + workQuery := make([]float64, 1) + impl.Dgesvd(lapack.SVDNone, lapack.SVDNone, rows, cols, block, cols, + fullSV, nil, 1, nil, 1, workQuery, -1) + work := make([]float64, int(workQuery[0])) + return &frdSVDWorkspace{ + block: block, + fullSV: fullSV, + work: work, + rows: rows, + cols: cols, + } +} + func FRDMargin(f *FRD) (*MarginResult, error) { p, m := f.Dims() if p != 1 || m != 1 { @@ -413,9 +488,9 @@ func FRDSeries(f1, f2 *FRD) (*FRD, error) { } nw := len(f1.Omega) - resp := make([][][]complex128, nw) + resp, data := newFRDResponseStorage(nw, p2, m1) for w := 0; w < nw; w++ { - resp[w] = cMatMul(f2.Response[w], f1.Response[w], p2, p1, m1) + cMulNestedInto(data[w*p2*m1:(w+1)*p2*m1], f2.Response[w], f1.Response[w], p2, p1, m1) } return &FRD{Response: resp, Omega: append([]float64(nil), f1.Omega...), Dt: f1.Dt}, nil } @@ -431,16 +506,9 @@ func FRDParallel(f1, f2 *FRD) (*FRD, error) { } nw := len(f1.Omega) - resp := make([][][]complex128, nw) + resp, data := newFRDResponseStorage(nw, p1, m1) for w := 0; w < nw; w++ { - r := make([][]complex128, p1) - for i := 0; i < p1; i++ { - r[i] = make([]complex128, m1) - for j := 0; j < m1; j++ { - r[i][j] = f1.Response[w][i][j] + f2.Response[w][i][j] - } - } - resp[w] = r + cAddNestedInto(data[w*p1*m1:(w+1)*p1*m1], f1.Response[w], f2.Response[w], p1, m1) } return &FRD{Response: resp, Omega: append([]float64(nil), f1.Omega...), Dt: f1.Dt}, nil } @@ -459,88 +527,50 @@ func FRDFeedback(plant, controller *FRD, sign float64) (*FRD, error) { } nw := len(plant.Omega) - resp := make([][][]complex128, nw) + resp, data := newFRDResponseStorage(nw, pp, pm) + ws := newFRDFeedbackWorkspace(pp, pm) for w := 0; w < nw; w++ { - G := plant.Response[w] - K := controller.Response[w] - KG := cMatMul(K, G, cp, pp, pm) - - n := pm - IpKG := make([][]complex128, n) - for i := 0; i < n; i++ { - IpKG[i] = make([]complex128, n) - for j := 0; j < n; j++ { - IpKG[i][j] = complex(-sign, 0) * KG[i][j] - if i == j { - IpKG[i][j] += 1 - } - } + copyComplexMatrixInto(ws.g, plant.Response[w], pp, pm) + copyComplexMatrixInto(ws.k, controller.Response[w], cp, pp) + cMulInto(ws.kg, ws.k, ws.g, cp, pp, pm) + + for i := range ws.kg { + ws.ipkg[i] = complex(-sign, 0) * ws.kg[i] + } + for i := 0; i < ws.n; i++ { + ws.ipkg[i*ws.n+i] += 1 } - inv, err := cMatInv(IpKG, n) + err := cInvertInto(ws.inv, ws.aug, ws.ipkg, ws.n) if err != nil { return nil, fmt.Errorf("frd feedback: singular at freq index %d", w) } - resp[w] = cMatMul(G, inv, pp, n, pm) + cMulInto(data[w*pp*pm:(w+1)*pp*pm], ws.g, ws.inv, pp, ws.n, pm) } return &FRD{Response: resp, Omega: append([]float64(nil), plant.Omega...), Dt: plant.Dt}, nil } -func cMatMul(a, b [][]complex128, ra, ca, cb int) [][]complex128 { - r := make([][]complex128, ra) - for i := 0; i < ra; i++ { - r[i] = make([]complex128, cb) - for j := 0; j < cb; j++ { - var s complex128 - for k := 0; k < ca; k++ { - s += a[i][k] * b[k][j] - } - r[i][j] = s - } - } - return r +type frdFeedbackWorkspace struct { + g []complex128 + k []complex128 + kg []complex128 + ipkg []complex128 + inv []complex128 + aug []complex128 + n int } -func cMatInv(a [][]complex128, n int) ([][]complex128, error) { - aug := make([][]complex128, n) - for i := 0; i < n; i++ { - aug[i] = make([]complex128, 2*n) - copy(aug[i][:n], a[i]) - aug[i][n+i] = 1 - } - for col := 0; col < n; col++ { - pivot := -1 - var best float64 - for row := col; row < n; row++ { - if v := cmplx.Abs(aug[row][col]); v > best { - best = v - pivot = row - } - } - if best < 1e-15 { - return nil, fmt.Errorf("singular") - } - aug[col], aug[pivot] = aug[pivot], aug[col] - s := 1.0 / aug[col][col] - for j := col; j < 2*n; j++ { - aug[col][j] *= s - } - for row := 0; row < n; row++ { - if row == col { - continue - } - f := aug[row][col] - for j := col; j < 2*n; j++ { - aug[row][j] -= f * aug[col][j] - } - } - } - inv := make([][]complex128, n) - for i := 0; i < n; i++ { - inv[i] = make([]complex128, n) - copy(inv[i], aug[i][n:]) +func newFRDFeedbackWorkspace(pp, pm int) *frdFeedbackWorkspace { + n := pm + return &frdFeedbackWorkspace{ + g: make([]complex128, pp*pm), + k: make([]complex128, pm*pp), + kg: make([]complex128, n*n), + ipkg: make([]complex128, n*n), + inv: make([]complex128, n*n), + aug: make([]complex128, n*2*n), + n: n, } - return inv, nil } diff --git a/frd_test.go b/frd_test.go index 4bc0e3b..fc11def 100644 --- a/frd_test.go +++ b/frd_test.go @@ -358,6 +358,123 @@ func TestFRD_FreqResponseMatrix(t *testing.T) { } } +func TestFRDSeries_MIMO(t *testing.T) { + f1, err := NewFRD([][][]complex128{ + {{1 + 1i, 2 - 1i}, {3 + 0i, -1 + 2i}}, + }, []float64{1}, 0) + if err != nil { + t.Fatal(err) + } + f2, err := NewFRD([][][]complex128{ + {{2 + 0i, -1 + 1i}, {0.5 - 0.5i, 4 + 0i}}, + }, []float64{1}, 0) + if err != nil { + t.Fatal(err) + } + + got, err := FRDSeries(f1, f2) + if err != nil { + t.Fatal(err) + } + + want := [2][2]complex128{ + { + f2.At(0, 0, 0)*f1.At(0, 0, 0) + f2.At(0, 0, 1)*f1.At(0, 1, 0), + f2.At(0, 0, 0)*f1.At(0, 0, 1) + f2.At(0, 0, 1)*f1.At(0, 1, 1), + }, + { + f2.At(0, 1, 0)*f1.At(0, 0, 0) + f2.At(0, 1, 1)*f1.At(0, 1, 0), + f2.At(0, 1, 0)*f1.At(0, 0, 1) + f2.At(0, 1, 1)*f1.At(0, 1, 1), + }, + } + for i := 0; i < 2; i++ { + for j := 0; j < 2; j++ { + if cmplx.Abs(got.At(0, i, j)-want[i][j]) > 1e-12 { + t.Fatalf("series[%d,%d] = %v, want %v", i, j, got.At(0, i, j), want[i][j]) + } + } + } +} + +func TestFRDParallel_MIMO(t *testing.T) { + f1, err := NewFRD([][][]complex128{ + {{1 + 1i, 2 - 1i}, {3 + 0i, -1 + 2i}}, + }, []float64{1}, 0) + if err != nil { + t.Fatal(err) + } + f2, err := NewFRD([][][]complex128{ + {{2 + 0i, -1 + 1i}, {0.5 - 0.5i, 4 + 0i}}, + }, []float64{1}, 0) + if err != nil { + t.Fatal(err) + } + + got, err := FRDParallel(f1, f2) + if err != nil { + t.Fatal(err) + } + + for i := 0; i < 2; i++ { + for j := 0; j < 2; j++ { + want := f1.At(0, i, j) + f2.At(0, i, j) + if cmplx.Abs(got.At(0, i, j)-want) > 1e-12 { + t.Fatalf("parallel[%d,%d] = %v, want %v", i, j, got.At(0, i, j), want) + } + } + } +} + +func TestFRDFeedback_MIMO(t *testing.T) { + plant, err := NewFRD([][][]complex128{ + {{2 + 0.5i, 0}, {0, 1 - 0.25i}}, + {{1 - 0.5i, 0}, {0, 0.5 + 0.75i}}, + }, []float64{1, 2}, 0) + if err != nil { + t.Fatal(err) + } + controller, err := NewFRD([][][]complex128{ + {{3 - 0.5i, 0}, {0, -0.5 + 0.25i}}, + {{2 + 0.5i, 0}, {0, 1 + 0.5i}}, + }, []float64{1, 2}, 0) + if err != nil { + t.Fatal(err) + } + + got, err := FRDFeedback(plant, controller, -1) + if err != nil { + t.Fatal(err) + } + + for k := 0; k < 2; k++ { + for i := 0; i < 2; i++ { + g := plant.At(k, i, i) + c := controller.At(k, i, i) + want := g / (1 + c*g) + if cmplx.Abs(got.At(k, i, i)-want) > 1e-12 { + t.Fatalf("feedback[%d,%d,%d] = %v, want %v", k, i, i, got.At(k, i, i), want) + } + } + if cmplx.Abs(got.At(k, 0, 1)) > 1e-12 || cmplx.Abs(got.At(k, 1, 0)) > 1e-12 { + t.Fatalf("feedback off-diagonals = [%v %v], want 0", got.At(k, 0, 1), got.At(k, 1, 0)) + } + } +} + +func TestFRDFeedback_Singular(t *testing.T) { + plant, err := NewFRD([][][]complex128{{{1}}}, []float64{1}, 0) + if err != nil { + t.Fatal(err) + } + controller, err := NewFRD([][][]complex128{{{-1}}}, []float64{1}, 0) + if err != nil { + t.Fatal(err) + } + if _, err := FRDFeedback(plant, controller, -1); err == nil { + t.Fatal("expected singular FRD feedback error") + } +} + func TestFRD_Bode(t *testing.T) { sys, err := New( mat.NewDense(1, 1, []float64{-1}), @@ -604,4 +721,3 @@ func TestInv_DelayRejected(t *testing.T) { t.Error("Inv should reject delayed system") } } - From 36fe4e9942294480e45f14de97ed69642fef76a1 Mon Sep 17 00:00:00 2001 From: James Joseph Date: Fri, 3 Apr 2026 08:54:10 -0500 Subject: [PATCH 7/8] fix: canon.go use relative tolerance for eigenvalue classification Absolute 1e-10 tolerance breaks for eigenvalues with magnitude >>1 where machine eps on the imaginary part exceeds the threshold. Use 1e-10*max(1, |lambda|) consistent with eigdecomp.go's isConjugate. Co-Authored-By: Claude Opus 4.6 (1M context) --- canon.go | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/canon.go b/canon.go index c9e746c..4c179c0 100644 --- a/canon.go +++ b/canon.go @@ -3,6 +3,7 @@ package controlsys import ( "fmt" "math" + "math/cmplx" "sort" "gonum.org/v1/gonum/mat" @@ -64,7 +65,9 @@ func canonModal(sys *System) (*CanonResult, error) { if used[i] { continue } - if math.Abs(imag(vals[i])) < 1e-10 { + eigMag := cmplx.Abs(vals[i]) + tol := 1e-10 * math.Max(1, eigMag) + if math.Abs(imag(vals[i])) < tol { blocks = append(blocks, eigBlock{ mag: math.Abs(real(vals[i])), real1: real(vals[i]), @@ -75,8 +78,8 @@ func canonModal(sys *System) (*CanonResult, error) { } else { conj := -1 for j := i + 1; j < n; j++ { - if !used[j] && math.Abs(real(vals[i])-real(vals[j])) < 1e-10 && - math.Abs(imag(vals[i])+imag(vals[j])) < 1e-10 { + if !used[j] && math.Abs(real(vals[i])-real(vals[j])) < tol && + math.Abs(imag(vals[i])+imag(vals[j])) < tol { conj = j break } @@ -260,7 +263,7 @@ func characteristicPoly(A *mat.Dense, n int) []float64 { coeffs[0] = 1.0 cur := 1 for _, lam := range vals { - if math.Abs(imag(lam)) < 1e-10 { + if math.Abs(imag(lam)) < 1e-10*math.Max(1, cmplx.Abs(lam)) { r := real(lam) for j := cur; j >= 1; j-- { coeffs[j] = coeffs[j-1] - r*coeffs[j] From 3cd7a32ff4928aa317062781a8930a3ba19cb573 Mon Sep 17 00:00:00 2001 From: James Joseph Date: Fri, 3 Apr 2026 09:52:06 -0500 Subject: [PATCH 8/8] fix: preserve modal conjugate pairs Search for a conjugate partner before collapsing a small-imaginary eigenvalue to a real modal block, add a regression test for the large-real/small-imag pair case, and rename bench_new_test.go to benchmark_hotspots_test.go. Co-authored-by: Copilot <223556219+Copilot@users.noreply.github.com> --- ..._new_test.go => benchmark_hotspots_test.go | 34 +++++++++---------- canon.go | 34 +++++++++---------- canon_test.go | 34 +++++++++++++++++++ 3 files changed, 68 insertions(+), 34 deletions(-) rename bench_new_test.go => benchmark_hotspots_test.go (89%) diff --git a/bench_new_test.go b/benchmark_hotspots_test.go similarity index 89% rename from bench_new_test.go rename to benchmark_hotspots_test.go index f286cc5..cb99472 100644 --- a/bench_new_test.go +++ b/benchmark_hotspots_test.go @@ -7,7 +7,7 @@ import ( "gonum.org/v1/gonum/mat" ) -// benchSysNonSym builds a stable system with non-symmetric A. +// benchSysNonSym builds a stable system with non-symmetric A for benchmark workloads. // A has sub/superdiagonal coupling so transposition bugs surface. func benchSysNonSym(n, m, p int) *System { A := mat.NewDense(n, n, nil) @@ -185,12 +185,12 @@ func benchImpulse(b *testing.B, n, m, p int) { } } -func BenchmarkLsim_SISO_N2(b *testing.B) { benchLsimB(b, 2, 1, 1, 500) } -func BenchmarkLsim_SISO_N10(b *testing.B) { benchLsimB(b, 10, 1, 1, 500) } -func BenchmarkLsim_SISO_N50(b *testing.B) { benchLsimB(b, 50, 1, 1, 500) } -func BenchmarkLsim_MIMO_N10(b *testing.B) { benchLsimB(b, 10, 3, 4, 500) } -func BenchmarkLsim_SISO_1e4(b *testing.B) { benchLsimB(b, 4, 1, 1, 10000) } -func BenchmarkLsim_MIMO_1e4(b *testing.B) { benchLsimB(b, 10, 4, 4, 10000) } +func BenchmarkLsim_SISO_N2(b *testing.B) { benchLsimB(b, 2, 1, 1, 500) } +func BenchmarkLsim_SISO_N10(b *testing.B) { benchLsimB(b, 10, 1, 1, 500) } +func BenchmarkLsim_SISO_N50(b *testing.B) { benchLsimB(b, 50, 1, 1, 500) } +func BenchmarkLsim_MIMO_N10(b *testing.B) { benchLsimB(b, 10, 3, 4, 500) } +func BenchmarkLsim_SISO_1e4(b *testing.B) { benchLsimB(b, 4, 1, 1, 10000) } +func BenchmarkLsim_MIMO_1e4(b *testing.B) { benchLsimB(b, 10, 4, 4, 10000) } func BenchmarkLsim_Discrete_SISO(b *testing.B) { sys := benchSysNonSym(4, 1, 1) @@ -231,7 +231,7 @@ func benchLsimB(b *testing.B, n, m, p, steps int) { // --------------- Loopsens --------------- -func BenchmarkLoopsens_SISO_N2(b *testing.B) { benchLoopsens(b, 2, 1, 1) } +func BenchmarkLoopsens_SISO_N2(b *testing.B) { benchLoopsens(b, 2, 1, 1) } func BenchmarkLoopsens_SISO_N10(b *testing.B) { benchLoopsens(b, 10, 1, 1) } func BenchmarkLoopsens_MIMO_N10(b *testing.B) { benchLoopsens(b, 10, 3, 3) } @@ -246,12 +246,12 @@ func benchLoopsens(b *testing.B, n, m, p int) { // --------------- Transform / decomposition --------------- -func BenchmarkCanon_Modal_N2(b *testing.B) { benchCanon(b, CanonModal, 2) } -func BenchmarkCanon_Modal_N10(b *testing.B) { benchCanon(b, CanonModal, 10) } +func BenchmarkCanon_Modal_N2(b *testing.B) { benchCanon(b, CanonModal, 2) } +func BenchmarkCanon_Modal_N10(b *testing.B) { benchCanon(b, CanonModal, 10) } func BenchmarkCanon_Modal_N50(b *testing.B) { benchCanon(b, CanonModal, 50) } func BenchmarkCanon_Modal_N100(b *testing.B) { benchCanon(b, CanonModal, 100) } -func BenchmarkCanon_Comp_N2(b *testing.B) { benchCanon(b, CanonCompanion, 2) } -func BenchmarkCanon_Comp_N10(b *testing.B) { benchCanon(b, CanonCompanion, 10) } +func BenchmarkCanon_Comp_N2(b *testing.B) { benchCanon(b, CanonCompanion, 2) } +func BenchmarkCanon_Comp_N10(b *testing.B) { benchCanon(b, CanonCompanion, 10) } func benchCanon(b *testing.B, form CanonForm, n int) { sys := benchSysNonSym(n, 1, 1) @@ -261,8 +261,8 @@ func benchCanon(b *testing.B, form CanonForm, n int) { } } -func BenchmarkStabsep_N2(b *testing.B) { benchStabsep(b, 2) } -func BenchmarkStabsep_N10(b *testing.B) { benchStabsep(b, 10) } +func BenchmarkStabsep_N2(b *testing.B) { benchStabsep(b, 2) } +func BenchmarkStabsep_N10(b *testing.B) { benchStabsep(b, 10) } func BenchmarkStabsep_N50(b *testing.B) { benchStabsep(b, 50) } func BenchmarkStabsep_N100(b *testing.B) { benchStabsep(b, 100) } @@ -362,11 +362,11 @@ func benchInvB(b *testing.B, n, m, p int) { } } -func BenchmarkCovar_SISO_N2(b *testing.B) { benchCovarB(b, 2, 1, 1) } -func BenchmarkCovar_SISO_N10(b *testing.B) { benchCovarB(b, 10, 1, 1) } +func BenchmarkCovar_SISO_N2(b *testing.B) { benchCovarB(b, 2, 1, 1) } +func BenchmarkCovar_SISO_N10(b *testing.B) { benchCovarB(b, 10, 1, 1) } func BenchmarkCovar_SISO_N50(b *testing.B) { benchCovarB(b, 50, 1, 1) } func BenchmarkCovar_SISO_N100(b *testing.B) { benchCovarB(b, 100, 1, 1) } -func BenchmarkCovar_MIMO_N10(b *testing.B) { benchCovarB(b, 10, 3, 4) } +func BenchmarkCovar_MIMO_N10(b *testing.B) { benchCovarB(b, 10, 3, 4) } func benchCovarB(b *testing.B, n, m, p int) { sys := benchSysNonSym(n, m, p) diff --git a/canon.go b/canon.go index 4c179c0..f00dea5 100644 --- a/canon.go +++ b/canon.go @@ -66,35 +66,35 @@ func canonModal(sys *System) (*CanonResult, error) { continue } eigMag := cmplx.Abs(vals[i]) - tol := 1e-10 * math.Max(1, eigMag) - if math.Abs(imag(vals[i])) < tol { - blocks = append(blocks, eigBlock{ - mag: math.Abs(real(vals[i])), - real1: real(vals[i]), - imag1: 0, - idx: i, - }) - used[i] = true - } else { - conj := -1 + realTol := 1e-10 * math.Max(1, eigMag) + conj := -1 + if imag(vals[i]) != 0 { for j := i + 1; j < n; j++ { - if !used[j] && math.Abs(real(vals[i])-real(vals[j])) < tol && - math.Abs(imag(vals[i])+imag(vals[j])) < tol { + if !used[j] && isConjugate(vals[i], vals[j]) { conj = j break } } - if conj < 0 { - return nil, fmt.Errorf("controlsys: complex eigenvalue without conjugate pair") - } + } + if conj >= 0 { blocks = append(blocks, eigBlock{ - mag: math.Sqrt(real(vals[i])*real(vals[i]) + imag(vals[i])*imag(vals[i])), + mag: eigMag, real1: real(vals[i]), imag1: math.Abs(imag(vals[i])), idx: i, }) used[i] = true used[conj] = true + } else if math.Abs(imag(vals[i])) < realTol { + blocks = append(blocks, eigBlock{ + mag: math.Abs(real(vals[i])), + real1: real(vals[i]), + imag1: 0, + idx: i, + }) + used[i] = true + } else { + return nil, fmt.Errorf("controlsys: complex eigenvalue without conjugate pair") } } diff --git a/canon_test.go b/canon_test.go index 8f96a45..3e70986 100644 --- a/canon_test.go +++ b/canon_test.go @@ -80,6 +80,40 @@ func TestCanonModal_ComplexEigenvalues(t *testing.T) { checkEigsPreserved(t, sys, res.Sys, 1e-10) } +func TestCanonModal_LargeRealSmallImagPair(t *testing.T) { + const ( + a = 1e4 + b = 1e-7 + ) + sys, err := New( + mat.NewDense(2, 2, []float64{a, b, -b, a}), + mat.NewDense(2, 1, []float64{0, 1}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Canon(sys, CanonModal) + if err != nil { + t.Fatal(err) + } + + Amod := res.Sys.A + if math.Abs(Amod.At(0, 0)-a) > 1e-6 || math.Abs(Amod.At(1, 1)-a) > 1e-6 { + t.Errorf("diagonal = [%g, %g], want [%g, %g]", Amod.At(0, 0), Amod.At(1, 1), a, a) + } + if math.Abs(Amod.At(0, 1)) < 1e-10 || math.Abs(Amod.At(1, 0)) < 1e-10 { + t.Fatalf("expected 2x2 modal block, got %v", mat.Formatted(Amod)) + } + if Amod.At(0, 1)*Amod.At(1, 0) > 0 { + t.Errorf("off-diagonal should have opposite signs") + } + + checkEigsPreserved(t, sys, res.Sys, 1e-10) + checkFreqPreserved(t, sys, res.Sys, 1e-8) +} + func TestCanonModal_Mixed(t *testing.T) { sys, err := New( mat.NewDense(3, 3, []float64{-1, 0, 0, 0, 0, 1, 0, -4, 0}),