Improve numerical architecture and performance

bench_test.go

@@ -273,6 +273,53 @@ func BenchmarkFreqResponse(b *testing.B) {
 	}
 }
 
+func BenchmarkFreqResponse_ShortSweep(b *testing.B) {
+	sys := benchSys(10, 2, 3)
+	omega := logspace(-2, 2, 8)
+	b.ResetTimer()
+	for b.Loop() {
+		sys.FreqResponse(omega)
+	}
+}
+
+func BenchmarkFrequencySweepKernels(b *testing.B) {
+	tests := []struct {
+		name        string
+		n, m, p, nw int
+	}{
+		{name: "N4_W2", n: 4, m: 2, p: 2, nw: 2},
+		{name: "N4_W8", n: 4, m: 2, p: 2, nw: 8},
+		{name: "N4_W32", n: 4, m: 2, p: 2, nw: 32},
+		{name: "N4_W100", n: 4, m: 2, p: 2, nw: 100},
+		{name: "N10_W2", n: 10, m: 2, p: 3, nw: 2},
+		{name: "N10_W8", n: 10, m: 2, p: 3, nw: 8},
+		{name: "N10_W32", n: 10, m: 2, p: 3, nw: 32},
+		{name: "N10_W100", n: 10, m: 2, p: 3, nw: 100},
+	}
+	for _, test := range tests {
+		sys := benchSys(test.n, test.m, test.p)
+		evaluator := newFrequencyEvaluator(sys)
+		omega := logspace(-2, 2, test.nw)
+		size := test.nw * test.p * test.m
+		b.Run(test.name+"/StateSpace", func(b *testing.B) {
+			for b.Loop() {
+				data := make([]complex128, size)
+				if err := evaluator.evalStateSpaceSweepInto(omega, data); err != nil {
+					b.Fatal(err)
+				}
+			}
+		})
+		b.Run(test.name+"/TransferFunction", func(b *testing.B) {
+			for b.Loop() {
+				data := make([]complex128, size)
+				if err := evaluator.evalTransferFunctionSweepInto(omega, data); err != nil {
+					b.Fatal(err)
+				}
+			}
+		})
+	}
+}
+
 func BenchmarkBode(b *testing.B) {
 	sys := benchSys(10, 2, 3)
 	b.ResetTimer()

frequency.go

@@ -144,6 +144,8 @@ type frequencyEvaluator struct {
 	p   int
 }
 
+const directFrequencySweepWorkLimit = 80
+
 func newFrequencyEvaluator(sys *System) frequencyEvaluator {
 	n, m, p := sys.Dims()
 	return frequencyEvaluator{sys: sys, n: n, m: m, p: p}
@@ -171,33 +173,21 @@ func (e frequencyEvaluator) response(omega []float64) (*FreqResponseMatrix, erro
 	pm := e.p * e.m
 	data := make([]complex128, nw*pm)
 	if e.sys.IsDescriptor() {
-		ws := newSSEvalWorkspace(e.n, e.p, e.m)
-		for k, w := range omega {
-			s := e.sAt(w)
-			if err := evalFrSSInto(ws, e.sys, s, e.n, e.p, e.m); err != nil {
-				return nil, err
-			}
-			copy(data[k*pm:(k+1)*pm], ws.g[:pm])
+		if err := e.evalStateSpaceSweepInto(omega, data); err != nil {
+			return nil, err
 		}
 		applyIODelayPhase(e.sys, omega, data, e.p, e.m, true)
 		return e.matrix(data, omega), nil
 	}
-	if nw == 1 {
-		s := e.sAt(omega[0])
-		if err := e.evalStateSpaceInto(s, data); err != nil {
-			return nil, err
+	if e.useStateSpaceSweep(nw) {
+		if err := e.evalStateSpaceSweepInto(omega, data); err == nil {
+			applyIODelayPhase(e.sys, omega, data, e.p, e.m, true)
+			return e.matrix(data, omega), nil
 		}
-		applyIODelayAtS(e.sys, s, data, e.p, e.m, true)
-		return e.matrix(data, omega), nil
 	}
-
-	res, err := e.sys.TransferFunction(nil)
-	if err != nil {
+	if err := e.evalTransferFunctionSweepInto(omega, data); err != nil {
 		return nil, err
 	}
-	for k, w := range omega {
-		res.TF.evalInto(e.sAt(w), data[k*pm:(k+1)*pm])
-	}
 	applyIODelayPhase(e.sys, omega, data, e.p, e.m, false)
 	return e.matrix(data, omega), nil
 }
@@ -238,6 +228,37 @@ func (e frequencyEvaluator) evalStateSpaceInto(s complex128, dst []complex128) e
 	return nil
 }
 
+func (e frequencyEvaluator) useStateSpaceSweep(nw int) bool {
+	if nw <= 1 || e.n == 0 {
+		return true
+	}
+	return nw <= directFrequencySweepWorkLimit/e.n
+}
+
+func (e frequencyEvaluator) evalStateSpaceSweepInto(omega []float64, dst []complex128) error {
+	pm := e.p * e.m
+	ws := newSSEvalWorkspace(e.n, e.p, e.m)
+	for k, w := range omega {
+		if err := evalFrSSInto(ws, e.sys, e.sAt(w), e.n, e.p, e.m); err != nil {
+			return err
+		}
+		copy(dst[k*pm:(k+1)*pm], ws.g[:pm])
+	}
+	return nil
+}
+
+func (e frequencyEvaluator) evalTransferFunctionSweepInto(omega []float64, dst []complex128) error {
+	res, err := e.sys.TransferFunction(nil)
+	if err != nil {
+		return err
+	}
+	pm := e.p * e.m
+	for k, w := range omega {
+		res.TF.evalInto(e.sAt(w), dst[k*pm:(k+1)*pm])
+	}
+	return nil
+}
+
 func (e frequencyEvaluator) matrix(data []complex128, omega []float64) *FreqResponseMatrix {
 	return newFreqResponseMatrix(data, omega, e.p, e.m, e.sys.InputName, e.sys.OutputName)
 }

frequency_svd.go

@@ -12,6 +12,7 @@ type complexSVDWorkspace struct {
 	block  []float64
 	eig    []float64
 	work   []float64
+	scale  float64
 	nSV    int
 	gramN  int
 	blockN int
@@ -54,20 +55,35 @@ func (ws *complexSVDWorkspace) singularValuesFromNested(dst []float64, data [][]
 func (ws *complexSVDWorkspace) fillBlock(at func(i, j int) complex128, p, m int) {
 	n := ws.gramN
 	stride := ws.blockN
-	for i := range ws.block {
-		ws.block[i] = 0
+	clear(ws.block)
+	ws.scale = 0
+	for i := range p {
+		for j := range m {
+			magnitude := cmplx.Abs(at(i, j))
+			if math.IsNaN(magnitude) || magnitude > ws.scale {
+				ws.scale = magnitude
+			}
+		}
+	}
+	if ws.scale == 0 {
+		return
 	}
+	invScale := complex(1/ws.scale, 0)
 
 	for a := range n {
 		for b := range n {
 			var g complex128
 			if ws.useCol {
 				for row := range p {
-					g += cmplx.Conj(at(row, a)) * at(row, b)
+					left := at(row, a) * invScale
+					right := at(row, b) * invScale
+					g += cmplx.Conj(left) * right
 				}
 			} else {
 				for col := range m {
-					g += at(a, col) * cmplx.Conj(at(b, col))
+					left := at(a, col) * invScale
+					right := at(b, col) * invScale
+					g += left * cmplx.Conj(right)
 				}
 			}
 
@@ -81,8 +97,7 @@ func (ws *complexSVDWorkspace) fillBlock(at func(i, j int) complex128, p, m int)
 }
 
 func (ws *complexSVDWorkspace) singularValues(dst []float64) {
-	ok := impl.Dsyev(lapack.EVNone, blas.Upper, ws.blockN, ws.block, ws.blockN, ws.eig, ws.work, len(ws.work))
-	if !ok {
+	if !ws.factorize() {
 		for i := range dst {
 			dst[i] = math.NaN()
 		}
@@ -94,8 +109,63 @@ func (ws *complexSVDWorkspace) singularValues(dst []float64) {
 	}
 	for i := range dst {
 		lambda := nonnegativeGramEigenvalue(ws.eig[ws.blockN-1-2*i], scale)
-		dst[i] = math.Sqrt(lambda)
+		dst[i] = ws.scale * math.Sqrt(lambda)
+	}
+}
+
+func (ws *complexSVDWorkspace) maximumFromFlat(data []complex128, base, p, m int) (float64, bool) {
+	if p == 0 || m == 0 {
+		return 0, true
+	}
+	values := data[base : base+p*m]
+	if p == 1 || m == 1 {
+		var norm float64
+		for _, value := range values {
+			norm = math.Hypot(norm, cmplx.Abs(value))
+		}
+		return norm, true
+	}
+	if p == 2 && m == 2 {
+		return maximumComplex2x2SingularValue(values), true
+	}
+	ws.fillBlock(func(i, j int) complex128 {
+		return data[base+i*m+j]
+	}, p, m)
+	if !ws.factorize() {
+		return 0, false
 	}
+	scale := 1.0
+	for _, lambda := range ws.eig {
+		scale = max(scale, math.Abs(lambda))
+	}
+	lambda := nonnegativeGramEigenvalue(ws.eig[ws.blockN-1], scale)
+	return ws.scale * math.Sqrt(lambda), true
+}
+
+func (ws *complexSVDWorkspace) factorize() bool {
+	return impl.Dsyev(lapack.EVNone, blas.Upper, ws.blockN, ws.block, ws.blockN, ws.eig, ws.work, len(ws.work))
+}
+
+func maximumComplex2x2SingularValue(data []complex128) float64 {
+	scale := 0.0
+	for _, value := range data[:4] {
+		scale = math.Max(scale, cmplx.Abs(value))
+	}
+	if scale == 0 {
+		return 0
+	}
+	a00 := data[0] / complex(scale, 0)
+	a01 := data[1] / complex(scale, 0)
+	a10 := data[2] / complex(scale, 0)
+	a11 := data[3] / complex(scale, 0)
+	frobeniusSquared := complexMagnitudeSquared(a00) + complexMagnitudeSquared(a01) + complexMagnitudeSquared(a10) + complexMagnitudeSquared(a11)
+	determinantSquared := complexMagnitudeSquared(a00*a11 - a01*a10)
+	discriminant := math.Max(0, frobeniusSquared*frobeniusSquared-4*determinantSquared)
+	return scale * math.Sqrt((frobeniusSquared+math.Sqrt(discriminant))/2)
+}
+
+func complexMagnitudeSquared(value complex128) float64 {
+	return real(value)*real(value) + imag(value)*imag(value)
 }
 
 func nonnegativeGramEigenvalue(lambda, scale float64) float64 {

frequency_svd_test.go

@@ -0,0 +1,112 @@
+package controlsys
+
+import (
+	"math"
+	"testing"
+
+	"gonum.org/v1/gonum/mat"
+)
+
+func TestComplexSVDWorkspaceMaximumMatchesGonum(t *testing.T) {
+	tests := []struct {
+		name string
+		p, m int
+		data []complex128
+	}{
+		{
+			name: "Rectangular",
+			p:    3, m: 2,
+			data: []complex128{1 + 2i, -3 + 0.5i, 0.25 - 4i, 2 + 1i, -1.5 + 0.75i, 0.2 - 0.1i},
+		},
+		{
+			name: "RankDeficient",
+			p:    3, m: 3,
+			data: []complex128{
+				1 + 2i, 2 - 1i, -0.5 + 0.25i,
+				2 + 4i, 4 - 2i, -1 + 0.5i,
+				-1 - 2i, -2 + 1i, 0.5 - 0.25i,
+			},
+		},
+		{
+			name: "NonSymmetric2x2",
+			p:    2, m: 2,
+			data: []complex128{1e-150 + 2e-150i, -3e-150 + 0.5e-150i, 4e-150 - 2e-150i, 0.25e-150 + 1e-150i},
+		},
+		{
+			name: "LargeScale",
+			p:    3, m: 2,
+			data: []complex128{1e150 + 2e150i, -3e150, 0.5e150 - 0.25e150i, 2e150 + 1e150i, -1e150, 0.75e150i},
+		},
+		{
+			name: "Vector",
+			p:    1, m: 4,
+			data: []complex128{1e200, 2e200i, -3e200 + 0.5e200i, 0.25e200 - 0.75e200i},
+		},
+	}
+	for _, test := range tests {
+		t.Run(test.name, func(t *testing.T) {
+			var workspace *complexSVDWorkspace
+			if test.p > 1 && test.m > 1 && (test.p != 2 || test.m != 2) {
+				workspace = newComplexSVDWorkspace(test.p, test.m)
+			}
+			got, ok := workspace.maximumFromFlat(test.data, 0, test.p, test.m)
+			if !ok {
+				t.Fatal("maximum singular-value decomposition failed")
+			}
+			want := gonumComplexSingularValues(t, test.data, test.p, test.m)[0]
+			if !sameRelative(got, want, 2e-12) {
+				t.Fatalf("maximum singular value = %.17g, want %.17g", got, want)
+			}
+		})
+	}
+}
+
+func TestComplexSVDWorkspaceScaledSingularValues(t *testing.T) {
+	p, m := 4, 3
+	data := []complex128{
+		1e150 + 2e150i, -3e150, 0.5e150 - 0.25e150i,
+		2e150 + 1e150i, -1e150, 0.75e150i,
+		-0.5e150, 0.2e150 + 0.1e150i, 4e150 - 2e150i,
+		1.5e150 - 0.5e150i, -2e150 + 3e150i, 0.25e150,
+	}
+	want := gonumComplexSingularValues(t, data, p, m)
+	got := make([]float64, min(p, m))
+	workspace := newComplexSVDWorkspace(p, m)
+	workspace.singularValuesFromFlat(got, data, 0, p, m)
+	for i := range got {
+		if !sameRelative(got[i], want[i], 3e-12) {
+			t.Fatalf("singular value %d = %.17g, want %.17g", i, got[i], want[i])
+		}
+	}
+}
+
+func gonumComplexSingularValues(t *testing.T, data []complex128, p, m int) []float64 {
+	t.Helper()
+	realForm := mat.NewDense(2*p, 2*m, nil)
+	for i := range p {
+		for j := range m {
+			value := data[i*m+j]
+			realForm.Set(i, j, real(value))
+			realForm.Set(i, j+m, -imag(value))
+			realForm.Set(i+p, j, imag(value))
+			realForm.Set(i+p, j+m, real(value))
+		}
+	}
+	var decomposition mat.SVD
+	if ok := decomposition.Factorize(realForm, mat.SVDNone); !ok {
+		t.Fatal("Gonum SVD factorization failed")
+	}
+	values := decomposition.Values(nil)
+	result := make([]float64, min(p, m))
+	for i := range result {
+		result[i] = values[2*i]
+	}
+	return result
+}
+
+func sameRelative(got, want, tolerance float64) bool {
+	if got == want {
+		return true
+	}
+	return math.Abs(got-want) <= tolerance*math.Max(math.Abs(got), math.Abs(want))
+}