From d2779cd43bdf36cf38d783967daa333c94e1ccab Mon Sep 17 00:00:00 2001 From: James Joseph Date: Sat, 18 Jul 2026 18:26:02 -0500 Subject: [PATCH] Optimize complex frequency-domain solves --- benchmark_hotspots_test.go | 15 +++- descriptor_test.go | 53 +++++++++++++ frd.go | 28 ++++--- frd_test.go | 41 ++++++++++ frequency.go | 151 ++++++++++++++++++++++++++++++++----- 5 files changed, 259 insertions(+), 29 deletions(-) diff --git a/benchmark_hotspots_test.go b/benchmark_hotspots_test.go index aa01fec..e65f775 100644 --- a/benchmark_hotspots_test.go +++ b/benchmark_hotspots_test.go @@ -228,6 +228,9 @@ func BenchmarkFRDFeedback_SISO_2000(b *testing.B) { benchFRDFeedback(b, 2, 1, 1 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 BenchmarkFRDFeedback_MIMO8x8_2000(b *testing.B) { + benchFRDFeedback(b, 20, 8, 8, 2000) +} func benchFRDFeedback(b *testing.B, n, m, p, nw int) { plant := benchSysNonSym(n, m, p) @@ -458,8 +461,16 @@ func BenchmarkDescriptorToExplicit_N10(b *testing.B) { } func BenchmarkDescriptorFreqResponse_MIMO_N10x200(b *testing.B) { - sys := benchDescriptorSystem(b, 10, 3, 3) - omega := logspace(-2, 3, 200) + benchDescriptorFreqResponse(b, 10, 3, 3, 200) +} + +func BenchmarkDescriptorFreqResponse_MIMO_N50x200(b *testing.B) { + benchDescriptorFreqResponse(b, 50, 5, 5, 200) +} + +func benchDescriptorFreqResponse(b *testing.B, n, m, p, nw int) { + sys := benchDescriptorSystem(b, n, m, p) + omega := logspace(-2, 3, nw) b.ResetTimer() for i := 0; i < b.N; i++ { if _, err := sys.FreqResponse(omega); err != nil { diff --git a/descriptor_test.go b/descriptor_test.go index e26ef80..d5edddb 100644 --- a/descriptor_test.go +++ b/descriptor_test.go @@ -200,6 +200,59 @@ func TestDescriptorSystem_FrequencyResponseUsesDescriptorPencil(t *testing.T) { } } +func TestDescriptorSystem_FrequencyResponsePivotsNonSymmetricPencil(t *testing.T) { + a := mat.NewDense(2, 2, []float64{0, -2, 3, -1}) + e := mat.NewDense(2, 2, []float64{0, 1, 1, 0.5}) + b := mat.NewDense(2, 2, []float64{1, -1, 2, 0.5}) + c := mat.NewDense(2, 2, []float64{1, 0.25, -0.5, 2}) + d := mat.NewDense(2, 2, []float64{0.1, 0.2, -0.3, 0.4}) + sys, err := NewDescriptor(a, b, c, d, e, 0) + if err != nil { + t.Fatal(err) + } + + s := complex(0, 1) + pencil := [4]complex128{ + s*complex(e.At(0, 0), 0) - complex(a.At(0, 0), 0), + s*complex(e.At(0, 1), 0) - complex(a.At(0, 1), 0), + s*complex(e.At(1, 0), 0) - complex(a.At(1, 0), 0), + s*complex(e.At(1, 1), 0) - complex(a.At(1, 1), 0), + } + det := pencil[0]*pencil[3] - pencil[1]*pencil[2] + inv := [4]complex128{pencil[3] / det, -pencil[1] / det, -pencil[2] / det, pencil[0] / det} + want := make([][]complex128, 2) + for i := range 2 { + want[i] = make([]complex128, 2) + for j := range 2 { + for k := range 2 { + for l := range 2 { + want[i][j] += complex(c.At(i, k), 0) * inv[k*2+l] * complex(b.At(l, j), 0) + } + } + want[i][j] += complex(d.At(i, j), 0) + } + } + + got, err := sys.EvalFr(s) + if err != nil { + t.Fatalf("EvalFr: %v", err) + } + response, err := sys.FreqResponse([]float64{1}) + if err != nil { + t.Fatalf("FreqResponse: %v", err) + } + for i := range 2 { + for j := range 2 { + if diff := cmplx.Abs(got[i][j] - want[i][j]); diff > 1e-12 { + t.Fatalf("EvalFr(%d,%d) diff=%g: got %v, want %v", i, j, diff, got[i][j], want[i][j]) + } + if diff := cmplx.Abs(response.At(0, i, j) - want[i][j]); diff > 1e-12 { + t.Fatalf("FreqResponse(%d,%d) diff=%g: got %v, want %v", i, j, diff, response.At(0, i, j), want[i][j]) + } + } + } +} + func TestDescriptorSystem_IdentityDescriptorUsesStandardOperations(t *testing.T) { sys, err := New( mat.NewDense(2, 2, []float64{-1, 2, 0.5, -3}), diff --git a/frd.go b/frd.go index b315a1d..ed736b6 100644 --- a/frd.go +++ b/frd.go @@ -700,18 +700,28 @@ func FRDFeedback(plant, controller *FRD, sign float64) (*FRD, error) { 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 := range ws.n { + for j := range ws.n { + ws.ipkg[j*ws.n+i] = complex(-sign, 0) * ws.kg[i*ws.n+j] + } } for i := 0; i < ws.n; i++ { ws.ipkg[i*ws.n+i] += 1 } - - err := cInvertInto(ws.inv, ws.aug, ws.ipkg, ws.n) - if err != nil { + for i := range pp { + for j := range ws.n { + ws.rhs[j*pp+i] = ws.g[i*ws.n+j] + } + } + if err := cSolveInPlace(ws.ipkg, ws.rhs, ws.n, pp); err != nil { return nil, fmt.Errorf("frd feedback: singular at freq index %d", w) } - cMulInto(data[w*pp*pm:(w+1)*pp*pm], ws.g, ws.inv, pp, ws.n, pm) + dst := data[w*pp*pm : (w+1)*pp*pm] + for i := range pp { + for j := range ws.n { + dst[i*ws.n+j] = ws.rhs[j*pp+i] + } + } } return result, nil @@ -722,8 +732,7 @@ type frdFeedbackWorkspace struct { k []complex128 kg []complex128 ipkg []complex128 - inv []complex128 - aug []complex128 + rhs []complex128 n int } @@ -734,8 +743,7 @@ func newFRDFeedbackWorkspace(pp, pm int) *frdFeedbackWorkspace { 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), + rhs: make([]complex128, n*pp), n: n, } } diff --git a/frd_test.go b/frd_test.go index 9a9ff3d..5396b62 100644 --- a/frd_test.go +++ b/frd_test.go @@ -715,6 +715,47 @@ func TestFRDFeedback_MIMO(t *testing.T) { } } +func TestFRDFeedback_CoupledRectangularMIMO(t *testing.T) { + plant, err := NewFRD([][][]complex128{{ + {1 + 0.2i, -0.5i, 0.25}, + {0.3 - 0.1i, 2, -0.4 + 0.6i}, + }}, []float64{1}, 0) + if err != nil { + t.Fatal(err) + } + controller, err := NewFRD([][][]complex128{{ + {0.5, -0.1 + 0.2i}, + {0.3i, 0.4}, + {-0.2, 0.1 - 0.1i}, + }}, []float64{1}, 0) + if err != nil { + t.Fatal(err) + } + + got, err := FRDFeedback(plant, controller, -1) + if err != nil { + t.Fatal(err) + } + for i := range 2 { + for j := range 3 { + var product complex128 + for l := range 3 { + m := complex(0, 0) + if l == j { + m = 1 + } + for k := range 2 { + m += controller.At(0, l, k) * plant.At(0, k, j) + } + product += got.At(0, i, l) * m + } + if diff := cmplx.Abs(product - plant.At(0, i, j)); diff > 1e-12 { + t.Fatalf("closed-loop residual (%d,%d) = %g", i, j, diff) + } + } + } +} + func TestFRDFeedback_Singular(t *testing.T) { plant, err := NewFRD([][][]complex128{{{1}}}, []float64{1}, 0) if err != nil { diff --git a/frequency.go b/frequency.go index f1f0e70..8c56c2d 100644 --- a/frequency.go +++ b/frequency.go @@ -355,20 +355,16 @@ func applyIODelayMatrixAtS(sys *System, s complex128, data []complex128, p, m in } type ssEvalWorkspace struct { - sIA []complex128 - resolvent []complex128 - invBuf []complex128 - temp []complex128 - g []complex128 + pencil []complex128 + rhs []complex128 + g []complex128 } func newSSEvalWorkspace(n, p, m int) *ssEvalWorkspace { return &ssEvalWorkspace{ - sIA: make([]complex128, n*n), - resolvent: make([]complex128, n*n), - invBuf: make([]complex128, n*2*n), - temp: make([]complex128, n*max(m, p)), - g: make([]complex128, p*m), + pencil: make([]complex128, n*n), + rhs: make([]complex128, n*m), + g: make([]complex128, p*m), } } @@ -380,24 +376,145 @@ func evalFrSSInto(ws *ssEvalWorkspace, sys *System, s complex128, n, p, m int) e dData, dStride = dRaw.Data, dRaw.Stride } if n == 0 { - cComputeHInto(ws.g, ws.temp, ws.resolvent, nil, 0, nil, 0, dData, dStride, n, p, m) + copyRealMatrixToComplex(ws.g, dData, dStride, p, m) return nil } aRaw := sys.A.RawMatrix() - var err error if sys.E == nil { - err = cResolventInto(ws.resolvent, ws.sIA, ws.invBuf, aRaw.Data, aRaw.Stride, s, n) + fillComplexPencil(ws.pencil, aRaw.Data, aRaw.Stride, nil, 0, s, n) } else { eRaw := sys.E.RawMatrix() - err = cDescriptorResolventInto(ws.resolvent, ws.sIA, ws.invBuf, aRaw.Data, aRaw.Stride, eRaw.Data, eRaw.Stride, s, n) + fillComplexPencil(ws.pencil, aRaw.Data, aRaw.Stride, eRaw.Data, eRaw.Stride, s, n) } - if err != nil { + bRaw := sys.B.RawMatrix() + copyRealMatrixToComplex(ws.rhs, bRaw.Data, bRaw.Stride, n, m) + if err := cSolveInPlace(ws.pencil, ws.rhs, n, m); err != nil { return err } - bRaw := sys.B.RawMatrix() cRaw := sys.C.RawMatrix() - cComputeHInto(ws.g, ws.temp, ws.resolvent, cRaw.Data, cRaw.Stride, bRaw.Data, bRaw.Stride, dData, dStride, n, p, m) + cRealMulComplexInto(ws.g, cRaw.Data, cRaw.Stride, ws.rhs, dData, dStride, n, p, m) + return nil +} + +func fillComplexPencil(dst []complex128, a []float64, aStride int, e []float64, eStride int, s complex128, n int) { + for i := range n { + row := i * n + aRow := i * aStride + if e == nil { + for j := range n { + dst[row+j] = -complex(a[aRow+j], 0) + } + dst[row+i] += s + continue + } + eRow := i * eStride + for j := range n { + dst[row+j] = s*complex(e[eRow+j], 0) - complex(a[aRow+j], 0) + } + } +} + +func copyRealMatrixToComplex(dst []complex128, src []float64, stride, rows, cols int) { + if src == nil { + clear(dst[:rows*cols]) + return + } + for i := range rows { + for j := range cols { + dst[i*cols+j] = complex(src[i*stride+j], 0) + } + } +} + +func cRealMulComplexInto(dst []complex128, a []float64, aStride int, b []complex128, d []float64, dStride, inner, rows, cols int) { + for i := range rows { + for j := range cols { + var sum complex128 + for k := range inner { + sum += complex(a[i*aStride+k], 0) * b[k*cols+j] + } + if d != nil { + sum += complex(d[i*dStride+j], 0) + } + dst[i*cols+j] = sum + } + } +} + +func cSolveInPlace(a, b []complex128, n, nrhs int) error { + if n == 1 { + if a[0] == 0 { + return fmt.Errorf("controlsys: singular complex matrix: %w", ErrSingularTransform) + } + for j := range nrhs { + b[j] /= a[0] + } + return nil + } + + maxAbs := 0.0 + for _, v := range a[:n*n] { + if av := cmplx.Abs(v); av > maxAbs { + maxAbs = av + } + } + tol := float64(n) * maxAbs * eps() + if tol == 0 { + tol = 1e-15 + } + + for k := range n { + pivot := k + best := cmplx.Abs(a[k*n+k]) + for i := k + 1; i < n; i++ { + if candidate := cmplx.Abs(a[i*n+k]); candidate > best { + best = candidate + pivot = i + } + } + if best < tol { + return fmt.Errorf("controlsys: singular complex matrix: %w", ErrSingularTransform) + } + if pivot != k { + for j := range n { + a[k*n+j], a[pivot*n+j] = a[pivot*n+j], a[k*n+j] + } + for j := range nrhs { + b[k*nrhs+j], b[pivot*nrhs+j] = b[pivot*nrhs+j], b[k*nrhs+j] + } + } + + akk := a[k*n+k] + for i := k + 1; i < n; i++ { + factor := a[i*n+k] / akk + a[i*n+k] = factor + for j := k + 1; j < n; j++ { + a[i*n+j] -= factor * a[k*n+j] + } + } + } + + for i := 1; i < n; i++ { + for k := 0; k < i; k++ { + factor := a[i*n+k] + for j := range nrhs { + b[i*nrhs+j] -= factor * b[k*nrhs+j] + } + } + } + for i := n - 1; i >= 0; i-- { + for k := i + 1; k < n; k++ { + factor := a[i*n+k] + for j := range nrhs { + b[i*nrhs+j] -= factor * b[k*nrhs+j] + } + } + diag := a[i*n+i] + for j := range nrhs { + b[i*nrhs+j] /= diag + } + } return nil }