diff --git a/canon.go b/canon.go index f00dea5..498a2ae 100644 --- a/canon.go +++ b/canon.go @@ -6,6 +6,9 @@ import ( "math/cmplx" "sort" + "gonum.org/v1/gonum/blas" + "gonum.org/v1/gonum/blas/blas64" + "gonum.org/v1/gonum/lapack" "gonum.org/v1/gonum/mat" ) @@ -49,6 +52,16 @@ func canonModal(sys *System) (*CanonResult, error) { return &CanonResult{Sys: cp, T: &mat.Dense{}}, nil } + res, err := canonModalEig(sys, n, m, p) + if err == nil { + return res, nil + } + return canonModalSchur(sys, n, m, p) +} + +// canonModalEig attempts the eigendecomposition-based modal form. +// Returns error if eigenvector matrix is too ill-conditioned. +func canonModalEig(sys *System, n, m, p int) (*CanonResult, error) { var eig mat.Eigen ok := eig.Factorize(sys.A, mat.EigenRight) if !ok { @@ -124,26 +137,25 @@ func canonModal(sys *System) (*CanonResult, error) { 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) + cond := lu.Cond() + if math.IsNaN(cond) || math.IsInf(cond, 1) || cond > 1e12 { + return nil, fmt.Errorf("controlsys: eigenvector matrix ill-conditioned (κ=%.1e)", cond) } + // Anew = T⁻¹*A*T: compute AT = A*T, then solve T*Anew = AT + AT := mat.NewDense(n, n, nil) + AT.Mul(sys.A, T) Anew := mat.NewDense(n, n, nil) - tmp := mat.NewDense(n, n, nil) - tmp.Mul(Tinv, sys.A) - Anew.Mul(tmp, T) + if err := lu.SolveTo(Anew, false, AT); err != nil { + return nil, fmt.Errorf("controlsys: solve failed: %w", ErrSingularTransform) + } + // Bnew = T⁻¹*B: solve T*Bnew = B Bnew := mat.NewDense(n, m, nil) - Bnew.Mul(Tinv, sys.B) + if err := lu.SolveTo(Bnew, false, sys.B); err != nil { + return nil, fmt.Errorf("controlsys: solve failed: %w", ErrSingularTransform) + } Cnew := mat.NewDense(p, n, nil) Cnew.Mul(sys.C, T) @@ -159,6 +171,107 @@ func canonModal(sys *System) (*CanonResult, error) { return &CanonResult{Sys: newSys, T: T}, nil } +// canonModalSchur computes a quasi-modal form via ordered real Schur decomposition. +// Numerically stable even for near-repeated eigenvalues. +// A_modal is quasi-upper-triangular: 1×1 blocks for real eigenvalues, +// 2×2 blocks for complex pairs, ordered by eigenvalue magnitude. +func canonModalSchur(sys *System, n, m, p int) (*CanonResult, error) { + aRaw := sys.A.RawMatrix() + + t := make([]float64, n*n) + 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, fmt.Errorf("controlsys: Schur decomposition failed: %w", ErrSchurFailed) + } + + // Sort Schur blocks by eigenvalue magnitude (ascending) + schurSortByMagnitude(t, z, n) + + bRaw := sys.B.RawMatrix() + cRaw := sys.C.RawMatrix() + + zGen := blas64.General{Rows: n, Cols: n, Stride: n, Data: z} + + // B_modal = Z' * B + bNew := 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: bNew}) + + // C_modal = C * Z + cNew := 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: cNew}) + + Anew := mat.NewDense(n, n, t) + Bnew := mat.NewDense(n, m, bNew) + Cnew := mat.NewDense(p, n, cNew) + Dnew := denseCopy(sys.D) + + newSys, err := newNoCopy(Anew, Bnew, Cnew, Dnew, sys.Dt) + if err != nil { + return nil, err + } + propagateIONames(newSys, sys) + + T := mat.NewDense(n, n, z) + return &CanonResult{Sys: newSys, T: T}, nil +} + +// schurSortByMagnitude reorders Schur blocks by eigenvalue magnitude (ascending). +func schurSortByMagnitude(t, z []float64, n int) { + trexcWork := make([]float64, n) + nPlaced := 0 + for nPlaced < n { + evals := schurEigenvaluesRaw(t, n) + + bestIdx := nPlaced + bestMag := cmplx.Abs(evals[nPlaced]) + i := nPlaced + for i < n { + blockSize := 1 + if i+1 < n && t[(i+1)*n+i] != 0 { + blockSize = 2 + } + mag := cmplx.Abs(evals[i]) + if mag < bestMag { + bestMag = mag + bestIdx = i + } + i += blockSize + } + + if bestIdx != nPlaced { + _, _, ok := impl.Dtrexc(lapack.UpdateSchur, n, t, n, z, n, bestIdx, nPlaced, trexcWork) + if !ok { + break + } + } + + if nPlaced+1 < n && t[(nPlaced+1)*n+nPlaced] != 0 { + nPlaced += 2 + } else { + nPlaced++ + } + } +} + func canonCompanion(sys *System) (*CanonResult, error) { n, m, p := sys.Dims() if n == 0 { @@ -212,22 +325,19 @@ func canonCompanion(sys *System) (*CanonResult, error) { 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 = T⁻¹*A*T: compute AT = A*T, then solve T*Anew = AT + AT := mat.NewDense(n, n, nil) + AT.Mul(sys.A, T) Anew := mat.NewDense(n, n, nil) - tmp := mat.NewDense(n, n, nil) - tmp.Mul(Tinv, sys.A) - Anew.Mul(tmp, T) + if err := luT.SolveTo(Anew, false, AT); err != nil { + return nil, fmt.Errorf("controlsys: solve failed: %w", ErrSingularTransform) + } + // Bnew = T⁻¹*B: solve T*Bnew = B Bnew := mat.NewDense(n, 1, nil) - Bnew.Mul(Tinv, sys.B) + if err := luT.SolveTo(Bnew, false, sys.B); err != nil { + return nil, fmt.Errorf("controlsys: solve failed: %w", ErrSingularTransform) + } Cnew := mat.NewDense(1, n, nil) Cnew.Mul(sys.C, T) diff --git a/convert.go b/convert.go index a84fd99..f7698e8 100644 --- a/convert.go +++ b/convert.go @@ -1411,7 +1411,7 @@ func matchedGainFallback(num, den Poly, discZeros, discPoles []complex128, dt fl if cmplx.Abs(discVal) < 1e-14 { return 0, fmt.Errorf("DiscretizeMatched: cannot determine gain: %w", ErrSingularTransform) } - return cmplx.Abs(contVal) / cmplx.Abs(discVal), nil + return real(contVal / discVal), nil } func (sys *System) D2D(newDt float64, opts C2DOptions) (*System, error) { diff --git a/ctrbobsv.go b/ctrbobsv.go index 0f2817b..bd5c55f 100644 --- a/ctrbobsv.go +++ b/ctrbobsv.go @@ -1,6 +1,7 @@ package controlsys import ( + "math" "math/cmplx" "gonum.org/v1/gonum/blas" @@ -193,12 +194,13 @@ func IsStabilizable(A, B *mat.Dense, continuous bool) (bool, error) { vals := eig.Values(nil) for _, v := range vals { + tol := 1e-10 * math.Max(1, cmplx.Abs(v)) if continuous { - if real(v) >= 0 { + if real(v) >= -tol { return false, nil } } else { - if cmplx.Abs(v) >= 1-1e-10 { + if cmplx.Abs(v) >= 1-tol { return false, nil } } diff --git a/errors.go b/errors.go index dde128f..5378fde 100644 --- a/errors.go +++ b/errors.go @@ -9,6 +9,7 @@ var ( ErrInvalidSampleTime = errors.New("controlsys: sample time must be positive") ErrSingularDenom = errors.New("controlsys: zero or near-zero leading denominator") ErrOverflow = errors.New("controlsys: coefficient overflow") + ErrImproperTF = errors.New("controlsys: transfer function is improper (numerator degree exceeds denominator degree)") ErrNegativeDelay = errors.New("controlsys: delay must be non-negative") ErrFractionalDelay = errors.New("controlsys: discrete delay must be non-negative integer") @@ -23,6 +24,7 @@ var ( ErrMixedDelayTypes = errors.New("controlsys: InternalDelay and IODelay cannot coexist") ErrNotSymmetric = errors.New("controlsys: matrix must be symmetric") + ErrNotPSD = errors.New("controlsys: Q matrix must be positive semi-definite") ErrSchurFailed = errors.New("controlsys: Schur decomposition failed to converge") ErrSingularEquation = errors.New("controlsys: matrix equation is singular or nearly singular") ErrNoStabilizing = errors.New("controlsys: no stabilizing solution exists") @@ -48,4 +50,5 @@ var ( ErrH2DirectFeedthrough = errors.New("controlsys: H2 synthesis requires D22 = 0") ErrGammaNotAchievable = errors.New("controlsys: no stabilizing controller exists for given gamma") ErrDescriptorSingular = errors.New("controlsys: descriptor matrix E is singular") + ErrDescriptorRiccati = errors.New("controlsys: standard Riccati solvers do not support descriptor systems (E != I)") ) diff --git a/h2syn.go b/h2syn.go index f25f648..f1c9734 100644 --- a/h2syn.go +++ b/h2syn.go @@ -17,6 +17,9 @@ func H2Syn(P *System, nmeas, ncont int) (*H2SynResult, error) { if !P.IsContinuous() { return nil, ErrWrongDomain } + if P.IsDescriptor() { + return nil, ErrDescriptorRiccati + } n, m, p := P.Dims() if n == 0 { diff --git a/hardening_test.go b/hardening_test.go new file mode 100644 index 0000000..f3bde6a --- /dev/null +++ b/hardening_test.go @@ -0,0 +1,135 @@ +package controlsys + +import ( + "errors" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestStateSpace_RejectsImproperTF(t *testing.T) { + tf := &TransferFunc{ + Num: [][][]float64{{{1, 0}}}, // s (degree 1) + Den: [][]float64{{1}}, // 1 (degree 0) + } + _, err := tf.StateSpace(nil) + if !errors.Is(err, ErrImproperTF) { + t.Errorf("expected ErrImproperTF, got %v", err) + } +} + +func TestStateSpace_AcceptsProperTF(t *testing.T) { + tf := &TransferFunc{ + Num: [][][]float64{{{1}}}, + Den: [][]float64{{1, 1}}, + } + _, err := tf.StateSpace(nil) + if err != nil { + t.Errorf("proper TF should be accepted, got %v", err) + } +} + +func TestStateSpace_AcceptsBiproperTF(t *testing.T) { + tf := &TransferFunc{ + Num: [][][]float64{{{1, 1}}}, + Den: [][]float64{{1, 2}}, + } + _, err := tf.StateSpace(nil) + if err != nil { + t.Errorf("biproper TF (equal degree) should be accepted, got %v", err) + } +} + +func TestCare_RejectsNegativeDefiniteQ(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, -2, -3}) + B := mat.NewDense(2, 1, []float64{0, 1}) + Q := mat.NewDense(2, 2, []float64{-1, 0, 0, -1}) + R := mat.NewDense(1, 1, []float64{1}) + + _, err := Care(A, B, Q, R, nil) + if !errors.Is(err, ErrNotPSD) { + t.Errorf("expected ErrNotPSD for negative definite Q, got %v", err) + } +} + +func TestCare_RejectsIndefiniteQ(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, -2, -3}) + B := mat.NewDense(2, 1, []float64{0, 1}) + Q := mat.NewDense(2, 2, []float64{1, 0, 0, -1}) + R := mat.NewDense(1, 1, []float64{1}) + + _, err := Care(A, B, Q, R, nil) + if !errors.Is(err, ErrNotPSD) { + t.Errorf("expected ErrNotPSD for indefinite Q, got %v", err) + } +} + +func TestCare_AcceptsPSDQ(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, -2, -3}) + B := mat.NewDense(2, 1, []float64{0, 1}) + Q := mat.NewDense(2, 2, []float64{1, 0, 0, 0}) // PSD but singular + R := mat.NewDense(1, 1, []float64{1}) + + _, err := Care(A, B, Q, R, nil) + if err != nil { + t.Errorf("PSD Q should be accepted, got %v", err) + } +} + +func TestDare_RejectsNegativeDefiniteQ(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0.9, 0.1, 0, 0.8}) + B := mat.NewDense(2, 1, []float64{0, 1}) + Q := mat.NewDense(2, 2, []float64{-1, 0, 0, -1}) + R := mat.NewDense(1, 1, []float64{1}) + + _, err := Dare(A, B, Q, R, nil) + if !errors.Is(err, ErrNotPSD) { + t.Errorf("expected ErrNotPSD for negative definite Q, got %v", err) + } +} + +func TestCare_RejectsNonPDR(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, -2, -3}) + B := mat.NewDense(2, 1, []float64{0, 1}) + Q := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + R := mat.NewDense(1, 1, []float64{-1}) + + _, err := Care(A, B, Q, R, nil) + if !errors.Is(err, ErrSingularR) { + t.Errorf("expected ErrSingularR for non-PD R, got %v", err) + } +} + +func TestKalman_RejectsDescriptor(t *testing.T) { + sys, _ := New( + mat.NewDense(2, 2, []float64{-1, 0, 0, -2}), + mat.NewDense(2, 1, []float64{1, 0}), + mat.NewDense(1, 2, []float64{1, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + sys.E = mat.NewDense(2, 2, []float64{1, 0, 0, 2}) + + Qn := mat.NewDense(1, 1, []float64{1}) + Rn := mat.NewDense(1, 1, []float64{1}) + _, err := Kalman(sys, Qn, Rn, nil) + if !errors.Is(err, ErrDescriptorRiccati) { + t.Errorf("expected ErrDescriptorRiccati, got %v", err) + } +} + +func TestLqg_RejectsDescriptor(t *testing.T) { + sys, _ := New( + mat.NewDense(2, 2, []float64{-1, 0, 0, -2}), + mat.NewDense(2, 1, []float64{1, 0}), + mat.NewDense(1, 2, []float64{1, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + sys.E = mat.NewDense(2, 2, []float64{1, 0, 0, 2}) + + Q := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + R := mat.NewDense(1, 1, []float64{1}) + Qn := mat.NewDense(1, 1, []float64{1}) + Rn := mat.NewDense(1, 1, []float64{1}) + _, err := Lqg(sys, Q, R, Qn, Rn, nil) + if !errors.Is(err, ErrDescriptorRiccati) { + t.Errorf("expected ErrDescriptorRiccati, got %v", err) + } +} diff --git a/hinfsyn.go b/hinfsyn.go index 3d2672a..8251544 100644 --- a/hinfsyn.go +++ b/hinfsyn.go @@ -20,6 +20,9 @@ func HinfSyn(P *System, nmeas, ncont int) (*HinfSynResult, error) { if !P.IsContinuous() { return nil, ErrWrongDomain } + if P.IsDescriptor() { + return nil, ErrDescriptorRiccati + } n, m, p := P.Dims() if n == 0 { diff --git a/lqg.go b/lqg.go index 62e15d8..3e47fa1 100644 --- a/lqg.go +++ b/lqg.go @@ -22,6 +22,9 @@ func Lqg(sys *System, Q, R, Qn, Rn *mat.Dense, opts *RiccatiOpts) (*LqgResult, e if n == 0 { return nil, fmt.Errorf("Lqg: system has no states: %w", ErrDimensionMismatch) } + if sys.IsDescriptor() { + return nil, fmt.Errorf("Lqg: %w", ErrDescriptorRiccati) + } var kRes *RiccatiResult var err error diff --git a/matutil.go b/matutil.go index 3690d79..4b61585 100644 --- a/matutil.go +++ b/matutil.go @@ -3,6 +3,7 @@ package controlsys import ( "math" + "gonum.org/v1/gonum/blas" "gonum.org/v1/gonum/mat" ) @@ -89,6 +90,25 @@ func isSymmetric(m *mat.Dense, tol float64) bool { return true } +// isPSD checks if a symmetric matrix is positive semi-definite +// by attempting Cholesky on Q + tol*I. A small shift avoids +// rejecting borderline-zero eigenvalues from roundoff. +func isPSD(m *mat.Dense) bool { + n, _ := m.Dims() + if n == 0 { + return true + } + raw := m.RawMatrix() + tmp := make([]float64, n*n) + copyStrided(tmp, n, raw.Data, raw.Stride, n, n) + symmetrize(tmp, n, n) + tol := eps() * denseNorm(m) * float64(n) + for i := 0; i < n; i++ { + tmp[i*n+i] += tol + } + return impl.Dpotrf(blas.Lower, n, tmp, n) +} + func symmetrize(data []float64, n, stride int) { for i := 0; i < n; i++ { for j := i + 1; j < n; j++ { diff --git a/observer.go b/observer.go index a01375e..de3746d 100644 --- a/observer.go +++ b/observer.go @@ -97,6 +97,9 @@ func Kalman(sys *System, Qn, Rn *mat.Dense, opts *RiccatiOpts) (*RiccatiResult, if n == 0 { return nil, fmt.Errorf("Kalman: system has no states: %w", ErrDimensionMismatch) } + if sys.IsDescriptor() { + return nil, fmt.Errorf("Kalman: %w", ErrDescriptorRiccati) + } qr, qc := Qn.Dims() if qr != m || qc != m { return nil, ErrDimensionMismatch diff --git a/riccati.go b/riccati.go index 1b539c1..1e186c2 100644 --- a/riccati.go +++ b/riccati.go @@ -120,6 +120,9 @@ func Care(A, B, Q, R *mat.Dense, opts *RiccatiOpts) (*RiccatiResult, error) { if !isSymmetric(R, eps()*denseNorm(R)) { return nil, ErrNotSymmetric } + if !isPSD(Q) { + return nil, ErrNotPSD + } var S *mat.Dense if opts != nil && opts.S != nil { @@ -328,6 +331,9 @@ func Dare(A, B, Q, R *mat.Dense, opts *RiccatiOpts) (*RiccatiResult, error) { if !isSymmetric(R, eps()*denseNorm(R)) { return nil, ErrNotSymmetric } + if !isPSD(Q) { + return nil, ErrNotPSD + } var S *mat.Dense if opts != nil && opts.S != nil { diff --git a/transfer.go b/transfer.go index 57beef2..388ea00 100644 --- a/transfer.go +++ b/transfer.go @@ -484,6 +484,10 @@ func (tf *TransferFunc) StateSpace(opts *StateSpaceOpts) (*StateSpaceResult, err return &StateSpaceResult{Sys: sys, MinimalOrder: 0}, nil } + if !tf.Isproper() { + return nil, ErrImproperTF + } + degrees := make([]int, p) totalN := 0 for i := 0; i < p; i++ {