Optimize complex frequency-domain solves

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 {

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

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

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 {

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
 }