diff --git a/hardening_feedback_test.go b/hardening_feedback_test.go new file mode 100644 index 0000000..412bc10 --- /dev/null +++ b/hardening_feedback_test.go @@ -0,0 +1,405 @@ +package controlsys + +import ( + "math" + "math/cmplx" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestFeedback_NegativeFeedback_StabilizesUnstable(t *testing.T) { + plant, 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) + } + + poles, err := plant.Poles() + if err != nil { + t.Fatal(err) + } + if real(poles[0]) <= 0 { + t.Fatal("plant should be unstable") + } + + controller, err := NewGain(mat.NewDense(1, 1, []float64{5}), 0) + if err != nil { + t.Fatal(err) + } + + cl, err := Feedback(plant, controller, -1) + if err != nil { + t.Fatal(err) + } + + stable, err := cl.IsStable() + if err != nil { + t.Fatal(err) + } + if !stable { + clPoles, _ := cl.Poles() + t.Errorf("closed-loop should be stable, poles = %v", clPoles) + } + + clPoles, _ := cl.Poles() + if math.Abs(real(clPoles[0])-(-4)) > 1e-8 { + t.Errorf("CL pole = %v, want -4", clPoles[0]) + } +} + +func TestFeedback_UnitFeedback_DCGain(t *testing.T) { + plant, err := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{10}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + controller, err := NewGain(mat.NewDense(1, 1, []float64{1}), 0) + if err != nil { + t.Fatal(err) + } + + cl, err := Feedback(plant, controller, -1) + if err != nil { + t.Fatal(err) + } + + dc, err := cl.DCGain() + if err != nil { + t.Fatal(err) + } + want := 10.0 / 11.0 + if math.Abs(dc.At(0, 0)-want) > 1e-6 { + t.Errorf("DC gain = %v, want %v", dc.At(0, 0), want) + } +} + +func TestFeedback_HighGain_Bounded(t *testing.T) { + plant, err := New( + mat.NewDense(1, 1, []float64{-2}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{10.314}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + controller, err := NewGain(mat.NewDense(1, 1, []float64{500}), 0) + if err != nil { + t.Fatal(err) + } + + cl, err := Feedback(plant, controller, -1) + if err != nil { + t.Fatal(err) + } + + stable, err := cl.IsStable() + if err != nil { + t.Fatal(err) + } + if !stable { + t.Fatal("closed-loop should be stable") + } + + resp, err := Step(cl, 5.0) + if err != nil { + t.Fatal(err) + } + + _, cols := resp.Y.Dims() + for k := 0; k < cols; k++ { + if math.Abs(resp.Y.At(0, k)) > 100 { + t.Fatalf("step response unbounded at t=%v: y=%v", resp.T[k], resp.Y.At(0, k)) + } + } +} + +func TestSeries_Cascade(t *testing.T) { + sys1, 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) + } + + sys2, 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) + } + + cascade, err := Series(sys1, sys2) + if err != nil { + t.Fatal(err) + } + + poles, err := cascade.Poles() + if err != nil { + t.Fatal(err) + } + matchRoots(t, poles, []complex128{-1, -2}, 1e-8) + + dc, err := cascade.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(dc.At(0, 0)-0.5) > 1e-10 { + t.Errorf("DC gain = %v, want 0.5", dc.At(0, 0)) + } +} + +func TestParallel_Addition(t *testing.T) { + sys1, 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) + } + + sys2, 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) + } + + par, err := Parallel(sys1, sys2) + if err != nil { + t.Fatal(err) + } + + n, _, _ := par.Dims() + if n != 2 { + t.Errorf("states = %d, want 2", n) + } + + dc, err := par.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(dc.At(0, 0)-1.5) > 1e-10 { + t.Errorf("DC gain = %v, want 1.5", dc.At(0, 0)) + } +} + +func TestFeedback_StaticGain(t *testing.T) { + plant, err := NewGain(mat.NewDense(1, 1, []float64{2}), 0) + if err != nil { + t.Fatal(err) + } + + controller, err := NewGain(mat.NewDense(1, 1, []float64{0.5}), 0) + if err != nil { + t.Fatal(err) + } + + cl, err := Feedback(plant, controller, -1) + if err != nil { + t.Fatal(err) + } + + dc, err := cl.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(dc.At(0, 0)-1.0) > 1e-10 { + t.Errorf("DC gain = %v, want 1.0", dc.At(0, 0)) + } +} + +func TestSS2TF_StrictlyProper(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) + } + + tfRes, err := sys.TransferFunction(nil) + if err != nil { + t.Fatal(err) + } + + num := tfRes.TF.Num[0][0] + den := tfRes.TF.Den[0] + if !Poly(num).Equal(Poly{1}, 1e-10) { + t.Errorf("num = %v, want [1]", num) + } + if !Poly(den).Equal(Poly{1, 1}, 1e-10) { + t.Errorf("den = %v, want [1 1]", den) + } + + ssRes, err := tfRes.TF.StateSpace(nil) + if err != nil { + t.Fatal(err) + } + recoveredPoles, err := ssRes.Sys.Poles() + if err != nil { + t.Fatal(err) + } + matchRoots(t, recoveredPoles, []complex128{-1}, 1e-8) +} + +func TestSS2TF_WithFeedthrough(t *testing.T) { + sys, err := New( + mat.NewDense(1, 1, []float64{-2}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{3}), + mat.NewDense(1, 1, []float64{1}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + dc, err := sys.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(dc.At(0, 0)-2.5) > 1e-10 { + t.Errorf("DC gain = %v, want 2.5", dc.At(0, 0)) + } + + tfRes, err := sys.TransferFunction(nil) + if err != nil { + t.Fatal(err) + } + + s := complex(0, 1.0) + tfVal := tfRes.TF.Eval(s)[0][0] + want := (complex(1, 0)*s + 5) / (s + 2) + if cmplx.Abs(tfVal-want) > 1e-8 { + t.Errorf("TF(j) = %v, want %v", tfVal, want) + } +} + +func TestFeedback_MIMO_2x2(t *testing.T) { + plant, 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{0, 0, 0, 0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + controller, err := NewGain(mat.NewDense(2, 2, []float64{1, 0, 0, 1}), 0) + if err != nil { + t.Fatal(err) + } + + cl, err := Feedback(plant, controller, -1) + if err != nil { + t.Fatal(err) + } + + stable, err := cl.IsStable() + if err != nil { + t.Fatal(err) + } + if !stable { + t.Error("closed-loop should be stable") + } + + n, _, _ := cl.Dims() + if n != 2 { + t.Errorf("states = %d, want 2", n) + } + + olPoles, _ := plant.Poles() + clPoles, _ := cl.Poles() + for i, clp := range clPoles { + if real(clp) >= real(olPoles[i]) { + t.Errorf("CL pole %v not more negative than OL pole %v", clp, olPoles[i]) + } + } +} + +func TestTF2SS_StaticGain(t *testing.T) { + tf := &TransferFunc{ + Num: [][][]float64{{{23}}}, + Den: [][]float64{{46}}, + } + + ssRes, err := tf.StateSpace(nil) + if err != nil { + t.Fatal(err) + } + + n, _, _ := ssRes.Sys.Dims() + if n != 0 { + t.Errorf("states = %d, want 0", n) + } + + dc, err := ssRes.Sys.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(dc.At(0, 0)-0.5) > 1e-10 { + t.Errorf("DC gain = %v, want 0.5", dc.At(0, 0)) + } +} + +func TestFeedback_Preserves_Discrete(t *testing.T) { + plant, err := New( + mat.NewDense(1, 1, []float64{0.9}), + 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) + } + + controller, err := NewGain(mat.NewDense(1, 1, []float64{1}), 0.1) + if err != nil { + t.Fatal(err) + } + + cl, err := Feedback(plant, controller, -1) + if err != nil { + t.Fatal(err) + } + + if cl.Dt != 0.1 { + t.Errorf("Dt = %v, want 0.1", cl.Dt) + } +} diff --git a/hardening_gramian_test.go b/hardening_gramian_test.go new file mode 100644 index 0000000..e228877 --- /dev/null +++ b/hardening_gramian_test.go @@ -0,0 +1,392 @@ +package controlsys + +import ( + "errors" + "math" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestGram_Continuous_MATLABValidated(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{1, -2, 3, -4}), + mat.NewDense(2, 2, []float64{5, 6, 7, 8}), + mat.NewDense(2, 2, []float64{4, 5, 6, 7}), + mat.NewDense(2, 2, []float64{13, 14, 15, 16}), 0) + if err != nil { + t.Fatal(err) + } + + wcRes, err := Gram(sys, GramControllability) + if err != nil { + t.Fatal(err) + } + wantWc := mat.NewDense(2, 2, []float64{18.5, 24.5, 24.5, 32.5}) + assertMatNearT(t, "Wc", wcRes.X, wantWc, 1e-4) + + woRes, err := Gram(sys, GramObservability) + if err != nil { + t.Fatal(err) + } + wantWo := mat.NewDense(2, 2, []float64{257.5, -94.5, -94.5, 56.5}) + assertMatNearT(t, "Wo", woRes.X, wantWo, 1e-4) +} + +func TestGram_Discrete_MATLABValidated(t *testing.T) { + csys, err := New( + mat.NewDense(2, 2, []float64{1, -2, 3, -4}), + mat.NewDense(2, 2, []float64{5, 6, 7, 8}), + mat.NewDense(2, 2, []float64{4, 5, 6, 7}), + mat.NewDense(2, 2, []float64{13, 14, 15, 16}), 0) + if err != nil { + t.Fatal(err) + } + + dsys, err := csys.DiscretizeZOH(0.2) + if err != nil { + t.Fatal(err) + } + + wcRes, err := Gram(dsys, GramControllability) + if err != nil { + t.Fatal(err) + } + woRes, err := Gram(dsys, GramObservability) + if err != nil { + t.Fatal(err) + } + + for _, res := range []*GramResult{wcRes, woRes} { + if !isSymmetric(res.X, 1e-10) { + t.Error("gramian not symmetric") + } + var eig mat.Eigen + ok := eig.Factorize(res.X, mat.EigenNone) + if !ok { + t.Fatal("eigendecomposition failed") + } + vals := eig.Values(nil) + for i, v := range vals { + if real(v) < -1e-10 { + t.Errorf("eigenvalue[%d] = %g, want >= 0", i, real(v)) + } + } + } + + cwcRes, _ := Gram(csys, GramControllability) + diff := mat.NewDense(2, 2, nil) + diff.Sub(wcRes.X, cwcRes.X) + if mat.Norm(diff, 1) < 1e-10 { + t.Error("discrete and continuous gramians should differ") + } +} + +func TestHSV_MATLABValidated(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{1, -2, 3, -4}), + mat.NewDense(2, 1, []float64{5, 7}), + mat.NewDense(1, 2, []float64{6, 8}), + mat.NewDense(1, 1, []float64{9}), 0) + if err != nil { + t.Fatal(err) + } + + hsv, err := HSV(sys) + if err != nil { + t.Fatal(err) + } + if len(hsv) != 2 { + t.Fatalf("len(hsv) = %d, want 2", len(hsv)) + } + want := []float64{24.42686, 0.5731395} + for i, w := range want { + if math.Abs(hsv[i]-w)/w > 1e-3 { + t.Errorf("hsv[%d] = %g, want %g", i, hsv[i], w) + } + } +} + +func TestH2Norm_FirstOrder(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) + } + + got, err := H2Norm(sys) + if err != nil { + t.Fatal(err) + } + want := 1.0 / math.Sqrt(2) + if math.Abs(got-want) > 1e-10 { + t.Errorf("H2 = %.15g, want %.15g", got, want) + } +} + +func TestHinfNorm_FirstOrder_Hardening(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) + } + + norm, _, err := HinfNorm(sys) + if err != nil { + t.Fatal(err) + } + if math.Abs(norm-1.0) > 1e-10 { + t.Errorf("Hinf = %.15g, want 1.0", norm) + } +} + +func TestH2Norm_Unstable_IsInfinite(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) + } + + got, err := H2Norm(sys) + if err == nil && !math.IsInf(got, 1) { + t.Errorf("H2 = %g for unstable system, want +Inf or error", got) + } +} + +func TestHinfNorm_MarginallyStable_IsInfinite(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) + } + + norm, _, err := HinfNorm(sys) + if err != nil { + return + } + if norm < 1e6 { + t.Errorf("Hinf = %g for marginally stable system, want very large or +Inf", norm) + } +} + +func TestNorms_MIMO_MATLABValidated(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{ + -1.017041847539126, -0.224182952826418, 0.042538079149249, + -0.310374015319095, -0.516461581407780, -0.119195790221750, + -1.452723568727942, 1.799586083710209, -1.491935830615152, + }), + mat.NewDense(3, 2, []float64{ + 0.312858596637428, -0.164879019209038, + -0.864879917324456, 0.627707287528727, + -0.030051296196269, 1.093265669039484, + }), + mat.NewDense(2, 3, []float64{ + 1.109273297614398, 0.077359091130425, -1.113500741486764, + -0.863652821988714, -1.214117043615409, -0.006849328103348, + }), + mat.NewDense(2, 2, nil), 0) + if err != nil { + t.Fatal(err) + } + + hinf, _, err := HinfNorm(sys) + if err != nil { + t.Fatal(err) + } + wantHinf := 4.276759162964244 + if math.Abs(hinf-wantHinf)/wantHinf > 1e-4 { + t.Errorf("Hinf = %g, want %g", hinf, wantHinf) + } + + h2, err := H2Norm(sys) + if err != nil { + t.Fatal(err) + } + wantH2 := 2.237461821810309 + if math.Abs(h2-wantH2)/wantH2 > 1e-4 { + t.Errorf("H2 = %g, want %g", h2, wantH2) + } +} + +func TestBalred_MATLABValidated(t *testing.T) { + sys, err := New( + mat.NewDense(4, 4, []float64{ + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1, + -60, -200, -60, -15, + }), + mat.NewDense(4, 1, []float64{0, 0, 0, 1}), + mat.NewDense(1, 4, []float64{32, 45, 11, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + origDC, err := sys.DCGain() + if err != nil { + t.Fatal(err) + } + + red, _, err := Balred(sys, 2, Truncate) + if err != nil { + t.Fatal(err) + } + nr, _, _ := red.Dims() + if nr != 2 { + t.Fatalf("n = %d, want 2", nr) + } + + stable, err := red.IsStable() + if err != nil { + t.Fatal(err) + } + if !stable { + t.Error("reduced system is not stable") + } + + redDC, err := red.DCGain() + if err != nil { + t.Fatal(err) + } + wantDC := origDC.At(0, 0) + gotDC := redDC.At(0, 0) + if math.Abs(gotDC-wantDC)/math.Abs(wantDC) > 0.5 { + t.Errorf("DC gain = %g, want ~%g", gotDC, wantDC) + } +} + +func TestBalred_MarginallyStable(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, 0, 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) + } + + defer func() { + if r := recover(); r != nil { + t.Fatalf("Balreal panicked: %v", r) + } + }() + + _, err = Balreal(sys) + if err != nil { + return + } + + defer func() { + if r := recover(); r != nil { + t.Fatalf("Balred panicked: %v", r) + } + }() + + _, _, err = Balred(sys, 1, Truncate) + if err != nil { + return + } +} + +func TestGram_Symmetry(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{-2, 0.5, 0, -3}), + 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) + } + + wcRes, err := Gram(sys, GramControllability) + if err != nil { + t.Fatal(err) + } + woRes, err := Gram(sys, GramObservability) + if err != nil { + t.Fatal(err) + } + + for _, tc := range []struct { + name string + g *mat.Dense + }{ + {"Wc", wcRes.X}, + {"Wo", woRes.X}, + } { + n, _ := tc.g.Dims() + nrm := mat.Norm(tc.g, 1) + if nrm < 1e-15 { + continue + } + for i := range n { + for j := range i { + diff := math.Abs(tc.g.At(i, j) - tc.g.At(j, i)) + if diff > 1e-12*nrm { + t.Errorf("%s[%d,%d] - %s[%d,%d] = %g, want symmetric", tc.name, i, j, tc.name, j, i, diff) + } + } + } + } +} + +func TestHinfNorm_WithFeedthrough(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{2}), 0) + if err != nil { + t.Fatal(err) + } + + norm, _, err := HinfNorm(sys) + if err != nil { + t.Fatal(err) + } + if math.Abs(norm-3.0) > 1e-6 { + t.Errorf("Hinf = %g, want 3.0", norm) + } + + dc, err := sys.DCGain() + if err != nil { + t.Fatal(err) + } + if norm < math.Abs(dc.At(0, 0))-1e-10 { + t.Errorf("Hinf(%g) < |DC gain|(%g)", norm, math.Abs(dc.At(0, 0))) + } +} + +func TestGram_Unstable_Continuous_Hardening(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 = Gram(sys, GramControllability) + if !errors.Is(err, ErrUnstableGramian) { + t.Errorf("got %v, want ErrUnstableGramian", err) + } + _, err = Gram(sys, GramObservability) + if !errors.Is(err, ErrUnstableGramian) { + t.Errorf("got %v, want ErrUnstableGramian", err) + } +} diff --git a/hardening_lyapcanon_test.go b/hardening_lyapcanon_test.go new file mode 100644 index 0000000..371e0b8 --- /dev/null +++ b/hardening_lyapcanon_test.go @@ -0,0 +1,322 @@ +package controlsys + +import ( + "math" + "math/cmplx" + "sort" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestLyap_Continuous_2x2(t *testing.T) { + A := mat.NewDense(2, 2, []float64{-1, 0.5, 0, -2}) + Q := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + X, err := Lyap(A, Q, nil) + if err != nil { + t.Fatal(err) + } + if res := lyapResidual(A, X, Q); res > 1e-10 { + t.Errorf("residual = %e", res) + } +} + +func TestLyap_Continuous_NonSymmetricA(t *testing.T) { + A := mat.NewDense(2, 2, []float64{1, -2, 3, -4}) + Q := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + X, err := Lyap(A, Q, nil) + if err != nil { + t.Fatal(err) + } + if res := lyapResidual(A, X, Q); res > 1e-10 { + t.Errorf("residual = %e", res) + } + r, c := X.Dims() + for i := range r { + for j := i + 1; j < c; j++ { + if d := math.Abs(X.At(i, j) - X.At(j, i)); d > 1e-12 { + t.Errorf("X not symmetric: X[%d,%d]-X[%d,%d] = %e", i, j, j, i, d) + } + } + } +} + +func TestDLyap_Discrete_2x2(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0.5, 0.1, 0, 0.8}) + Q := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + X, err := DLyap(A, Q, nil) + if err != nil { + t.Fatal(err) + } + if res := dlyapResidual(A, X, Q); res > 1e-10 { + t.Errorf("residual = %e", res) + } +} + +func TestDLyap_NonSymmetricA(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0.6, 0, 0.1, 0.4}) + Q := mat.NewDense(2, 2, []float64{2, 1, 1, 3}) + X, err := DLyap(A, Q, nil) + if err != nil { + t.Fatal(err) + } + if res := dlyapResidual(A, X, Q); res > 1e-10 { + t.Errorf("residual = %e", res) + } +} + +func TestLyap_3x3_Verify(t *testing.T) { + A := mat.NewDense(3, 3, []float64{ + -3, 1, 0, + 0, -2, 1, + 0, 0, -1, + }) + Q := mat.NewDense(3, 3, []float64{ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1, + }) + X, err := Lyap(A, Q, nil) + if err != nil { + t.Fatal(err) + } + if res := lyapResidual(A, X, Q); res > 1e-10 { + t.Errorf("residual = %e", res) + } + checkSymmetric(t, X, 1e-12) +} + +func TestLyap_NearSingular_Error(t *testing.T) { + eps := 1e-10 + A := mat.NewDense(2, 2, []float64{-eps, 1, -1, -eps}) + Q := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + X, err := Lyap(A, Q, nil) + if err != nil { + t.Skipf("solver returned error for near-singular case: %v", err) + } + if res := lyapResidual(A, X, Q); res > 1e-6 { + t.Errorf("residual = %e", res) + } +} + +func TestCanon_Modal_DistinctReal(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, 1}), + 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 + 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", i, j, Amod.At(i, j)) + } + } + } + + poles, _ := res.Sys.Poles() + sortPolesByReal(poles) + if math.Abs(real(poles[0])+3) > 1e-10 || math.Abs(real(poles[1])+1) > 1e-10 { + t.Errorf("poles = %v, want [-3, -1]", poles) + } + + checkEigsPreserved(t, sys, res.Sys, 1e-10) +} + +func TestCanon_Modal_ComplexPair(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -4, -2}), + 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)+1) > 1e-8 || math.Abs(Amod.At(1, 1)+1) > 1e-8 { + t.Errorf("diagonal should be -1, got [%g, %g]", Amod.At(0, 0), Amod.At(1, 1)) + } + wn := math.Sqrt(3) + if math.Abs(math.Abs(Amod.At(0, 1))-wn) > 1e-8 || math.Abs(math.Abs(Amod.At(1, 0))-wn) > 1e-8 { + t.Errorf("off-diagonal should be ±sqrt(3), 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 TestCanon_Modal_RepeatedEigenvalues(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{-1, 1, 0, -1}), + 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) + } + + Traw := res.T.RawMatrix() + maxEntry := 0.0 + for _, v := range Traw.Data { + if a := math.Abs(v); a > maxEntry { + maxEntry = a + } + } + if maxEntry > 1e6 { + t.Errorf("T has huge entries (max=%g), Schur fallback may have failed", maxEntry) + } + + checkEigsPreserved(t, sys, res.Sys, 1e-8) +} + +func TestCanon_Companion_Verify(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{ + 0, 1, 0, + 0, 0, 1, + -6, -11, -6, + }), + mat.NewDense(3, 1, []float64{0, 0, 1}), + mat.NewDense(1, 3, []float64{1, 0, 0}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + poles, err := sys.Poles() + if err != nil { + t.Fatal(err) + } + sort.Float64s(realParts(poles)) + wantPoles := []float64{-3, -2, -1} + rp := realParts(poles) + sort.Float64s(rp) + for i, w := range wantPoles { + if math.Abs(rp[i]-w) > 1e-8 { + t.Errorf("pole[%d] = %g, want %g", i, rp[i], w) + } + } + + res, err := Canon(sys, CanonCompanion) + if err != nil { + t.Fatal(err) + } + + checkEigsPreserved(t, sys, res.Sys, 1e-10) +} + +func TestCanon_Modal_PreservesPoles(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{ + -2, 1, 0, + 0, -3, 1, + 0, 0, -5, + }), + mat.NewDense(3, 1, []float64{1, 0, 0}), + mat.NewDense(1, 3, []float64{1, 1, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Canon(sys, CanonModal) + if err != nil { + t.Fatal(err) + } + + origPoles, _ := sys.Poles() + modPoles, _ := res.Sys.Poles() + sortPolesByReal(origPoles) + sortPolesByReal(modPoles) + for i := range origPoles { + if cmplx.Abs(origPoles[i]-modPoles[i]) > 1e-10 { + t.Errorf("pole[%d]: orig=%v modal=%v", i, origPoles[i], modPoles[i]) + } + } +} + +func TestCanon_Modal_PreservesDCGain(t *testing.T) { + sys, err := New( + mat.NewDense(3, 3, []float64{ + -2, 1, 0, + 0, -3, 1, + 0, 0, -5, + }), + mat.NewDense(3, 1, []float64{1, 0, 0}), + mat.NewDense(1, 3, []float64{1, 1, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + res, err := Canon(sys, CanonModal) + if err != nil { + t.Fatal(err) + } + + dc1, err := sys.DCGain() + if err != nil { + t.Fatal(err) + } + dc2, err := res.Sys.DCGain() + if err != nil { + t.Fatal(err) + } + + r, c := dc1.Dims() + for i := range r { + for j := range c { + if d := math.Abs(dc1.At(i, j) - dc2.At(i, j)); d > 1e-10 { + t.Errorf("DCGain[%d,%d]: orig=%g modal=%g", i, j, dc1.At(i, j), dc2.At(i, j)) + } + } + } +} + +func TestLyap_SolutionSymmetry(t *testing.T) { + A := mat.NewDense(2, 2, []float64{-3, 1, 0.5, -2}) + Q := mat.NewDense(2, 2, []float64{5, 1, 1, 3}) + X, err := Lyap(A, Q, nil) + if err != nil { + t.Fatal(err) + } + norm := denseNorm(X) + r, c := X.Dims() + for i := range r { + for j := i + 1; j < c; j++ { + if d := math.Abs(X.At(i, j) - X.At(j, i)); d/norm > 1e-12 { + t.Errorf("relative asymmetry X[%d,%d]-X[%d,%d] = %e (norm=%e)", i, j, j, i, d, norm) + } + } + } +} + +func realParts(poles []complex128) []float64 { + rp := make([]float64, len(poles)) + for i, p := range poles { + rp[i] = real(p) + } + return rp +} diff --git a/hardening_margin_test.go b/hardening_margin_test.go new file mode 100644 index 0000000..9023fff --- /dev/null +++ b/hardening_margin_test.go @@ -0,0 +1,417 @@ +package controlsys + +import ( + "math" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestMargin_ThirdOrderContinuous(t *testing.T) { + // G(s) = 1/(s+1)^3: DC gain = 1, so no gain crossover. + // GM = 18.06 dB at w_pc = sqrt(3), PM = +Inf (no gain crossover). + sys, err := NewFromSlices(3, 1, 1, + []float64{ + 0, 1, 0, + 0, 0, 1, + -1, -3, -3, + }, + []float64{0, 0, 1}, + []float64{1, 0, 0}, + []float64{0}, + 0, + ) + if err != nil { + t.Fatal(err) + } + + r, err := Margin(sys) + if err != nil { + t.Fatal(err) + } + + wantGmDB := 20 * math.Log10(8) + wantWpc := math.Sqrt(3) + + if math.Abs(r.GainMargin-wantGmDB)/wantGmDB > 0.05 { + t.Errorf("GM = %v dB, want ~%v dB", r.GainMargin, wantGmDB) + } + if math.Abs(r.WpFreq-wantWpc)/wantWpc > 0.05 { + t.Errorf("WpFreq = %v, want ~%v", r.WpFreq, wantWpc) + } + if !math.IsInf(r.PhaseMargin, 1) { + t.Errorf("PM = %v, want +Inf (no gain crossover)", r.PhaseMargin) + } +} + +func TestMargin_ThirdOrderWithGain(t *testing.T) { + // G(s) = 2/(s+1)^3: DC gain = 2 > 1, so gain crossover exists. + // GM = 12.04 dB at w_pc = sqrt(3), PM ≈ 67.6° at wgc ≈ 0.766 + sys, err := NewFromSlices(3, 1, 1, + []float64{ + 0, 1, 0, + 0, 0, 1, + -1, -3, -3, + }, + []float64{0, 0, 1}, + []float64{2, 0, 0}, + []float64{0}, + 0, + ) + if err != nil { + t.Fatal(err) + } + + r, err := Margin(sys) + if err != nil { + t.Fatal(err) + } + + wantGmDB := 20 * math.Log10(4) + wantWpc := math.Sqrt(3) + wantPm := 67.6 + wantWgc := 0.766 + + if math.Abs(r.GainMargin-wantGmDB)/wantGmDB > 0.05 { + t.Errorf("GM = %v dB, want ~%v dB", r.GainMargin, wantGmDB) + } + if math.Abs(r.WpFreq-wantWpc)/wantWpc > 0.05 { + t.Errorf("WpFreq = %v, want ~%v", r.WpFreq, wantWpc) + } + if math.Abs(r.PhaseMargin-wantPm) > 2 { + t.Errorf("PM = %v deg, want ~%v deg", r.PhaseMargin, wantPm) + } + if math.Abs(r.WgFreq-wantWgc)/wantWgc > 0.05 { + t.Errorf("WgFreq = %v, want ~%v", r.WgFreq, wantWgc) + } +} + +func TestMargin_IntegratorSystem(t *testing.T) { + // G(s) = 1/(s(s+1)): phase goes from -90 to -180, never crosses -180. + // GM = +Inf, PM ≈ 52° at wgc ≈ 0.786 + sys, err := NewFromSlices(2, 1, 1, + []float64{ + 0, 1, + 0, -1, + }, + []float64{0, 1}, + []float64{1, 0}, + []float64{0}, + 0, + ) + if err != nil { + t.Fatal(err) + } + + r, err := Margin(sys) + if err != nil { + t.Fatal(err) + } + + if !math.IsInf(r.GainMargin, 1) { + t.Errorf("GM = %v, want +Inf (phase never crosses -180)", r.GainMargin) + } + + wantPm := 52.0 + if math.Abs(r.PhaseMargin-wantPm) > 3 { + t.Errorf("PM = %v deg, want ~%v deg", r.PhaseMargin, wantPm) + } + if r.WgFreq < 0.5 || r.WgFreq > 1.0 { + t.Errorf("WgFreq = %v, want ~0.786", r.WgFreq) + } +} + +func TestMargin_DiscreteTime(t *testing.T) { + // G(s) = 10/((s+1)(s+10)) discretized at dt=0.1 + sys, err := NewFromSlices(2, 1, 1, + []float64{ + 0, 1, + -10, -11, + }, + []float64{0, 10}, + []float64{1, 0}, + []float64{0}, + 0, + ) + if err != nil { + t.Fatal(err) + } + + dsys, err := sys.DiscretizeZOH(0.1) + if err != nil { + t.Fatal(err) + } + + r, err := Margin(dsys) + if err != nil { + t.Fatal(err) + } + + if r.GainMargin <= 0 { + t.Errorf("GM = %v dB, want positive (stable system)", r.GainMargin) + } + if r.PhaseMargin <= 0 { + t.Errorf("PM = %v deg, want positive (stable system)", r.PhaseMargin) + } +} + +func TestBode_PhaseUnwrapping_ThirdOrder(t *testing.T) { + // G(s) = 1/(s+1)^3: phase at high freq approaches -270°. + // With multiple Bode points, unwrapping should produce continuous phase. + sys, err := NewFromSlices(3, 1, 1, + []float64{ + 0, 1, 0, + 0, 0, 1, + -1, -3, -3, + }, + []float64{0, 0, 1}, + []float64{1, 0, 0}, + []float64{0}, + 0, + ) + if err != nil { + t.Fatal(err) + } + + omega := make([]float64, 200) + for i := range omega { + omega[i] = math.Pow(10, -2+4*float64(i)/float64(len(omega)-1)) + } + + bode, err := sys.Bode(omega, 0) + if err != nil { + t.Fatal(err) + } + + for k := 1; k < len(omega); k++ { + diff := math.Abs(bode.PhaseAt(k, 0, 0) - bode.PhaseAt(k-1, 0, 0)) + if diff > 180 { + t.Errorf("phase jump at w=%v: %v -> %v (diff=%v)", + omega[k], bode.PhaseAt(k-1, 0, 0), bode.PhaseAt(k, 0, 0), diff) + break + } + } + + lastPhase := bode.PhaseAt(len(omega)-1, 0, 0) + if lastPhase > -250 { + t.Errorf("final phase = %v deg, want approaching -270 for 3rd order", lastPhase) + } +} + +func TestBode_PhaseUnwrapping_FourthOrder(t *testing.T) { + // G(s) = 24/((s+1)(s+2)(s+3)(s+4)): phase goes from 0 to -360°. + sys, err := NewFromSlices(4, 1, 1, + []float64{ + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1, + -24, -50, -35, -10, + }, + []float64{0, 0, 0, 24}, + []float64{1, 0, 0, 0}, + []float64{0}, + 0, + ) + if err != nil { + t.Fatal(err) + } + + omega := make([]float64, 200) + for i := range omega { + omega[i] = math.Pow(10, -1+3*float64(i)/float64(len(omega)-1)) + } + + bode, err := sys.Bode(omega, 0) + if err != nil { + t.Fatal(err) + } + + for k := 1; k < len(omega); k++ { + diff := math.Abs(bode.PhaseAt(k, 0, 0) - bode.PhaseAt(k-1, 0, 0)) + if diff > 180 { + t.Errorf("phase jump at w=%v: %v -> %v (diff=%v)", + omega[k], bode.PhaseAt(k-1, 0, 0), bode.PhaseAt(k, 0, 0), diff) + break + } + } + + lastPhase := bode.PhaseAt(len(omega)-1, 0, 0) + if lastPhase > -300 { + t.Errorf("final phase = %v deg, want approaching -360 for 4th order", lastPhase) + } +} + +func TestMargin_StableHighGain(t *testing.T) { + // G(s) = 100/(s^3 + 8s^2 + 17s + 10) = 100/((s+1)(s+2)(s+5)) + // DC gain = 100/10 = 10. Stable, with both gain and phase crossovers. + sys, err := NewFromSlices(3, 1, 1, + []float64{ + 0, 1, 0, + 0, 0, 1, + -10, -17, -8, + }, + []float64{0, 0, 1}, + []float64{100, 0, 0}, + []float64{0}, + 0, + ) + if err != nil { + t.Fatal(err) + } + + r, err := Margin(sys) + if err != nil { + t.Fatal(err) + } + + if r.GainMargin <= 0 { + t.Errorf("GM = %v dB, want positive (stable system)", r.GainMargin) + } + if r.PhaseMargin <= 0 || r.PhaseMargin > 90 { + t.Errorf("PM = %v deg, want small positive", r.PhaseMargin) + } + if math.IsNaN(r.WgFreq) || math.IsNaN(r.WpFreq) { + t.Errorf("crossover freqs should be finite: WgFreq=%v, WpFreq=%v", r.WgFreq, r.WpFreq) + } +} + +func TestBode_MIMOSystem(t *testing.T) { + // 2x2 diagonal: G11=1/(s+1), G22=1/(s+2), off-diagonal=0 + sys, err := NewFromSlices(2, 2, 2, + []float64{-1, 0, 0, -2}, + []float64{1, 0, 0, 1}, + []float64{1, 0, 0, 1}, + []float64{0, 0, 0, 0}, + 0, + ) + if err != nil { + t.Fatal(err) + } + + bode, err := sys.Bode([]float64{1.0}, 0) + if err != nil { + t.Fatal(err) + } + + mag00 := bode.MagDBAt(0, 0, 0) + wantMag00 := 20 * math.Log10(1.0 / math.Sqrt(2)) + if math.Abs(mag00-wantMag00) > 0.1 { + t.Errorf("(0,0) mag = %v dB, want ~%v dB", mag00, wantMag00) + } + + mag11 := bode.MagDBAt(0, 1, 1) + wantMag11 := 20 * math.Log10(1.0 / math.Sqrt(5)) + if math.Abs(mag11-wantMag11) > 0.1 { + t.Errorf("(1,1) mag = %v dB, want ~%v dB", mag11, wantMag11) + } + + mag01 := bode.MagDBAt(0, 0, 1) + if mag01 > -60 { + t.Errorf("(0,1) off-diagonal mag = %v dB, want << 0 (decoupled)", mag01) + } +} + +func TestMargin_PureGain(t *testing.T) { + sys, err := NewGain(mat.NewDense(1, 1, []float64{0.5}), 0) + if err != nil { + t.Fatal(err) + } + + r, err := Margin(sys) + if err != nil { + t.Fatal(err) + } + + if !math.IsInf(r.GainMargin, 1) { + t.Errorf("GM = %v, want +Inf for static gain < 1", r.GainMargin) + } + if !math.IsInf(r.PhaseMargin, 1) { + t.Errorf("PM = %v, want +Inf for static gain < 1", r.PhaseMargin) + } +} + +func TestMargin_PureGainAboveUnity(t *testing.T) { + sys, err := NewGain(mat.NewDense(1, 1, []float64{2.0}), 0) + if err != nil { + t.Fatal(err) + } + + r, err := Margin(sys) + if err != nil { + t.Fatal(err) + } + + if !math.IsInf(r.GainMargin, 1) { + t.Errorf("GM = %v, want +Inf (no phase crossover for pure gain)", r.GainMargin) + } +} + +func TestAllMargin_ThirdOrderContinuous(t *testing.T) { + // G(s) = 2/(s+1)^3: has both gain and phase crossovers. + sys, err := NewFromSlices(3, 1, 1, + []float64{ + 0, 1, 0, + 0, 0, 1, + -1, -3, -3, + }, + []float64{0, 0, 1}, + []float64{2, 0, 0}, + []float64{0}, + 0, + ) + if err != nil { + t.Fatal(err) + } + + all, err := AllMargin(sys) + if err != nil { + t.Fatal(err) + } + + if len(all.GainCrossFreqs) != 1 { + t.Fatalf("got %d gain crossovers, want 1", len(all.GainCrossFreqs)) + } + if len(all.PhaseCrossFreqs) != 1 { + t.Fatalf("got %d phase crossovers, want 1", len(all.PhaseCrossFreqs)) + } + + wantGm := 20 * math.Log10(4) + if math.Abs(all.GainMargins[0]-wantGm)/wantGm > 0.05 { + t.Errorf("GainMargin[0] = %v dB, want ~%v dB", all.GainMargins[0], wantGm) + } + if math.Abs(all.PhaseMargins[0]-67.6) > 2 { + t.Errorf("PhaseMargin[0] = %v deg, want ~67.6 deg", all.PhaseMargins[0]) + } +} + +func TestAllMargin_IntegratorSystem(t *testing.T) { + // G(s) = 1/(s(s+1)): no phase crossover, one gain crossover. + sys, err := NewFromSlices(2, 1, 1, + []float64{ + 0, 1, + 0, -1, + }, + []float64{0, 1}, + []float64{1, 0}, + []float64{0}, + 0, + ) + if err != nil { + t.Fatal(err) + } + + all, err := AllMargin(sys) + if err != nil { + t.Fatal(err) + } + + if len(all.PhaseCrossFreqs) != 0 { + t.Errorf("got %d phase crossovers, want 0 (phase never reaches -180)", len(all.PhaseCrossFreqs)) + } + if len(all.GainCrossFreqs) != 1 { + t.Fatalf("got %d gain crossovers, want 1", len(all.GainCrossFreqs)) + } + if all.PhaseMargins[0] < 40 || all.PhaseMargins[0] > 60 { + t.Errorf("PhaseMargin[0] = %v deg, want ~52", all.PhaseMargins[0]) + } +} diff --git a/hardening_placement_test.go b/hardening_placement_test.go new file mode 100644 index 0000000..f42c326 --- /dev/null +++ b/hardening_placement_test.go @@ -0,0 +1,188 @@ +package controlsys + +import ( + "math" + "math/cmplx" + "sort" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func sortEigenvalues(eigs []complex128) { + sort.Slice(eigs, func(i, j int) bool { + ri, rj := real(eigs[i]), real(eigs[j]) + if ri != rj { + return ri < rj + } + return imag(eigs[i]) < imag(eigs[j]) + }) +} + +func assertEigenvaluesMatch(t *testing.T, label string, A, B, K *mat.Dense, poles []complex128, tol float64) { + t.Helper() + eigs := closedLoopEig(A, B, K) + if eigs == nil { + t.Fatalf("%s: eigenvalue decomposition failed", label) + } + sortEigenvalues(eigs) + sorted := make([]complex128, len(poles)) + copy(sorted, poles) + sortEigenvalues(sorted) + if len(eigs) != len(sorted) { + t.Fatalf("%s: got %d eigenvalues, want %d", label, len(eigs), len(sorted)) + } + for i := range eigs { + if cmplx.Abs(eigs[i]-sorted[i]) > tol { + t.Errorf("%s: eig[%d] = %v, want %v", label, i, eigs[i], sorted[i]) + } + } +} + +func TestAcker_ReturnShape(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, 0, 0}) + B := mat.NewDense(2, 1, []float64{0, 1}) + poles := []complex128{-1 + 1i, -1 - 1i} + + K, err := Acker(A, B, poles) + if err != nil { + t.Fatal(err) + } + r, c := K.Dims() + if r != 1 || c != 2 { + t.Errorf("K dims = (%d,%d), want (1,2)", r, c) + } +} + +func TestAcker_MIMO_Rejection(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, 0, 0}) + B := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + poles := []complex128{-1, -2} + + _, err := Acker(A, B, poles) + if err == nil { + t.Fatal("expected error for MIMO system, got nil") + } +} + +func TestPlace_ScipyExample(t *testing.T) { + A := mat.NewDense(4, 4, []float64{ + 1.380, -0.2077, 6.715, -5.676, + -0.5814, -4.290, 0, 0.6750, + 1.067, 4.273, -6.654, 5.893, + 0.0480, 4.273, 1.343, -2.104, + }) + B := mat.NewDense(4, 2, []float64{ + 0, 5.679, + 1.136, 1.136, + 0, 0, + -3.146, 0, + }) + poles := []complex128{-0.5 + 1i, -0.5 - 1i, -5.0566, -8.6659} + + K, err := Place(A, B, poles) + if err != nil { + t.Fatal(err) + } + assertEigenvaluesMatch(t, "scipy_example", A, B, K, poles, 1e-4) +} + +func TestPlace_ComplexConjugateOnUnstable(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, 100, 0}) + B := mat.NewDense(2, 1, []float64{0, 1}) + poles := []complex128{-20 + 10i, -20 - 10i} + + K, err := Place(A, B, poles) + if err != nil { + t.Fatal(err) + } + assertEigenvaluesMatch(t, "unstable_complex", A, B, K, poles, 1e-8) +} + +func TestAcker_ComplexConjugatePoles(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, 0, 0}) + B := mat.NewDense(2, 1, []float64{0, 1}) + poles := []complex128{-2 + 3i, -2 - 3i} + + K, err := Acker(A, B, poles) + if err != nil { + t.Fatal(err) + } + assertEigenvaluesMatch(t, "acker_complex_conj", A, B, K, poles, 1e-10) +} + +func TestPlace_RealPoles(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, 0, 0}) + B := mat.NewDense(2, 1, []float64{0, 1}) + poles := []complex128{-1, -2} + + K, err := Place(A, B, poles) + if err != nil { + t.Fatal(err) + } + assertEigenvaluesMatch(t, "real_poles", A, B, K, poles, 1e-8) +} + +func TestCtrb_NonSymmetricA(t *testing.T) { + A := mat.NewDense(2, 2, []float64{1, -2, 3, -4}) + B := mat.NewDense(2, 1, []float64{5, 7}) + + got, err := Ctrb(A, B) + if err != nil { + t.Fatal(err) + } + + want := mat.NewDense(2, 2, []float64{ + 5, 1*5 + (-2)*7, + 7, 3*5 + (-4)*7, + }) + assertMatNearT(t, "Ctrb_NonSymA", got, want, 1e-10) +} + +func TestCtrb_Obsv_Duality_Hardening(t *testing.T) { + A := mat.NewDense(2, 2, []float64{1, -2, 3, -4}) + B := mat.NewDense(2, 2, []float64{5, 6, 7, 8}) + + wc, err := Ctrb(A, B) + if err != nil { + t.Fatal(err) + } + + wo, err := Obsv(mat.DenseCopyOf(A.T()), mat.DenseCopyOf(B.T())) + if err != nil { + t.Fatal(err) + } + + woT := mat.DenseCopyOf(wo.T()) + assertMatNearT(t, "duality_hardening", wc, woT, 1e-10) +} + +func TestPlace_3x1_System(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, 1}) + poles := []complex128{-10, -20, -30} + + K, err := Place(A, B, poles) + if err != nil { + t.Fatal(err) + } + assertEigenvaluesMatch(t, "3x1_system", A, B, K, poles, 1e-6) +} + +func TestAcker_SingleState(t *testing.T) { + A := mat.NewDense(1, 1, []float64{2}) + B := mat.NewDense(1, 1, []float64{1}) + poles := []complex128{-5} + + K, err := Acker(A, B, poles) + if err != nil { + t.Fatal(err) + } + if math.Abs(K.At(0, 0)-7) > 1e-10 { + t.Errorf("K = %v, want [[7]]", K.At(0, 0)) + } +} diff --git a/hardening_response_test.go b/hardening_response_test.go new file mode 100644 index 0000000..cf8bf4d --- /dev/null +++ b/hardening_response_test.go @@ -0,0 +1,305 @@ +package controlsys + +import ( + "math" + "math/cmplx" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestStep_Integrator(t *testing.T) { + sys, 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) + } + + resp, err := Step(sys, 2.0) + if err != nil { + t.Fatal(err) + } + + _, steps := resp.Y.Dims() + for k := 0; k < steps; k++ { + tk := resp.T[k] + got := resp.Y.At(0, k) + if math.Abs(got-tk) > 0.05 { + t.Errorf("t=%.3f: got %f, want %f (ramp)", tk, got, tk) + } + } + + k1 := 0 + for j := 1; j < steps; j++ { + if math.Abs(resp.T[j]-1.0) < math.Abs(resp.T[k1]-1.0) { + k1 = j + } + } + if math.Abs(resp.Y.At(0, k1)-1.0) > 0.05 { + t.Errorf("y(1.0) = %f, want ~1.0", resp.Y.At(0, k1)) + } + + k2 := 0 + for j := 1; j < steps; j++ { + if math.Abs(resp.T[j]-2.0) < math.Abs(resp.T[k2]-2.0) { + k2 = j + } + } + if math.Abs(resp.Y.At(0, k2)-2.0) > 0.05 { + t.Errorf("y(2.0) = %f, want ~2.0", resp.Y.At(0, k2)) + } +} + +func TestStep_DCGain_Convergence(t *testing.T) { + sys, err := New( + mat.NewDense(1, 1, []float64{-1}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{3}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + resp, err := Step(sys, 20.0) + if err != nil { + t.Fatal(err) + } + + _, steps := resp.Y.Dims() + lastVal := resp.Y.At(0, steps-1) + if math.Abs(lastVal-3.0) > 0.03 { + t.Errorf("final value = %f, want ~3.0 (DC gain)", lastVal) + } +} + +func TestDCGain_WithFeedthrough(t *testing.T) { + sys, err := New( + mat.NewDense(1, 1, []float64{-2}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{3}), + mat.NewDense(1, 1, []float64{1}), 0) + if err != nil { + t.Fatal(err) + } + + g, err := sys.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(g.At(0, 0)-2.5) > 1e-12 { + t.Errorf("DC gain = %f, want 2.5", g.At(0, 0)) + } +} + +func TestDCGain_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{0, 0, 0, 0}), 0) + if err != nil { + t.Fatal(err) + } + + g, err := sys.DCGain() + if err != nil { + t.Fatal(err) + } + + want := mat.NewDense(2, 2, []float64{1, 0, 0, 0.5}) + if !matEqual(g, want, 1e-12) { + t.Errorf("DC gain =\n%v\nwant\n%v", mat.Formatted(g), mat.Formatted(want)) + } +} + +func TestDamp_ContinuousSystem(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -4, -2}), + 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) + } + + info, err := Damp(sys) + if err != nil { + t.Fatal(err) + } + if len(info) != 2 { + t.Fatalf("got %d poles, want 2", len(info)) + } + + for _, d := range info { + if math.Abs(d.Wn-2.0) > 1e-10 { + t.Errorf("Wn = %f, want 2.0", d.Wn) + } + if math.Abs(d.Zeta-0.5) > 1e-10 { + t.Errorf("Zeta = %f, want 0.5", d.Zeta) + } + } +} + +func TestDamp_DiscreteNegativePole(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) + } + + info, err := Damp(sys) + if err != nil { + t.Fatal(err) + } + if len(info) != 1 { + t.Fatalf("got %d poles, want 1", len(info)) + } + + d := info[0] + if math.IsNaN(d.Wn) || math.IsNaN(d.Zeta) { + t.Fatalf("NaN in Damp result: Wn=%f, Zeta=%f", d.Wn, d.Zeta) + } + + sc := cmplx.Log(complex(-0.5, 0)) / complex(0.1, 0) + wantWn := cmplx.Abs(sc) + if math.Abs(d.Wn-wantWn) > 1e-10 { + t.Errorf("Wn = %f, want %f", d.Wn, wantWn) + } + if d.Wn > 0 { + wantZeta := -real(sc) / wantWn + if math.Abs(d.Zeta-wantZeta) > 1e-10 { + t.Errorf("Zeta = %f, want %f", d.Zeta, wantZeta) + } + } +} + +func TestDiscretize_ZOH_PreservesDCGain(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) + } + + dcCont, err := sys.DCGain() + if err != nil { + t.Fatal(err) + } + wantDC := dcCont.At(0, 0) + if math.Abs(wantDC-1.5) > 1e-12 { + t.Fatalf("continuous DC gain = %f, want 1.5", wantDC) + } + + dsys, err := sys.DiscretizeZOH(0.1) + if err != nil { + t.Fatal(err) + } + + dcDisc, err := dsys.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(dcDisc.At(0, 0)-wantDC) > 1e-6 { + t.Errorf("discrete ZOH DC gain = %f, want %f", dcDisc.At(0, 0), wantDC) + } +} + +func TestDiscretize_Tustin_PreservesDCGain(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) + } + + dcCont, err := sys.DCGain() + if err != nil { + t.Fatal(err) + } + wantDC := dcCont.At(0, 0) + + dsys, err := sys.Discretize(0.1) + if err != nil { + t.Fatal(err) + } + + dcDisc, err := dsys.DCGain() + if err != nil { + t.Fatal(err) + } + if math.Abs(dcDisc.At(0, 0)-wantDC) > 1e-6 { + t.Errorf("discrete Tustin DC gain = %f, want %f", dcDisc.At(0, 0), wantDC) + } +} + +func TestStep_DiscreteTime(t *testing.T) { + sys, err := 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) + if err != nil { + t.Fatal(err) + } + + resp, err := Step(sys, 10.0) + if err != nil { + t.Fatal(err) + } + + _, steps := resp.Y.Dims() + lastVal := resp.Y.At(0, steps-1) + + dcGain, err := sys.DCGain() + if err != nil { + t.Fatal(err) + } + wantDC := dcGain.At(0, 0) + if math.Abs(lastVal-wantDC) > 0.05 { + t.Errorf("final value = %f, want ~%f (DC gain)", lastVal, wantDC) + } +} + +func TestStep_NonMinimumPhase(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{-1, -1, 1, 0}), + mat.NewDense(2, 1, []float64{1, 0}), + mat.NewDense(1, 2, []float64{-1, 1}), + mat.NewDense(1, 1, []float64{0}), 0) + if err != nil { + t.Fatal(err) + } + + resp, err := Step(sys, 10.0) + if err != nil { + t.Fatal(err) + } + + _, steps := resp.Y.Dims() + + foundUndershoot := false + for k := 0; k < steps; k++ { + if resp.Y.At(0, k) < -0.01 { + foundUndershoot = true + break + } + } + if !foundUndershoot { + t.Error("non-minimum phase system should exhibit initial undershoot (y < 0)") + } + + lastVal := resp.Y.At(0, steps-1) + if math.Abs(lastVal-1.0) > 0.05 { + t.Errorf("final value = %f, want ~1.0 (DC gain)", lastVal) + } +} diff --git a/hardening_robust_test.go b/hardening_robust_test.go new file mode 100644 index 0000000..0fbe187 --- /dev/null +++ b/hardening_robust_test.go @@ -0,0 +1,421 @@ +package controlsys + +import ( + "math" + "math/cmplx" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func TestDiskMargin_FifthOrder(t *testing.T) { + A := mat.NewDense(5, 5, []float64{ + 0, 1, 0, 0, 0, + 0, 0, 1, 0, 0, + 0, 0, 0, 1, 0, + 0, 0, 0, 0, 1, + 0, -100, -200.18, -100.36, -2.18, + }) + B := mat.NewDense(5, 1, []float64{0, 0, 0, 0, 1}) + C := mat.NewDense(1, 5, []float64{93.75, 50, 6.25, 0, 0}) + D := mat.NewDense(1, 1, []float64{0}) + + sys, err := New(A, B, C, D, 0) + if err != nil { + t.Fatal(err) + } + + dm, err := DiskMargin(sys) + if err != nil { + t.Fatal(err) + } + + if dm.Alpha <= 0 { + t.Errorf("Alpha = %v, want > 0", dm.Alpha) + } + if dm.PeakSensitivity <= 1 { + t.Errorf("PeakSensitivity = %v, want > 1", dm.PeakSensitivity) + } + if dm.GainMargin[0] >= 1 || dm.GainMargin[1] <= 1 { + t.Errorf("GainMargin = %v, want [<1, >1]", dm.GainMargin) + } + if dm.PhaseMargin <= 0 { + t.Errorf("PhaseMargin = %v, want > 0", dm.PhaseMargin) + } + + poles, _ := sys.Poles() + if len(poles) != 5 { + t.Errorf("got %d poles, want 5", len(poles)) + } + hasIntegrator := false + for _, p := range poles { + if cmplx.Abs(p) < 0.01 { + hasIntegrator = true + } + } + if !hasIntegrator { + t.Error("expected an integrator pole near s=0") + } +} + +func TestMargin_Type2System(t *testing.T) { + G1 := buildSS(t, 1, 1, []float64{-10}, []float64{1}, []float64{10}, []float64{0}) + integrator := buildSS(t, 1, 1, []float64{0}, []float64{1}, []float64{1}, []float64{0}) + + plant, err := Series(integrator, G1) + if err != nil { + t.Fatal(err) + } + + Kp := 16.5 + Ki := 16.5 * 5.0 + piCtrl := buildSS(t, 1, 1, []float64{0}, []float64{1}, []float64{Ki}, []float64{Kp}) + + L2, err := Series(plant, piCtrl) + if err != nil { + t.Fatal(err) + } + + unity, _ := NewGain(mat.NewDense(1, 1, []float64{1}), 0) + cl2, err := Feedback(L2, unity, -1) + if err != nil { + t.Fatal(err) + } + stable2, err := cl2.IsStable() + if err != nil { + t.Fatal(err) + } + if !stable2 { + t.Fatal("type 2 closed-loop should be stable") + } + + mr2, err := Margin(L2) + if err != nil { + t.Fatal(err) + } + if mr2.PhaseMargin <= 0 || math.IsInf(mr2.PhaseMargin, 0) { + t.Errorf("Type2 PM = %v, want finite > 0", mr2.PhaseMargin) + } + t.Logf("Type2: GM=%.1f dB, PM=%.1f deg", mr2.GainMargin, mr2.PhaseMargin) +} + +func TestMargin_Type2VsType1(t *testing.T) { + G1 := buildSS(t, 1, 1, []float64{-10}, []float64{1}, []float64{10}, []float64{0}) + integrator := buildSS(t, 1, 1, []float64{0}, []float64{1}, []float64{1}, []float64{0}) + + plant, err := Series(integrator, G1) + if err != nil { + t.Fatal(err) + } + + Kp := 16.5 + Ki := 16.5 * 5.0 + piCtrl := buildSS(t, 1, 1, []float64{0}, []float64{1}, []float64{Ki}, []float64{Kp}) + + L2, err := Series(plant, piCtrl) + if err != nil { + t.Fatal(err) + } + + unity, _ := NewGain(mat.NewDense(1, 1, []float64{1}), 0) + cl2, err := Feedback(L2, unity, -1) + if err != nil { + t.Fatal(err) + } + stable2, err := cl2.IsStable() + if err != nil { + t.Fatal(err) + } + if !stable2 { + t.Fatal("type 2 closed-loop should be stable") + } + + mr2, err := Margin(L2) + if err != nil { + t.Fatal(err) + } + if mr2.PhaseMargin <= 0 || math.IsInf(mr2.PhaseMargin, 0) { + t.Errorf("Type2 PM = %v, want finite > 0", mr2.PhaseMargin) + } + + pCtrl, _ := NewGain(mat.NewDense(1, 1, []float64{Kp}), 0) + L1, err := Series(plant, pCtrl) + if err != nil { + t.Fatal(err) + } + mr1, err := Margin(L1) + if err != nil { + t.Fatal(err) + } + + t.Logf("Type1: PM=%.1f deg; Type2: PM=%.1f deg", mr1.PhaseMargin, mr2.PhaseMargin) + + if !math.IsInf(mr1.PhaseMargin, 0) && !math.IsInf(mr2.PhaseMargin, 0) { + if mr2.PhaseMargin >= mr1.PhaseMargin { + t.Errorf("Type2 PM=%.1f should be < Type1 PM=%.1f (more aggressive)", mr2.PhaseMargin, mr1.PhaseMargin) + } + } +} + +func buildSS(t *testing.T, n, m int, a, b, c, d []float64) *System { + t.Helper() + sys, err := New( + mat.NewDense(n, n, a), + mat.NewDense(n, m, b), + mat.NewDense(m, n, c), + mat.NewDense(m, m, d), + 0, + ) + if err != nil { + t.Fatal(err) + } + return sys +} + +func TestDamp_SecondOrder_SpringMassDamper(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, -0.16, -0.24}) + B := mat.NewDense(2, 1, []float64{0, 0.004}) + 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) + } + + info, err := Damp(sys) + if err != nil { + t.Fatal(err) + } + if len(info) != 2 { + t.Fatalf("got %d poles, want 2", len(info)) + } + + wnExpect := 0.4 + zetaExpect := 0.3 + + for i, d := range info { + if math.Abs(d.Wn-wnExpect) > 0.01 { + t.Errorf("pole[%d] Wn=%.4f, want ~%.4f", i, d.Wn, wnExpect) + } + if math.Abs(d.Zeta-zetaExpect) > 0.01 { + t.Errorf("pole[%d] Zeta=%.4f, want ~%.4f", i, d.Zeta, zetaExpect) + } + } +} + +func TestCtrb_3State_PhysicalParams(t *testing.T) { + A := mat.NewDense(3, 3, []float64{ + 1, -1, 1, + 1, -0.16, -0.24, + 1, 1, 1, + }) + B := mat.NewDense(3, 1, []float64{0, 0.004, 1}) + C := mat.NewDense(1, 3, []float64{1, 1, 0}) + + cm, err := Ctrb(A, B) + if err != nil { + t.Fatal(err) + } + r, c := cm.Dims() + if r != 3 || c != 3 { + t.Fatalf("Ctrb dims = (%d,%d), want (3,3)", r, c) + } + + ctrbRank := svdRank(t, cm) + if ctrbRank != 3 { + t.Errorf("Ctrb rank = %d, want 3 (fully controllable)", ctrbRank) + } + + om, err := Obsv(A, C) + if err != nil { + t.Fatal(err) + } + r2, c2 := om.Dims() + if r2 != 3 || c2 != 3 { + t.Fatalf("Obsv dims = (%d,%d), want (3,3)", r2, c2) + } + + obsvRank := svdRank(t, om) + if obsvRank != 3 { + t.Errorf("Obsv rank = %d, want 3 (fully observable)", obsvRank) + } +} + +func svdRank(t *testing.T, m *mat.Dense) int { + t.Helper() + var s mat.SVD + if !s.Factorize(m, mat.SVDNone) { + t.Fatal("SVD factorization failed") + } + sv := s.Values(nil) + rank := 0 + for _, v := range sv { + if v > 1e-10 { + rank++ + } + } + return rank +} + +func TestSeries_TF_SS_Roundtrip(t *testing.T) { + sys1, err := New( + mat.NewDense(2, 2, []float64{0, 1, -4, -1}), + 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) + } + + sys2, err := New( + mat.NewDense(1, 1, []float64{-5}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{-4.5}), + mat.NewDense(1, 1, []float64{1}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + ser, err := Series(sys1, sys2) + if err != nil { + t.Fatal(err) + } + + n, _, _ := ser.Dims() + if n != 3 { + t.Errorf("Series states = %d, want 3", n) + } + + dc, err := ser.DCGain() + if err != nil { + t.Fatal(err) + } + dcVal := dc.At(0, 0) + if math.Abs(dcVal-0.025) > 0.001 { + t.Errorf("DC gain = %v, want 0.025", dcVal) + } + + poles, err := ser.Poles() + if err != nil { + t.Fatal(err) + } + if len(poles) != 3 { + t.Fatalf("got %d poles, want 3", len(poles)) + } + + wantPoles := []complex128{ + complex(-0.5, math.Sqrt(15.0)/2.0), + complex(-0.5, -math.Sqrt(15.0)/2.0), + complex(-5, 0), + } + for _, wp := range wantPoles { + found := false + for _, gp := range poles { + if cmplx.Abs(gp-wp) < 0.01 { + found = true + break + } + } + if !found { + t.Errorf("expected pole near %v not found in %v", wp, poles) + } + } +} + +func TestParallel_SS_Verify(t *testing.T) { + sys1, err := New( + mat.NewDense(2, 2, []float64{0, 1, -4, -1}), + 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) + } + + sys2, err := New( + mat.NewDense(1, 1, []float64{-5}), + mat.NewDense(1, 1, []float64{1}), + mat.NewDense(1, 1, []float64{-4.5}), + mat.NewDense(1, 1, []float64{1}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + par, err := Parallel(sys1, sys2) + if err != nil { + t.Fatal(err) + } + + n, _, _ := par.Dims() + if n != 3 { + t.Errorf("Parallel states = %d, want 3", n) + } + + dc, err := par.DCGain() + if err != nil { + t.Fatal(err) + } + dcVal := dc.At(0, 0) + if math.Abs(dcVal-0.35) > 0.01 { + t.Errorf("DC gain = %v, want 0.35", dcVal) + } +} + +func TestBandwidth_FirstOrder_Robust(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) + } + + bw3, err := Bandwidth(sys, 0) + if err != nil { + t.Fatal(err) + } + if math.Abs(bw3-1.0) > 0.05 { + t.Errorf("3dB bandwidth = %v, want ~1.0", bw3) + } + + bw6, err := Bandwidth(sys, -6) + if err != nil { + t.Fatal(err) + } + if bw6 <= bw3 { + t.Errorf("6dB bandwidth=%v should exceed 3dB bandwidth=%v", bw6, bw3) + } +} + +func TestBandwidth_SecondOrder_Butterworth(t *testing.T) { + sys, err := New( + mat.NewDense(2, 2, []float64{0, 1, -100, -14.14}), + mat.NewDense(2, 1, []float64{0, 100}), + mat.NewDense(1, 2, []float64{1, 0}), + mat.NewDense(1, 1, []float64{0}), + 0, + ) + if err != nil { + t.Fatal(err) + } + + bw, err := Bandwidth(sys, 0) + if err != nil { + t.Fatal(err) + } + if math.Abs(bw-10.0)/10.0 > 0.2 { + t.Errorf("Bandwidth = %v, want ~10.0 (within 20%%)", bw) + } +} diff --git a/hardening_synthesis_test.go b/hardening_synthesis_test.go new file mode 100644 index 0000000..5ca6985 --- /dev/null +++ b/hardening_synthesis_test.go @@ -0,0 +1,369 @@ +package controlsys + +import ( + "math" + "testing" + + "gonum.org/v1/gonum/mat" +) + +func schererPlantA() *mat.Dense { + return mat.NewDense(3, 3, []float64{ + 0, 10, 2, + -1, 1, 0, + 0, 2, -5, + }) +} + +func pvtolAB() (*mat.Dense, *mat.Dense) { + A := mat.NewDense(6, 6, []float64{ + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1, + 0, 0, -9.8, -0.0125, 0, 0, + 0, 0, 0, 0, -0.0125, 0, + 0, 0, 0, 0, 0, 0, + }) + B := mat.NewDense(6, 2, []float64{ + 0, 0, + 0, 0, + 0, 0, + 0.25, 0, + 0, 0.25, + 5.2632, 0, + }) + return A, B +} + +func TestH2Syn_SchererExample7(t *testing.T) { + A := schererPlantA() + B1 := mat.NewDense(3, 1, []float64{1, 0, 1}) + B2 := mat.NewDense(3, 1, []float64{0, 1, 0}) + C1 := mat.NewDense(3, 3, []float64{ + 0, 1, 0, + 0, 0, 1, + 0, 0, 0, + }) + D11 := mat.NewDense(3, 1, []float64{0, 0, 0}) + D12 := mat.NewDense(3, 1, []float64{0, 0, 1}) + C2 := mat.NewDense(1, 3, []float64{0, 1, 0}) + D21 := mat.NewDense(1, 1, []float64{2}) + D22 := mat.NewDense(1, 1, []float64{0}) + + Baug := mat.NewDense(3, 2, nil) + Baug.Augment(B1, B2) + + Caug := mat.NewDense(4, 3, nil) + Caug.Stack(C1, C2) + + Daug := mat.NewDense(4, 2, nil) + var d11d12 mat.Dense + d11d12.Augment(D11, D12) + var d21d22 mat.Dense + d21d22.Augment(D21, D22) + Daug.Stack(&d11d12, &d21d22) + + P, err := New(A, Baug, Caug, Daug, 0) + if err != nil { + t.Fatal(err) + } + + res, err := H2Syn(P, 1, 1) + if err != nil { + t.Fatal(err) + } + + for _, p := range res.CLPoles { + if real(p) >= 0 { + t.Errorf("unstable closed-loop pole: %v", p) + } + } + + kn, km, kp := res.K.Dims() + if kn != 3 { + t.Errorf("controller states: got %d, want 3", kn) + } + if km != 1 { + t.Errorf("controller inputs: got %d, want 1", km) + } + if kp != 1 { + t.Errorf("controller outputs: got %d, want 1", kp) + } + + Ak := res.K.A + Bk := res.K.B + Ck := res.K.C + Dk := res.K.D + + n := 3 + nk, _ := Ak.Dims() + ncl := n + nk + + Acl := mat.NewDense(ncl, ncl, nil) + Bcl := mat.NewDense(ncl, 1, nil) + Ccl := mat.NewDense(3, ncl, nil) + Dcl := mat.NewDense(3, 1, nil) + + var dkc2 mat.Dense + dkc2.Mul(Dk, C2) + var b2dkc2 mat.Dense + b2dkc2.Mul(B2, &dkc2) + var topLeft mat.Dense + topLeft.Add(A, &b2dkc2) + + var b2ck mat.Dense + b2ck.Mul(B2, Ck) + + var bkc2 mat.Dense + bkc2.Mul(Bk, C2) + + for i := range n { + for j := range n { + Acl.Set(i, j, topLeft.At(i, j)) + } + for j := range nk { + Acl.Set(i, n+j, b2ck.At(i, j)) + } + } + for i := range nk { + for j := range n { + Acl.Set(n+i, j, bkc2.At(i, j)) + } + for j := range nk { + Acl.Set(n+i, n+j, Ak.At(i, j)) + } + } + + var dkd21 mat.Dense + dkd21.Mul(Dk, D21) + var b2dkd21 mat.Dense + b2dkd21.Mul(B2, &dkd21) + var bTop mat.Dense + bTop.Add(B1, &b2dkd21) + var bkd21 mat.Dense + bkd21.Mul(Bk, D21) + for i := range n { + Bcl.Set(i, 0, bTop.At(i, 0)) + } + for i := range nk { + Bcl.Set(n+i, 0, bkd21.At(i, 0)) + } + + var d12dkc2 mat.Dense + d12dkc2.Mul(D12, &dkc2) + var cLeft mat.Dense + cLeft.Add(C1, &d12dkc2) + var d12ck mat.Dense + d12ck.Mul(D12, Ck) + for i := range 3 { + for j := range n { + Ccl.Set(i, j, cLeft.At(i, j)) + } + for j := range nk { + Ccl.Set(i, n+j, d12ck.At(i, j)) + } + } + + var d12dkd21 mat.Dense + d12dkd21.Mul(D12, &dkd21) + Dcl.Add(D11, &d12dkd21) + + cl, err := New(Acl, Bcl, Ccl, Dcl, 0) + if err != nil { + t.Fatal(err) + } + + stable, err := cl.IsStable() + if err != nil { + t.Fatal(err) + } + if !stable { + t.Error("closed-loop system is not stable") + } + + h2, err := H2Norm(cl) + if err != nil { + t.Fatal(err) + } + + if math.Abs(h2-7.7484) > 0.1 { + t.Errorf("H2 norm = %v, want ≈ 7.7484", h2) + } +} + +func TestHinfSyn_SchererExample7(t *testing.T) { + A := schererPlantA() + B1 := mat.NewDense(3, 1, []float64{1, 0, 1}) + B2 := mat.NewDense(3, 1, []float64{0, 1, 0}) + C1 := mat.NewDense(2, 3, []float64{ + 1, 0, 0, + 0, 0, 0, + }) + D11 := mat.NewDense(2, 1, []float64{0, 0}) + D12 := mat.NewDense(2, 1, []float64{0, 1}) + C2 := mat.NewDense(1, 3, []float64{0, 1, 0}) + D21 := mat.NewDense(1, 1, []float64{2}) + D22 := mat.NewDense(1, 1, []float64{0}) + + Baug := mat.NewDense(3, 2, nil) + Baug.Augment(B1, B2) + + Caug := mat.NewDense(3, 3, nil) + Caug.Stack(C1, C2) + + Daug := mat.NewDense(3, 2, nil) + var d11d12 mat.Dense + d11d12.Augment(D11, D12) + var d21d22 mat.Dense + d21d22.Augment(D21, D22) + Daug.Stack(&d11d12, &d21d22) + + P, err := New(A, Baug, Caug, Daug, 0) + if err != nil { + t.Fatal(err) + } + + res, err := HinfSyn(P, 1, 1) + if err != nil { + t.Fatal(err) + } + + if res.GammaOpt <= 0 || math.IsInf(res.GammaOpt, 0) || math.IsNaN(res.GammaOpt) { + t.Errorf("GammaOpt = %v, want positive finite", res.GammaOpt) + } + + kn, _, _ := res.K.Dims() + if kn != 3 { + t.Errorf("controller states: got %d, want 3", kn) + } + + for _, p := range res.CLPoles { + if real(p) >= 0 { + t.Errorf("unstable closed-loop pole: %v", p) + } + } +} + +func TestLqr_PVTOL_6State(t *testing.T) { + A, B := pvtolAB() + + Q := mat.NewDense(6, 6, nil) + for i := range 6 { + Q.Set(i, i, 1) + } + R := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + + res, err := Lqr(A, B, Q, R, nil) + if err != nil { + t.Fatal(err) + } + + kr, kc := res.K.Dims() + if kr != 2 || kc != 6 { + t.Fatalf("K dims = %dx%d, want 2x6", kr, kc) + } + + for _, e := range res.Eig { + if real(e) >= 0 { + t.Errorf("unstable eigenvalue: %v", e) + } + } + + Q2 := mat.NewDense(6, 6, nil) + Q2.Set(0, 0, 100) + Q2.Set(1, 1, 10) + Q2.Set(2, 2, 2*math.Pi/5) + R2 := mat.NewDense(2, 2, []float64{0.1, 0, 0, 1.0}) + + res2, err := Lqr(A, B, Q2, R2, nil) + if err != nil { + t.Fatal(err) + } + + kr2, kc2 := res2.K.Dims() + if kr2 != 2 || kc2 != 6 { + t.Fatalf("K2 dims = %dx%d, want 2x6", kr2, kc2) + } + + for _, e := range res2.Eig { + if real(e) >= 0 { + t.Errorf("unstable eigenvalue (design 2): %v", e) + } + } +} + +func TestKalman_PVTOL_PartialMeasurement(t *testing.T) { + A, B := pvtolAB() + C := mat.NewDense(3, 6, []float64{ + 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, + }) + + Qn := mat.NewDense(2, 2, []float64{1e-2, 0, 0, 1e-2}) + Rn := mat.NewDense(3, 3, []float64{ + 2e-4, 0, 1e-5, + 0, 2e-4, 1e-5, + 1e-5, 1e-5, 1e-4, + }) + + res, err := Lqe(A, B, C, Qn, Rn, nil) + if err != nil { + t.Fatal(err) + } + + lr, lc := res.K.Dims() + if lr != 6 || lc != 3 { + t.Fatalf("L dims = %dx%d, want 6x3", lr, lc) + } + + for _, e := range res.Eig { + if real(e) >= 0 { + t.Errorf("unstable observer eigenvalue: %v", e) + } + } +} + +func TestLqr_PVTOL_HeavyInputPenalty(t *testing.T) { + A, B := pvtolAB() + + Q := mat.NewDense(6, 6, nil) + for i := range 6 { + Q.Set(i, i, 1) + } + + R1 := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + res1, err := Lqr(A, B, Q, R1, nil) + if err != nil { + t.Fatal(err) + } + + R2 := mat.NewDense(2, 2, []float64{1600, 0, 0, 1600}) + res2, err := Lqr(A, B, Q, R2, nil) + if err != nil { + t.Fatal(err) + } + + for _, e := range res2.Eig { + if real(e) >= 0 { + t.Errorf("unstable eigenvalue (heavy R): %v", e) + } + } + + maxReal1 := math.Inf(-1) + for _, e := range res1.Eig { + if real(e) > maxReal1 { + maxReal1 = real(e) + } + } + maxReal2 := math.Inf(-1) + for _, e := range res2.Eig { + if real(e) > maxReal2 { + maxReal2 = real(e) + } + } + + if maxReal2 <= maxReal1 { + t.Errorf("heavy R should have slower poles: max Re(eig) R=I=%v, R=1600I=%v", maxReal1, maxReal2) + } +} diff --git a/hardening_test.go b/hardening_test.go index f3bde6a..8d8a399 100644 --- a/hardening_test.go +++ b/hardening_test.go @@ -2,6 +2,8 @@ package controlsys import ( "errors" + "math" + "math/cmplx" "testing" "gonum.org/v1/gonum/mat" @@ -133,3 +135,367 @@ func TestLqg_RejectsDescriptor(t *testing.T) { t.Errorf("expected ErrDescriptorRiccati, got %v", err) } } + +func TestCare_SymmetryCheckScaleAware(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, -2, -3}) + B := mat.NewDense(2, 1, []float64{0, 1}) + R := mat.NewDense(1, 1, []float64{1}) + + t.Run("LargeScale", func(t *testing.T) { + Q := mat.NewDense(2, 2, []float64{1e8, 1e-8, 0, 1e8}) + res, err := Care(A, B, Q, R, nil) + if err != nil { + t.Fatalf("CARE should accept near-symmetric Q at large scale: %v", err) + } + r := careResidual(A, B, Q, R, res.X) + relTol := 1e-4 * mat.Norm(Q, 1) + if r > relTol { + t.Errorf("residual = %e, relTol = %e", r, relTol) + } + }) + + t.Run("SmallScale", func(t *testing.T) { + Q := mat.NewDense(2, 2, []float64{1e-8, 1e-24, 0, 1e-8}) + res, err := Care(A, B, Q, R, nil) + if err != nil { + t.Fatalf("CARE should accept near-symmetric Q at small scale: %v", err) + } + if r := careResidual(A, B, Q, R, res.X); r > 1e-6 { + t.Errorf("residual = %e", r) + } + }) +} + +func TestCare_GeneralizedWithCrossTerm(t *testing.T) { + A := mat.NewDense(2, 2, []float64{-2, -1, -1, -1}) + B := mat.NewDense(2, 2, []float64{1, 0, 0, 4}) + Q := mat.NewDense(2, 2, []float64{0, 0, 0, 1}) + R := mat.NewDense(2, 2, []float64{2, 0, 0, 1}) + S := mat.NewDense(2, 2, []float64{0, 0, 0, 0}) + + res, err := Care(A, B, Q, R, &RiccatiOpts{S: S}) + if err != nil { + t.Fatal(err) + } + + n := 2 + X := res.X + var atx, xa, rinv_bts, xb_rinv_bts, residual mat.Dense + + atx.Mul(A.T(), X) + xa.Mul(X, A) + + var xb mat.Dense + xb.Mul(X, B) + var xbPlusS mat.Dense + xbPlusS.Add(&xb, S) + + var btx mat.Dense + btx.Mul(B.T(), X) + var btxPlusSt mat.Dense + btxPlusSt.Add(&btx, S.T()) + + var luR mat.LU + luR.Factorize(R) + luR.SolveTo(&rinv_bts, false, &btxPlusSt) + + xb_rinv_bts.Mul(&xbPlusS, &rinv_bts) + + residual.Add(&atx, &xa) + residual.Sub(&residual, &xb_rinv_bts) + residual.Add(&residual, Q) + + rNorm := denseNorm(&residual) / float64(n) + if rNorm > 1e-10 { + t.Errorf("CARE residual = %e", rNorm) + } + checkSymmetric(t, X, 1e-10) +} + +func TestDare_GeneralizedWithCrossTerm(t *testing.T) { + A := mat.NewDense(2, 2, []float64{-0.6, 0, -0.1, -0.4}) + B := mat.NewDense(2, 2, []float64{2, 1, 0, 1}) + Q := mat.NewDense(2, 2, []float64{2, 1, 1, 1}) + R := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + S := mat.NewDense(2, 2, []float64{0, 0, 0, 0}) + + res, err := Dare(A, B, Q, R, &RiccatiOpts{S: S}) + if err != nil { + t.Fatal(err) + } + + if r := dareResidual(A, B, Q, R, res.X); r > 1e-9 { + t.Errorf("DARE residual = %e", r) + } + checkSymmetric(t, res.X, 1e-10) + + for _, e := range res.Eig { + if cmplx.Abs(e) >= 1.0 { + t.Errorf("unstable closed-loop eigenvalue: %v (|λ|=%v)", e, cmplx.Abs(e)) + } + } +} + +func TestDare_IllConditionedA(t *testing.T) { + A := mat.NewDense(2, 2, []float64{1.0, 0.001, 0, 0.999}) + 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}) + + res, err := Dare(A, B, Q, R, nil) + if err != nil { + t.Fatal(err) + } + + if r := dareResidual(A, B, Q, R, res.X); r > 1e-9 { + t.Errorf("DARE residual = %e", r) + } + + for _, e := range res.Eig { + if cmplx.Abs(e) >= 1.0 { + t.Errorf("unstable closed-loop eigenvalue: %v (|λ|=%v)", e, cmplx.Abs(e)) + } + } +} + +func TestCare_MATLABValidated_3x3(t *testing.T) { + A := mat.NewDense(3, 3, []float64{ + -1.017, -0.224, 0.043, + -0.310, -0.516, -0.119, + -1.453, 1.800, -1.492, + }) + B := mat.NewDense(3, 2, []float64{ + 0.313, -0.165, + -0.865, 0.628, + -0.030, 1.093, + }) + Q := mat.NewDense(3, 3, []float64{1, 0, 0, 0, 1, 0, 0, 0, 1}) + R := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + + res, err := Care(A, B, Q, R, nil) + if err != nil { + t.Fatal(err) + } + + if r := careResidual(A, B, Q, R, res.X); r > 1e-10 { + t.Errorf("CARE residual = %e", r) + } + checkSymmetric(t, res.X, 1e-10) + + for _, e := range res.Eig { + if real(e) >= 0 { + t.Errorf("non-stable closed-loop eigenvalue: %v", e) + } + } +} + +func TestLqr_ClosedLoopStability(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}) + + res, err := Lqr(A, B, Q, R, nil) + if err != nil { + t.Fatal(err) + } + + for _, e := range res.Eig { + if real(e) >= 0 { + t.Errorf("LQR produced non-stable eigenvalue: %v", e) + } + } + + var bk, acl mat.Dense + bk.Mul(B, res.K) + acl.Sub(A, &bk) + + var eig mat.Eigen + eig.Factorize(&acl, mat.EigenNone) + vals := eig.Values(nil) + for _, v := range vals { + if real(v) >= 0 { + t.Errorf("closed-loop eigenvalue from A-BK: %v", v) + } + } +} + +func TestDlqr_ClosedLoopStability(t *testing.T) { + A := mat.NewDense(2, 2, []float64{1.1, 0.1, 0, 0.95}) + 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}) + + res, err := Dlqr(A, B, Q, R, nil) + if err != nil { + t.Fatal(err) + } + + for _, e := range res.Eig { + if cmplx.Abs(e) >= 1.0 { + t.Errorf("DLQR produced unstable eigenvalue: %v (|λ|=%v)", e, cmplx.Abs(e)) + } + } + + var bk, acl mat.Dense + bk.Mul(B, res.K) + acl.Sub(A, &bk) + + var eig mat.Eigen + eig.Factorize(&acl, mat.EigenNone) + vals := eig.Values(nil) + for _, v := range vals { + if cmplx.Abs(v) >= 1.0 { + t.Errorf("closed-loop eigenvalue from A-BK: %v (|λ|=%v)", v, cmplx.Abs(v)) + } + } +} + +func TestCare_NearZeroQ(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, -2, -3}) + B := mat.NewDense(2, 1, []float64{0, 1}) + eps := 1e-14 + Q := mat.NewDense(2, 2, []float64{eps, 0, 0, eps}) + R := mat.NewDense(1, 1, []float64{1}) + + res, err := Care(A, B, Q, R, nil) + if err != nil { + t.Fatal(err) + } + + xNorm := mat.Norm(res.X, 1) + if xNorm > 1e-6 { + t.Errorf("expected near-zero X for near-zero Q on stable plant, got norm = %e", xNorm) + } +} + +func TestDare_LargeSystem_5x5(t *testing.T) { + A := mat.NewDense(5, 5, []float64{ + 0.5, 0.1, 0.1, 0.1, 0.1, + 0.1, 0.5, 0.1, 0.1, 0.1, + 0.1, 0.1, 0.5, 0.1, 0.1, + 0.1, 0.1, 0.1, 0.5, 0.1, + 0.1, 0.1, 0.1, 0.1, 0.5, + }) + B := mat.NewDense(5, 5, []float64{ + 1, 0, 0, 0, 0, + 0, 1, 0, 0, 0, + 0, 0, 1, 0, 0, + 0, 0, 0, 1, 0, + 0, 0, 0, 0, 1, + }) + Q := mat.NewDense(5, 5, []float64{ + 1, 0, 0, 0, 0, + 0, 1, 0, 0, 0, + 0, 0, 1, 0, 0, + 0, 0, 0, 1, 0, + 0, 0, 0, 0, 1, + }) + R := mat.NewDense(5, 5, []float64{ + 1, 0, 0, 0, 0, + 0, 1, 0, 0, 0, + 0, 0, 1, 0, 0, + 0, 0, 0, 1, 0, + 0, 0, 0, 0, 1, + }) + + res, err := Dare(A, B, Q, R, nil) + if err != nil { + t.Fatal(err) + } + + if r := dareResidual(A, B, Q, R, res.X); r > 1e-9 { + t.Errorf("DARE residual = %e", r) + } + checkSymmetric(t, res.X, 1e-10) + + for _, e := range res.Eig { + if cmplx.Abs(e) >= 1.0 { + t.Errorf("unstable closed-loop eigenvalue: %v (|λ|=%v)", e, cmplx.Abs(e)) + } + } + + kr, kc := res.K.Dims() + if kr != 5 || kc != 5 { + t.Errorf("K dims = %d×%d, want 5×5", kr, kc) + } +} + +func TestLqe_ClosedLoopStability(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, -2, 3}) + G := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + C := mat.NewDense(1, 2, []float64{1, 0}) + Qn := mat.NewDense(2, 2, []float64{1, 0, 0, 1}) + Rn := mat.NewDense(1, 1, []float64{1}) + + res, err := Lqe(A, G, C, Qn, Rn, nil) + if err != nil { + t.Fatal(err) + } + + for _, e := range res.Eig { + if real(e) >= 0 { + t.Errorf("LQE produced non-stable eigenvalue: %v", e) + } + } + + var lc, alc mat.Dense + lc.Mul(res.K, C) + alc.Sub(A, &lc) + + var eig mat.Eigen + eig.Factorize(&alc, mat.EigenNone) + vals := eig.Values(nil) + for _, v := range vals { + if real(v) >= 0 { + t.Errorf("observer eigenvalue from A-LC: %v", v) + } + } +} + +func TestKalman_ClosedLoopStability(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) + } + + Qn := mat.NewDense(1, 1, []float64{1}) + Rn := mat.NewDense(1, 1, []float64{1}) + + res, err := Kalman(sys, Qn, Rn, nil) + if err != nil { + t.Fatal(err) + } + + for _, e := range res.Eig { + if real(e) >= 0 { + t.Errorf("Kalman produced non-stable eigenvalue: %v", e) + } + } +} + +func TestCare_NearZeroQ_UnstablePlant(t *testing.T) { + A := mat.NewDense(2, 2, []float64{0, 1, 2, 0}) + B := mat.NewDense(2, 1, []float64{0, 1}) + eps := 1e-14 + Q := mat.NewDense(2, 2, []float64{eps, 0, 0, eps}) + R := mat.NewDense(1, 1, []float64{1}) + + res, err := Care(A, B, Q, R, nil) + if err != nil { + t.Fatal(err) + } + + if r := careResidual(A, B, Q, R, res.X); r > 1e-10 { + t.Errorf("CARE residual = %e", r) + } + + xNorm := math.Abs(mat.Norm(res.X, 1)) + if xNorm < 1e-10 { + t.Error("expected non-zero X for near-zero Q on unstable plant") + } +} diff --git a/hinfsyn.go b/hinfsyn.go index 8251544..31d4d61 100644 --- a/hinfsyn.go +++ b/hinfsyn.go @@ -149,6 +149,7 @@ func HinfSyn(P *System, nmeas, ncont int) (*HinfSynResult, error) { Ak.Add(Ak, mulDense(ZpL, C2)) Bk := mulDense(Zp, L) + Bk.Scale(-1, Bk) Ck := denseCopy(F) Dk := mat.NewDense(m2, p2, nil) diff --git a/python-control-audit.md b/python-control-audit.md new file mode 100644 index 0000000..a37b207 --- /dev/null +++ b/python-control-audit.md @@ -0,0 +1,1368 @@ +# python-control Numerical Audit for controlsys Hardening + +Compiled from 100+ GitHub issues, 40+ PRs, discussions, test suites, and +14 Jupyter notebook examples from +[python-control/python-control](https://github.com/python-control/python-control). + +Focus: numerical discrepancies vs MATLAB, edge cases, and regression patterns +that apply to our Go implementation. + +--- + +## Table of Contents + +1. [Riccati Solvers (CARE/DARE)](#1-riccati-solvers-caredare) +2. [LQR / LQE / Kalman](#2-lqr--lqe--kalman) +3. [Lyapunov Solvers](#3-lyapunov-solvers) +4. [Pole Placement](#4-pole-placement) +5. [Controllability & Observability](#5-controllability--observability) +6. [Gramians & Hankel Singular Values](#6-gramians--hankel-singular-values) +7. [Canonical Forms & Modal Decomposition](#7-canonical-forms--modal-decomposition) +8. [Transfer Function / State Space Conversion](#8-transfer-function--state-space-conversion) +9. [System Interconnection (Feedback, Series, Parallel)](#9-system-interconnection) +10. [Stability Margins](#10-stability-margins) +11. [Frequency Response & Bode Plots](#11-frequency-response--bode-plots) +12. [Time-Domain Simulation](#12-time-domain-simulation) +13. [Discretization (c2d / d2c)](#13-discretization-c2d--d2c) +14. [Model Reduction (Balanced Realization)](#14-model-reduction) +15. [System Norms (H2, H-infinity)](#15-system-norms-h2-h-infinity) +16. [DC Gain & Zeros](#16-dc-gain--zeros) +17. [Nyquist Plots](#17-nyquist-plots) +18. [Numerical Primitives & General Patterns](#18-numerical-primitives--general-patterns) + +--- + +## 1. Riccati Solvers (CARE/DARE) + +### 1.1 Symmetry Check Must Be Scale-Aware + +**Source**: Issue #1174, PR #348 + +The elementwise check `(M - M.T) < eps` is broken in two ways: +- Missing `abs()`: small negative roundoff passes the check incorrectly +- Fixed `eps` doesn't scale with matrix magnitude + +**Correct approach** (from scipy): +``` +norm(M - M.T, 1) > spacing(norm(M, 1)) * 100 +``` + +**Test case**: +```go +// Matrix that is symmetric up to roundoff +Q := mat.NewDense(2, 2, []float64{1.0, 0.5 + 1e-16, 0.5 - 1e-16, 1.0}) +// Should pass symmetry check +``` + +### 1.2 DARE Wrong Results vs MATLAB + +**Source**: Issue #8, #157 + +scipy's `solve_discrete_are` uses van Dooren's generalized eigenvalue method; +MATLAB uses Arnold-Laub (SLICOT sb02od) which is more robust for +ill-conditioned A matrices. + +**Test case** (ill-conditioned): +```go +// System where scipy fails but MATLAB succeeds +// A with condition number > 1e8 +A := mat.NewDense(2, 2, []float64{1e4, 1, 0, 1e-4}) +B := mat.NewDense(2, 1, []float64{1, 1}) +Q := eye(2) +R := eye(1) +// Should produce stabilizing solution, not error +``` + +### 1.3 Generalized DARE with S != 0, E != I + +**Source**: Issue #36 (open 8+ years), python-control test suite + +scipy only handles S=0, E=I. MATLAB handles the full generalized form. + +**Test matrices** (from python-control mateqn_test.py): +```go +A := []float64{-2, -1, -1, -1} // 2x2 +Q := []float64{0, 0, 0, 1} +B := []float64{1, 0, 0, 4} +R := []float64{2, 0, 0, 1} +S := []float64{0, 0, 0, 0} +E := []float64{2, 1, 1, 2} +// Verify: A'XE + E'XA - (E'XB+S)*inv(R)*(B'XE+S') + Q = 0 +``` + +### 1.4 Q/R Positive Definiteness Validation + +**Source**: Issue #252, #274 + +LQR/DARE should validate that the combined cost `[Q N; N' R]` is positive +semi-definite before solving. Non-PD costs produce eigenvalues with real parts +~1e-16 that flip sign randomly. + +**Test case**: +```go +// Non-PD cost should error, not silently produce garbage +Q := mat.NewDense(2, 2, []float64{1, 0, 0, 0}) +R := mat.NewDense(1, 1, []float64{-1}) // negative R +// Lqr(A, B, Q, R) should return error +``` + +### 1.5 CARE/DARE Verification Pattern + +Always verify the residual equation: +- **CARE**: `A'X + XA - XBR^{-1}B'X + Q = 0` +- **DARE**: `X = A'XA - A'XB(B'XB+R)^{-1}B'XA + Q` +- **Always** check closed-loop stability: eigenvalues in LHP (continuous) or + inside unit circle (discrete) + +--- + +## 2. LQR / LQE / Kalman + +### 2.1 dlqe: Filter Gain vs Predictor Gain + +**Source**: Issue #1173 (open) + +python-control's `dlqe` returns the predictor gain: +``` +L_pred = A*P*C'*(C*P*C'+R)^{-1} +``` +MATLAB returns the filter gain: +``` +L_filt = P*C'*(C*P*C'+R)^{-1} +``` + +When A is singular, you **cannot** recover filter gain from predictor gain. + +**Test case**: +```go +// Singular A -- filter vs predictor gain differs fundamentally +A := mat.NewDense(2, 2, []float64{0, 1, 0, 0}) // double integrator +B := mat.NewDense(2, 1, []float64{0, 1}) +C := mat.NewDense(1, 2, []float64{1, 0}) +Qn := eye(2) +Rn := eye(1) +// L_filt = P*C'*inv(C*P*C'+Rn) +// L_pred = A*L_filt -- but A is singular! +``` + +### 2.2 lqe Default Noise Input Matrix + +**Source**: Issue #1159 (open) + +When calling `lqe(sys, QN, RN)` without explicit G (disturbance input matrix), +the default differs between MATLAB/Octave and python-control. Dimension +mismatch errors result. + +### 2.3 LQR/LQE Return Type Consistency + +**Source**: Issue #439, PR #477 + +`lqr` returned eigenvalues as 1D; `lqe` returned 2D matrix. Both should +return consistent shapes. + +### 2.4 MATLAB-Validated Gramian Test Vectors + +**Source**: python-control statefbk_test.py + +```go +A := []float64{1, -2, 3, -4} // 2x2 NON-SYMMETRIC (critical!) +B := []float64{5, 6, 7, 8} // 2x2 +C := []float64{4, 5, 6, 7} // 2x2 +D := []float64{13, 14, 15, 16} // 2x2 + +// Continuous controllability Gramian (from MATLAB): +Wc := []float64{18.5, 24.5, 24.5, 32.5} + +// Continuous observability Gramian (from MATLAB): +Wo := []float64{257.5, -94.5, -94.5, 56.5} + +// Also test discrete versions after c2d with dt=0.2 +``` + +--- + +## 3. Lyapunov Solvers + +### 3.1 Skew-Symmetric A (Eigenvalues Symmetric About Imaginary Axis) + +**Source**: Issue #342 + +For A where eigenvalues of A and -A are close (e.g., orthogonal/skew-symmetric +matrices), the continuous Lyapunov solver (Bartels-Stewart) fails. The discrete +solver works fine for the same system. + +**Test case**: +```go +// Skew-symmetric A from orthogonal group +// A and -A have shared eigenvalues +A := mat.NewDense(3, 3, []float64{ + 0, -1, 0, + 1, 0, -1, + 0, 1, 0, +}) +Q := eye(3) +// Lyap(A, Q) should detect this and return meaningful error +// DLyap(sqrt(0.9)*A, Q) should work fine +``` + +### 3.2 Workspace Reuse Pattern + +Pre-allocate Lyapunov workspace for repeated solves (e.g., inside iterative +Riccati or H-infinity synthesis loops). + +--- + +## 4. Pole Placement + +### 4.1 Ackermann Return Shape + +**Source**: Issue #1190, PR #1195 + +`place_acker` returned 1D array instead of 2D. The bug was +`K[-1, :]` (collapses dimension) vs `K[-1:, :]` (preserves 2D). + +**Test case**: +```go +A := mat.NewDense(2, 2, []float64{0, 1, 0, 0}) +B := mat.NewDense(2, 1, []float64{0, 1}) +poles := []complex128{-1 + 1i, -1 - 1i} +K := Acker(A, B, poles) +// K should be (1, 2) not (2,) +``` + +### 4.2 Ackermann Must Reject MIMO + +**Source**: Issue #1190, #1100 + +Ackermann's formula is SISO-only. Must error on MIMO input, not produce +garbage. + +### 4.3 Ackermann Input Validation Bug + +**Source**: Issue #1100 + +The python-control `acker` function validated inputs via `_ssmatrix()` but then +continued using the raw (unvalidated) `A`, `B` variables instead of the +converted `a`, `b`. + +### 4.4 Wrong Poles from Old Algorithm + +**Source**: Issue #117 + +Old Ackermann-based multi-input placement returned wrong eigenvalues. +MATLAB's `place` uses Tits-Yang robust algorithm. + +**Test case** (from scipy example): +```go +A := mat.NewDense(4, 4, []float64{ + 1.380, -0.2077, 6.715, -5.676, + -0.5814, -4.290, 0, 0.6750, + 1.067, 4.273, -6.654, 5.893, + 0.0480, 4.273, 1.343, -2.104, +}) +B := mat.NewDense(4, 2, []float64{ + 0, 5.679, + 1.136, 1.136, + 0, 0, + -3.146, 0, +}) +P := []complex128{-0.5+1i, -0.5-1i, -5.0566, -8.6659} +K := Place(A, B, P) +// Verify: eig(A - B*K) matches P +``` + +### 4.5 Repeated Poles with Insufficient Rank + +**Source**: python-control statefbk_test.py + +```go +// 3 repeated poles but rank(B)=2 -- should fail +P := []complex128{-0.5, -0.5, -0.5, -8.6659} +// Place(A, B, P) should return error +``` + +### 4.6 Regression: Complex Conjugate Poles on Unstable System + +**Source**: Issue #177 + +```go +A := mat.NewDense(2, 2, []float64{0, 1, 100, 0}) // unstable +B := mat.NewDense(2, 1, []float64{0, 1}) +P := []complex128{-20+10i, -20-10i} +K := Place(A, B, P) +// Must not error with "complex pair on real eigenvalue" +``` + +--- + +## 5. Controllability & Observability + +### 5.1 Input Validation for 1D Arrays + +**Source**: Issue #1097, PR #1099 + +Passing 1D `B` (e.g., `[1, 1]` instead of column `[[1], [1]]`) produces wrong +dimensions silently. + +**Test case**: +```go +A := mat.NewDense(2, 2, []float64{1, 2, 3, 4}) +B := mat.NewDense(2, 1, []float64{5, 7}) +Wc := Ctrb(A, B) +// Wc should be 2x2: [B, AB] = [[5, 19], [7, 43]] +``` + +### 5.2 Duality Test + +```go +// Ctrb(A, B) == Obsv(A', B')' +// This catches transposition bugs +``` + +### 5.3 Performance: Reuse Previous Power + +**Source**: PR #941 + +Don't call `matrix_power(A, i)` each iteration. Reuse: `A^i = A * A^{i-1}`. + +--- + +## 6. Gramians & Hankel Singular Values + +### 6.1 Discrete-Time Gramians Must Use Discrete Lyapunov + +**Source**: Issue #967 + +python-control silently used continuous Lyapunov for discrete systems. +Must use `A*X*A' - X + B*B' = 0` (discrete), not `A*X + X*A' + B*B' = 0`. + +### 6.2 Cholesky-Factored Gramians: Preserve Schur Form + +**Source**: Issue #1123 (open) + +When computing Cholesky-factored Gramians, transposing A before passing to the +SLICOT routine destroys Schur form triangularity, forcing re-QR which +introduces epsilon errors. Use `tra='T'` flag instead of explicit transpose. + +### 6.3 MATLAB-Validated HSV Test + +**Source**: python-control modelsimp_test.py + +```go +A := []float64{1, -2, 3, -4} // 2x2 +B := []float64{5, 7} // 2x1 +C := []float64{6, 8} // 1x2 +D := []float64{9} // 1x1 +// Hankel singular values (from MATLAB): +hsv := []float64{24.42686, 0.5731395} +``` + +--- + +## 7. Canonical Forms & Modal Decomposition + +### 7.1 Repeated Eigenvalues Blow Up Eigenvector-Based Modal Form + +**Source**: Issue #318, PR #495 + +Eigenvector matrix T becomes singular (entries ~4.5e15) for repeated +eigenvalues. **Our codebase already handles this with Schur fallback** +(commit 982472e). + +### 7.2 bdschur Test Parameterization + +**Source**: python-control canonical_test.py + +```go +// Real distinct eigenvalues +eigvals := []complex128{-1,-2,-3,-4,-5} +// condmax=nil -> blksizes=[1,1,1,1,1] + +// Force all into one block +// condmax=1.01 -> blksizes=[5] + +// Repeated eigenvalues +eigvals = []complex128{-1,-1,-2,-2,-2} +// condmax=nil -> blksizes=[2,3] + +// Complex pairs +eigvals = []complex128{-1+1i,-1-1i,-2+2i,-2-2i,-2} +// condmax=nil -> blksizes=[2,2,1] +``` + +### 7.3 Defective 2x2 Blocks + +A 2x2 block `[[a, b], [-b, a]]` with near-zero `b` is defective (repeated +real eigenvalue), not a complex pair. + +### 7.4 Reachable/Observable Form Validation + +**Source**: python-control canonical_test.py + +```go +// Start with known companion form, apply random transform, convert back +coeffs := []float64{1.0, 2.0, 3.0, 4.0, 1.0} +A_true := companion(coeffs) +T_true := randomOrthogonal(n) +A := inv(T_true) * A_true * T_true +// Canon(ss(A,B,C,D), "reachable") should recover A_true +``` + +Edge cases: +- Unreachable systems -> error +- Unobservable systems -> error +- MIMO systems -> error for reachable/observable forms + +--- + +## 8. Transfer Function / State Space Conversion + +### 8.1 ss2tf Produces Spurious Zeros for Ill-Conditioned Systems + +**Source**: Issue #1068 + +```go +// DC motor with widely separated poles +// Poles at 0, -10, -990 -> coefficient range spans many orders +A := mat.NewDense(3, 3, []float64{ + 0, 1, 0, + 0, -10, 1, + 0, 0, -990, +}) +B := mat.NewDense(3, 1, []float64{0, 0, 1}) +C := mat.NewDense(1, 3, []float64{1, 0, 0}) +// ss2tf should NOT produce spurious zeros at +/-6.6e9j +``` + +### 8.2 MIMO tf2ss Near-Cancellation + +**Source**: Issue #240 + +```go +// 2x2 MIMO with 1e-10 perturbation in denominator +// H11 = 0.25/(s+1), H12 = 0/(1), H21 = 0/(1), H22 = 0.25/(s+1+1e-10) +// Step response should give ~0.25 for both channels, not 0 +``` + +### 8.3 Static Gain (Zero States) + +**Source**: Issue #244, PR #129 + +```go +// tf2ss of static gain must produce 0-state system, not crash +tf := NewTransferFunc([]float64{23}, []float64{46}) +sys := tf.StateSpace() +// sys.A should be 0x0, sys.D should be [[0.5]] +``` + +### 8.4 High-Order TF with Wide Coefficient Range + +**Source**: Issue #935 + +16th-order TF with coefficients spanning 1e-31 to 1e-8 produced unstable +state-space via SLICOT (correct via scipy). Guard against high polynomial +order + wide dynamic range. + +### 8.5 ss2tf with D=0 (Strictly Proper) + +**Source**: Issue #156 + +Simple `1/(s+1)` (D=0) crashed python-control's ss2tf. Ensure strictly proper +systems are handled. + +### 8.6 SIMO tf2ss Shape Mismatch + +**Source**: Issue #235 + +Single-input, multiple-output TF conversion crashed with broadcast error in +common denominator computation. + +--- + +## 9. System Interconnection + +### 9.1 Feedback with ZPK is Numerically Dangerous + +**Source**: Issue #1067, #1102 + +```go +// This blows up in polynomial domain (overshoot ~1e52) +sys := NewZPK([]complex128{}, []complex128{-2, -2.0/3.0}, 10.314) +cl := Feedback(sys, 1.0, -1) +// Step response should be bounded, not 1e52 + +// Same system via state-space works perfectly +// LESSON: Always compute feedback in state-space, not polynomial form +``` + +### 9.2 MIMO Scalar Multiply + +**Source**: PR #1078 + +`2 * mimo_sys` should scale all channels, not fill all entries with 2. + +### 9.3 Off-by-One in Connect + +**Source**: Issue #421, PR #474 + +Negative feedback output indexing had boundary check bug: +`outp < 0 and -outp >= -sys.outputs` should be `-outp <= sys.outputs`. + +### 9.4 MIMO Feedback Regression + +**Source**: Issue #1172 + +MIMO feedback with numpy array controller path incorrectly converted to +TransferFunction, triggering "MIMO not implemented" error. + +### 9.5 Series State Ordering vs MATLAB + +**Source**: Issue #725 (closed, wontfix) + +python-control's `series(sys2, sys1)` produces state vector `(x2, x1)` while +MATLAB produces `(x1, x2)`. Documented difference, not a bug. + +--- + +## 10. Stability Margins + +### 10.1 Discrete-Time Margins Need Z-Domain Treatment + +**Source**: Issue #465, #523, PR #469, #566 + +DT systems need `z = exp(j*w*dt)` substitution, not `s = jw`. Polynomial +methods for finding |H(z)|=1 crossings are numerically fragile when +num/den coefficient magnitudes differ greatly. + +**Fallback strategy** (PR #566): detect numerical inaccuracy via +`||num(z)*num(1/z)|| < 1e-3 * ||den(z)*den(1/z)||` and fall back to FRD +interpolation. + +### 10.2 Gain Margin: Reciprocal Bug + +**Source**: Issue #947 + +Gain margin was returned as magnitude (0.1) instead of margin (10 = 1/0.1). + +### 10.3 Integer vs Float Input Types + +**Source**: Issue #462 + +`margin(tf(50000, den))` (integer) gave wrong results; +`margin(tf(50000., den))` (float) was correct. + +### 10.4 FRD Margin: Bad Seed for Root Finder + +**Source**: Issue #61 (open) + +Phase crossover frequency finder seeded with lowest frequency, converging to +wrong solution. Should seed with median frequency. + +### 10.5 Multiple Crossings + +**Source**: Issue #784 (open) + +Only finding one gain margin when phase crosses -180 multiple times. All +crossings should be reported. + +### 10.6 MATLAB-Validated Margin Test + +```go +// Type 0 system with clear margins +num := []float64{1} +den := []float64{1, 3, 3, 1} // (s+1)^3 +// MATLAB: GM = 8 (18.06 dB) at wg = 1.732 rad/s +// PM = 41.4 deg at wp = 0.767 rad/s +``` + +--- + +## 11. Frequency Response & Bode Plots + +### 11.1 Phase Wrapping: Must Unwrap Like MATLAB + +**Source**: Issue #467, #1179 + +python-control wraps phase to [-180, 180], producing discontinuous jumps. +MATLAB does continuous unwrapping. + +```go +// 1/s^3 should show -270 deg (MATLAB), not +90 deg +sys := NewTransferFunc([]float64{1}, []float64{1, 0, 0, 0}) +// Phase at any frequency should be -270, not +90 +``` + +### 11.2 Phase Through Zeros: Wrong Direction + +**Source**: Issue #1179 (open) + +Third-order system with undamped zeros: phase jumps go in wrong direction. +MATLAB shows -90 -> +90 -> -90; python-control shows -90 -> -270 -> -450. + +### 11.3 High-Order MIMO Frequency Response + +**Source**: Issue #860 + +24x18 MIMO system: `frequency_response()` on derived TFs (H/(1+P)) returns +noisy/incorrect results due to polynomial coefficient cancellation in +high-order TF algebra. Manual complex arithmetic on state-space is correct. + +**Lesson**: Evaluate frequency response in state-space domain +(`C*(jwI-A)^{-1}*B + D`), not via polynomial evaluation. + +### 11.4 Phase Crossover with Poles at Origin + +**Source**: Issue #1105, PR #1106 + +`phase_crossover_frequencies` crashes on `200/(s^3 + 21s^2 + 20s)`. + +### 11.5 Bode Phase Leak Between Systems + +**Source**: Issue #862 + +When plotting multiple systems, phase wrapping state leaked between systems, +producing different phases for identical systems. + +--- + +## 12. Time-Domain Simulation + +### 12.1 Marginally Stable Systems: Diverging Instead of Oscillating + +**Source**: Issue #384 + +Closed-loop system with purely imaginary poles produced unstable diverging +response instead of sustained oscillation. Octave gave correct sinusoidal. + +### 12.2 Integrator Step Response + +**Source**: Issue #470 + +`(0.8s+1)/(s^2+s)` should give exponential step response starting at 0.8->1.0 +but gave a ramp y=t. Pole at origin mishandled. + +### 12.3 Discrete Impulse Scaling + +**Source**: Issue #974 + +DT impulse response was scaled by extra `1/T`. Accumulator `H(z)=Tz/(z-1)` +with T=0.1: python-control gave 1.0, MATLAB/Octave gave 0.1 (correct). + +### 12.4 zpk vs tf Give Different Impulse Responses + +**Source**: Issue #1062 + +Same system defined as zpk vs tf gave unstable vs stable impulse responses. +Internal representation mismatch. + +### 12.5 Delayed Dirac Input Ignored + +**Source**: Issue #1152 + +Dirac impulse applied after index 5 completely ignored. ODE solver step size +determination skipped over the impulse. + +### 12.6 MATLAB-Validated Step Response + +**Source**: python-control timeresp_test.py + +```go +A := []float64{1, -2, 3, -4} // 2x2 +B := []float64{5, 7} // 2x1 +C := []float64{6, 8} // 1x2 +D := []float64{9} // 1x1 +t := linspace(0, 1, 10) +// Step response (from MATLAB): +y := []float64{9., 17.6457, 24.7072, 30.4855, 35.2234, + 39.1165, 42.3227, 44.9694, 47.1599, 48.9776} +``` + +### 12.7 MATLAB-Validated Step Info + +**Source**: python-control timeresp_test.py + +```go +// G(s) = (s^2 + 5s + 5) / (s^4 + 1.65s^3 + 5s^2 + 6.5s + 2) +// From MATLAB online help: +// RiseTime: 3.8456 +// SettlingTime: 27.9762 +// Overshoot: 7.4915% +// SteadyStateValue: 2.5 +``` + +### 12.8 Non-Minimum Phase Step Info + +```go +// G(s) = (-s+1)/(s^2+s+1) +// Undershoot: 28% +// Overshoot: 20.84% +``` + +### 12.9 Overshoot: Use dcgain(), Not Final Simulated Value + +**Source**: PR #555 + +Overshoot relative to `dcgain()`, not last simulation sample. Non-strictly +proper TFs give wrong overshoot with final value approach. + +--- + +## 13. Discretization (c2d / d2c) + +### 13.1 Matched Method DC Gain Normalization + +**Source**: Issue #950, PR #951 + +Matched pole-zero method used polynomial leading coefficient (`sgain`) instead +of `dcgain()` for gain normalization, producing ~10x error. + +**Correct approach**: Normalize so that `sys_ct(0) == sys_dt(1)`. + +**Test case**: +```go +// For all discretization methods, verify: +// dcgain(c2d(sys, dt)) == dcgain(sys) +sys := NewSS(A, B, C, D) +for _, method := range []string{"zoh", "foh", "tustin", "matched"} { + sysd := Discretize(sys, dt, method) + assert(abs(DCGain(sysd) - DCGain(sys)) < tol) +} +``` + +### 13.2 Direct State-Space Discretization + +**Source**: Issue #23 + +Never discretize by converting to TF first. Use matrix exponential +`Ad = expm(A*T)` directly. + +### 13.3 ZPK Timebase Default + +**Source**: PR #1064 + +`zpk()` defaulted timebase to `nil` (discrete) instead of 0 (continuous). +All factory functions should use consistent default. + +--- + +## 14. Model Reduction + +### 14.1 Marginally Stable Systems: Warn, Don't Error + +**Source**: Issue #1166, PR #1074 + +Double integrator `A=[[0,1],[0,0]]` (poles at origin) was rejected as +"unstable" by balanced reduction. Should warn, not error. Eigenvalues +at exactly 0 or on imaginary axis need tolerance-aware stability check. + +### 14.2 MATLAB-Validated Balanced Reduction + +**Source**: python-control modelsimp_test.py + +```go +// TF: num=[1,11,45,32], den=[1,15,60,200,60] +// Controllable canonical form (4 states) +// Reduced to order 2 (truncate): +Ar := []float64{-1.958, -1.194, -1.194, -0.8344} // from MATLAB +Br := []float64{0.9057, 0.4068} +// Note: state ordering may differ -- test via similarity transform +``` + +### 14.3 Unstable System Warning Control + +Test that `warn_unstable=false` suppresses warnings for intentionally unstable +systems. + +### 14.4 Chained Transforms Corrupt Data + +**Source**: Issue #1177 (open) + +Combining similarity transforms + balanced reduction corrupts state data. +Test sequential operations for data integrity. + +--- + +## 15. System Norms (H2, H-infinity) + +### 15.1 MATLAB-Validated Norm Test Cases + +**Source**: python-control sysnorm_test.py + +```go +// 1st order stable: G = 1/(s+1) +// Hinf = 1.0, H2 = 0.707106781186547 + +// 1st order unstable: G = 1/(1-s) +// Hinf = 1.0, H2 = inf (with warning) + +// 2nd order with imaginary poles: G = 1/(s^2+1) +// Hinf = inf (marginally stable), H2 = inf + +// 3rd order MIMO (from MATLAB): +A := []float64{ + -1.017, -0.224, 0.043, + -0.310, -0.516, -0.119, + -1.453, 1.800, -1.492, +} // 3x3 +B := []float64{0.313, -0.165, -0.865, 0.628, -0.030, 1.093} // 3x2 +C := []float64{1.109, 0.077, -1.114, -0.864, -1.214, -0.007} // 2x3 +// Hinf = 4.276759162964244 (from MATLAB) +// H2 = 2.237461821810309 (from MATLAB) +// Also test after discretization with dt=0.1 +``` + +### 15.2 Hinfsyn Sensitivity to Realization + +**Source**: Issue #367 (open) + +Same plant: MATLAB finds stabilizing H-infinity controller, python-control +says "cannot be found". Root cause: slightly different tf2ss realizations +produce numerically different state-space matrices. + +**Lesson**: hinfsyn is extremely sensitive to the state-space realization. +Consider pre-balancing or using balanced realization before synthesis. + +### 15.3 Hinfsyn MATLAB/Octave Reference + +**Source**: python-control robust_test.py + +```go +// P = ss(-1, [1,1], [[1],[1]], [[0,1],[1,0]]) +// k, cl, gam, rcond = hinfsyn(P, 1, 1) +// From Octave (SB10AD): +// k.A = [[-3]], k.B = [[1]], k.C = [[-1]], k.D = [[0]] +``` + +--- + +## 16. DC Gain & Zeros + +### 16.1 Poles at Origin: Per-Channel DC Gain + +**Source**: Issue #128 (open) + +If any eigenvalue of A is 0, `dcgain()` returns NaN for ALL outputs, even for +channels that have finite DC gain. Should compute per-channel via +`C*(s*I-A)^{-1}*B + D` evaluated at s=0 for each I/O pair. + +### 16.2 Pole-Zero Cancellation at Origin + +**Source**: Issue #127 (open) + +Both num and den have roots at s=0: should compute the limit (ratio of lowest +non-zero coefficients), not return NaN. + +### 16.3 Consistent Evaluation at s=0 + +**Source**: Issue #532 + +`tf([1],[1,0])(0)` returns `inf+0j`; SS version raises `LinAlgError`. +`dcgain()` returns inf vs nan depending on representation. Must be consistent. + +### 16.4 Non-Square MIMO Zeros + +**Source**: Issue #859 + +Transmission zeros for non-square MIMO (e.g., 3-in 6-out) require generalized +eigenvalue approach that handles rectangular matrices. + +### 16.5 Small Real Parts from Roundoff + +**Source**: Issue #768 + +Zeros reported as `-4.72e-16 +/- 1j` instead of clean `+/- 1j`. Inherent +floating-point limitation -- consider optional cleanup threshold. + +--- + +## 17. Nyquist Plots + +### 17.1 Indentation Around Imaginary-Axis Poles + +**Source**: PR #722 + +Default indentation radius (0.1) around poles near imaginary axis could miss +right-half-plane closed-loop poles. Reduced to 1e-4. + +### 17.2 DT Poles at z=0 and z=1 + +**Source**: PR #885 + +Nyquist plot failed for DT TFs with poles at z=0 and z=1. Need special +handling for these discrete-time singularities. + +### 17.3 Indentation Direction + +**Source**: Issue #1191 (open) + +Indentation curves around imaginary-axis poles go counter-clockwise instead of +standard clockwise. + +--- + +## 18. Numerical Primitives & General Patterns + +### 18.1 Use solve(), Never inv() + +**Source**: PR #91, #101, #200 + +Replace ALL `inv(M) * X` with `solve(M, X)`. This applies to: +- Riccati gain computation +- Canonical form transforms +- Feedback computation +- DC gain (C * inv(-A) * B) + +### 18.2 Use eigvals(), Never roots(poly(A)) + +**Source**: PR #91, Issue #84 + +Computing poles via `det(sI-A)` -> roots is O(n!) worse numerically than +direct eigenvalue decomposition. + +### 18.3 Separate Real/Imaginary Tolerances for Pole Matching + +**Source**: PR #345 + +```go +realTol := math.Sqrt(eps * float64(nInputs*nOutputs)) +imagTol := 2 * realTol +// Poles with |imag| < imagTol are forced to real +``` + +### 18.4 Complex-to-Real Casting After Polynomial Reconstruction + +**Source**: PR #1086 + +After reconstructing polynomials from complex poles/zeros, cast coefficients +to real to avoid spurious complex warnings. + +### 18.5 Non-Symmetric A Matrices Are Critical for Testing + +**Source**: CLAUDE.md, python-control test suite + +Nearly every test suite uses `A = [[1, -2], [3, -4]]` or similar +non-symmetric matrices. Diagonal or symmetric A hides transposition bugs. + +### 18.6 Replace det() Singularity Check with rank() + +**Source**: PR #91, #101 + +`abs(det(F)) < threshold` is unreliable. Use `matrix_rank()` (SVD-based with +proper numerical threshold) instead. + +### 18.7 damp() for DT: Cast to Complex Before Log + +**Source**: Issue #646 + +Discrete-time systems with negative poles: `log(poles)` fails for real +negative values. Must cast to complex first: `log(complex(pole))`. + +### 18.8 StateSpace Constructor: Never Auto-Reduce + +**Source**: Issue #244 + +Don't call `_remove_useless_states()` in the constructor. `ss(0,0,1,0)` must +preserve the single state, not collapse to empty. + +### 18.9 Overshoot Relative to dcgain(), Not Simulation End + +**Source**: PR #555 + +For non-strictly proper systems, the final simulated value != DC gain. +Use `dcgain()` as the reference for percent overshoot calculation. + +--- + +## Appendix A: Test System Zoo + +Standard test systems used across python-control (with MATLAB reference values): + +| Name | A | B | C | D | Notes | +|------|---|---|---|---|-------| +| **Basic 2x2** | `[[1,-2],[3,-4]]` | `[[5],[7]]` | `[[6,8]]` | `[[9]]` | Non-symmetric A, primary test system | +| **Basic 2x2 MIMO** | `[[1,-2],[3,-4]]` | `[[5,6],[7,8]]` | `[[4,5],[6,7]]` | `[[13,14],[15,16]]` | MIMO variant | +| **Double integrator** | `[[0,1],[0,0]]` | `[[0],[1]]` | `[[1,0]]` | `[[0]]` | Marginally stable, poles at 0 | +| **Unstable** | `[[0,1],[100,0]]` | `[[0],[1]]` | `[[1,0]]` | `[[0]]` | Unstable open-loop | +| **1/(s+1)** | `[[-1]]` | `[[1]]` | `[[1]]` | `[[0]]` | Hinf=1, H2=0.7071 | +| **1/(s^2+1)** | `[[0,1],[-1,0]]` | `[[0],[1]]` | `[[1,0]]` | `[[0]]` | Marginally stable, imaginary poles | +| **DC motor** | 3x3, poles at 0,-10,-990 | | | | Wide pole spread | +| **4-state (scipy)** | 4x4 dense | 4x2 | | | Pole placement reference | + +## Appendix B: Priority Matrix + +| Priority | Area | Issue | Our Status | +|----------|------|-------|------------| +| **DONE** | Modal form | Schur fallback for repeated eigenvalues | Commit 982472e | +| **DONE** | Input validation | Improper TF, descriptor Riccati, Q PSD | Commit 44d7971 | +| **DONE** | Numerical hardening | Python-control PR audit fixes | Commit 2392746 | +| HIGH | Riccati | Scale-aware symmetry check | Verify current impl | +| HIGH | DARE | Ill-conditioned A robustness | Test coverage | +| HIGH | Margins | DT margin computation | Test coverage | +| HIGH | Feedback | ZPK/polynomial blow-up | Test with pathological gains | +| HIGH | ss2tf | Spurious zeros for ill-conditioned systems | Test coverage | +| MED | dlqe | Filter vs predictor gain convention | Document + test | +| MED | Gramians | DT Gramian correctness | Test coverage | +| MED | Model reduction | Marginally stable handling | Test coverage | +| MED | DC gain | Poles at origin, per-channel | Test + fix | +| MED | Phase | Unwrapping like MATLAB | Test coverage | +| LOW | Acker | Return shape, MIMO rejection | Verify | +| LOW | c2d matched | DC gain normalization | Test coverage | +| LOW | Norms | Discrete-time H2/Hinf | MATLAB-validated tests | + +## Appendix C: Key python-control PRs for Reference + +| PR | Title | Key Pattern | +|----|-------|-------------| +| #91 | eigvals for poles, solve for feedback | Never use roots(poly(A)) | +| #101 | Replace inv() with solve() | Numerical stability | +| #200 | Fix observable canonical form | solve() + rank check | +| #206 | _common_den rewrite | Pole matching algorithm | +| #345 | Root precision tolerance | Separate real/imag tolerances | +| #348 | Machine-epsilon symmetry check | Scale-aware tolerance | +| #469 | DT stability margins | z-domain treatment | +| #495 | bdschur for modal form | Condition number control | +| #566 | Margin fallback to FRD | Auto-detect numerical issues | +| #683 | LQR using scipy | Dual-method approach | +| #951 | Matched c2d DC gain | Use dcgain() not leading coeff | +| #1078 | MIMO scalar multiply | Element-wise, not filled matrix | +| #1195 | place_acker return shape | Preserve 2D with [-1:, :] | + +## Appendix D: Real-World Systems from Jupyter Notebooks + +Extracted from 14 Caltech CDS 110/112 course notebooks. These are physically +meaningful systems that stress the library differently than toy 2x2 examples. + +### D.1 PVTOL Aircraft (6-state, 2-input MIMO) + +The most exercised system across notebooks. Parameters: m=4, J=0.0475, +r=0.25, g=9.8, c=0.05. + +Linearized at hover (xe=0, ue=[0, m*g]): + +```go +A := []float64{ + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1, + 0, 0, -9.8, -0.0125, 0, 0, + 0, 0, 0, 0, -0.0125, 0, + 0, 0, 0, 0, 0, 0, +} // 6x6 +B := []float64{ + 0, 0, + 0, 0, + 0, 0, + 0.25, 0, + 0, 0.25, + 5.2632, 0, +} // 6x2, r/J=5.2632 +C_pos := eye(3, 6) // measures x, y, theta only +``` + +**LQR weights tested** (3 cases): +- Identity: Qx=I(6), Qu=I(2) +- Physical: Qx=diag(100, 10, 180/pi/5, 0, 0, 0), Qu=diag(10, 1) +- High-drag (c=20): Qx=diag(10, 100, 180/pi/5, 0, 0, 0), Qu=diag(10, 1) + +**Kalman with non-diagonal Qw**: +```go +Qv := []float64{1e-2, 0, 0, 1e-2} // 2x2 disturbance +Qw := []float64{ + 2e-4, 0, 1e-5, + 0, 2e-4, 1e-5, + 1e-5, 1e-5, 1e-4, +} // 3x3 measurement noise, cross-terms! +``` + +**Why valuable**: Open-loop unstable (double integrator in theta), MIMO, +real aircraft parameters, exercises LQR + Kalman + margins simultaneously. + +### D.2 Coupled Spring-Mass (4-state, 2-output) + +m=1, c=0.1, k=2. + +```go +A := []float64{ + 0, 0, 1, 0, + 0, 0, 0, 1, + -4, 2, -0.1, 0, + 2, -4, 0, -0.1, +} // 4x4 +B := []float64{0, 0, 0, 2} // 4x1 +C := []float64{1, 0, 0, 0, 0, 1, 0, 0} // 2x4 +``` + +**Why valuable**: Has transmission zero (dip in frequency response of q2 +at ~2 rad/s). Lightly damped. Good for SIMO transfer function and +frequency response testing. + +### D.3 Inverted Pendulum (unstable SISO) + +```go +// P(s) = 1/(s^2 + 0.1s - 1), one RHP pole +A := []float64{0, 1, 1, -0.1} // 2x2 +B := []float64{0, 1} +C := []float64{1, 0} +D := []float64{0} +``` + +**Why valuable**: Open-loop unstable. Requires Nyquist encirclement +N=-1 for closed-loop stability. With PD controller kp=10, kd=2: +`L(s) = (2s+10)/(s^2+0.1s-1)`. + +### D.4 Maglev System (unstable, real hardware) + +Caltech maglev experiment. Parameters: m=0.2, g=9.81, km=3.13e-4. + +2-state, 1-input, 1-output. Open-loop unstable (one RHP pole). + +Analog controller (RC circuit): +```go +// C(s) = -0.5 * (0.024s + 1)/(0.002s + 1) +// Lead compensator from actual hardware +``` + +**Why valuable**: Real hardware parameters. Tests Bode sensitivity +integral: `integral(ln|S(jw)|, 0, inf) = pi * sum(Re(unstable_poles))`. +Demonstrates fundamental performance limits from RHP poles. + +### D.5 Vehicle Steering (double integrator + observer) + +Bicycle model at v0=15 m/s, normalized to double integrator: + +```go +A := []float64{0, 1, 0, 0} // 2x2 +B := []float64{0, 1} +C := []float64{1, 0} +``` + +**Placement tested**: +- Feedback: wc=0.7, zeta=0.707 (standard) and wc=10 (high bandwidth) +- Observer: wo=1, zeta=0.7 +- Observer gain via duality: `L = Place(A', C', obs_poles)'` + +**Why valuable**: Tests observer+controller design, duality for observer +gain. Also tested in reverse (v=-2 m/s) which creates non-minimum phase +dynamics. + +### D.6 Predator-Prey (unstable complex poles) + +Lotka-Volterra: r=1.6, d=0.56, b=0.6, k=125, a=3.2, c=50. +2-state, 1-input. Linearized at interior equilibrium (~[21.05, 24.61]). + +**Why valuable**: Unstable complex eigenvalues from biological system. +Demonstrates sensitivity: changing r from 1.6 to 1.65 (<4%) destabilizes +proportional controller, requiring integral action (Ki=0.0001). + +### D.7 Cruise Control (nonlinear 1-state) + +m=1600 kg, Tm=190 N-m. Single velocity state. +Linearized: first-order P(s)=b/(s+a) at v=20 m/s. + +```go +// State feedback K=0.5, feedforward kf = -1/(C*inv(A-BK)*B) +// Integral gain ki=0.1 +// Robustness test: m varies from 1200 to 2000 kg +``` + +**Why valuable**: Simplest nonlinear system. Tests robustness to +parameter uncertainty (25% mass variation). + +### D.8 Servomechanism (margins edge case) + +J=100, b=10, k=1. Third-order with actuator. + +```go +// PI that creates UNSTABLE closed loop (Nyquist encirclement): +// C(s) = (s+1)/s, with plant P(s) = 0.02/(s^2 + 0.1s + 0.00966) + +// PI with reduced integral gain (stable): +// C(s) = (s+0.005)/s + +// PID (loop-shaped): +// kp=150, ki=30, kd=150 +``` + +**Why valuable**: Same plant produces unstable CL with kp=ki=1 but stable +CL with ki=0.005. Tests that margin computation correctly identifies +instability. Ziegler-Nichols tuning also demonstrated. + +## Appendix E: Example Scripts — Textbook Reference Values + +Extracted from Python scripts in `examples/` folder. These provide +published/textbook numerical values for verification. + +### E.1 Scherer et al. H-infinity / H2 Synthesis (Gold Standard) + +**Reference**: Scherer, Gahinet & Chilali, "Multiobjective Output-Feedback +Control via LMI Optimization", IEEE TAC, Vol. 42, No. 7, July 1997, Example 7. + +```go +A := []float64{ + 0, 10, 2, + -1, 1, 0, + 0, 2, -5, +} // 3x3, open-loop UNSTABLE (eigs ≈ -5.37, 1.69±2.50j) + +B1 := []float64{1, 0, 1} // 3x1 disturbance +B2 := []float64{0, 1, 0} // 3x1 control + +// H2 performance output +C1_h2 := []float64{0, 1, 0, 0, 0, 1, 0, 0, 0} // 3x3 +D12_h2 := []float64{0, 0, 1} // 3x1 + +// Measurement +C2 := []float64{0, 1, 0} // 1x3 +D21 := []float64{2} // 1x1 +``` + +**H2 synthesis**: `h2syn(P, nmeas=1, ncon=1)` +**Published H2 norm = 7.748350599360575** + +**H-infinity synthesis**: Same plant, different C1/D12: +```go +C1_hinf := []float64{1, 0, 0, 0, 0, 0} // 2x3 +D12_hinf := []float64{0, 1} // 2x1 +``` + +### E.2 Skogestad Mixed-Sensitivity MIMO (Textbook Gammas) + +**Reference**: Skogestad & Postlethwaite, Example 3.8, 1st Ed. + +```go +// 2x2 MIMO plant with RHP zero at s=0.5 +// den = 0.2s^2 + 1.2s + 1 +// G = [[1/den, 1/den], [(2s+1)/den, 2/den]] + +// Sensitivity weight: W(s) = (s/M + wb) / (s + wb*A) +// M=1.5, A=1e-4 +``` + +**Published gammas from textbook**: +- wb1=0.25, wb2=0.25 → **gamma ≈ 2.80** +- wb1=0.25, wb2=25 → **gamma ≈ 2.92** +- wb1=25, wb2=0.25 → **gamma ≈ 6.73** + +### E.3 Disk Margins Test Systems + +**SISO system 1**: +```go +// L(s) = 25 / (s^3 + 10s^2 + 10s + 10) +A := []float64{0, 1, 0, 0, 0, 1, -10, -10, -10} // 3x3 companion +B := []float64{0, 0, 1} +C := []float64{25, 0, 0} +``` +Test with skew = {-1, 0, 1} (T-based, balanced, S-based). + +**SISO system 2** (5th order, more complex): +```go +// L(s) = 6.25(s+3)(s+5) / (s(s+1)^2(s^2+0.18s+100)) +// num = [6.25, 50, 93.75] +// den = [1, 2.18, 100.36, 200.18, 100, 0] +``` + +**MIMO disk margins**: +```go +// Plant: purely oscillatory +A := []float64{0, 10, -10, 0} // 2x2, eigs at ±10j +B := eye(2) +C := []float64{1, 10, -10, 1} // 2x2 +D := zeros(2, 2) +// Static controller K = [[1,-2],[0,1]] +``` + +### E.4 ERA System Identification (2-DOF MSD) + +**Reference**: Mechanical Vibrations textbook, Ex 6.7. + +```go +A := []float64{ + 0, 0, 1, 0, + 0, 0, 0, 1, + -6, 2, -2, 1, + 1, -4, 0.5, -1.5, +} // 4x4 +B := []float64{0, 0, 0, 0, 1, 0, 0, 0.5} // 4x2 +C := []float64{1, 0, 0, 0, 0, 1, 0, 0} // 2x4 +D := zeros(2, 2) +dt := 0.1 +``` + +Recover 4-state system from impulse response via `ERA(markov, r=4, dt=0.1)`. +Verify: recovered poles match original, frequency response matches. + +### E.5 Balanced Reduction (Deterministic, MATLAB-Origin) + +```go +A := []float64{ + -15, -7.5, -6.25, -1.875, + 8, 0, 0, 0, + 0, 4, 0, 0, + 0, 0, 1, 0, +} // 4x4 controllable canonical form +B := []float64{2, 0, 0, 0} // 4x1 +C := []float64{0.5, 0.6875, 0.7031, 0.5} // 1x4 +D := []float64{0} +// TF: (s^3+11s^2+45s+32) / (s^4+15s^3+60s^2+200s+60) +// DC gain = 32/60 = 0.5333 +``` + +Reduce order 4 → 3 (truncate). Verify DC gain preserved. +Reduce order 4 → 2 (truncate). From MATLAB: +```go +Ar := []float64{-1.958, -1.194, -1.194, -0.8344} // 2x2 +Br := []float64{0.9057, 0.4068} // 2x1 +``` + +### E.6 Type 2/3 Systems (Margin Edge Cases) + +Multiple integrators in the loop — stress test for margin computation. + +```go +// Plant: pure integrator P(s) = 1/s with friction b=10 +// Inner plant: Peff(s) = (1/s) / (1 + 10/s) = 1/(s+10) + +// Type 2 controller: C(s) = 165*(s+55)/s +// Loop has 1 integrator from plant → type 1; +1 from controller → type 2 + +// Type 3 controller: C(s) = 110*((s+55)/s)^2 +// Loop has 1 integrator from plant → type 1; +2 from controller → type 3 +// Phase at DC = -270° (3 integrators), margin computation must handle this +``` + +### E.7 Block Diagram Algebra (SS↔TF Roundtrip) + +```go +// System 1 (state-space): +A1 := []float64{0, 1, -4, -1} // underdamped, wn=2, zeta=0.25 +B1 := []float64{0, 1} +C1 := []float64{1, 0} +// TF: G1(s) = 1/(s^2+s+4) + +// System 2 (transfer function): +// G2(s) = (s+0.5)/(s+5) + +// Verify: Series, Parallel, Feedback produce correct poles +``` + +### E.8 Second-Order Reference (wn, zeta verification) + +```go +// Mass-spring-damper: m=250, k=40, b=60 +A := []float64{0, 1, -0.16, -0.24} // 2x2 +B := []float64{0, 0.004} +C := []float64{1, 0} +// wn = sqrt(0.16) = 0.4 rad/s +// zeta = 0.24/(2*0.4) = 0.3 +// Verify Damp() returns these values +``` + +### E.9 Controllability/Observability (3-State with Physical Params) + +```go +A := []float64{1, -1, 1, 1, -0.16, -0.24, 1, 1, 1} // 3x3 +B := []float64{0, 0.004, 1} // 3x1 +C := []float64{1, 0, 1} // 1x3 +// Ctrb = [B, AB, A^2*B] → 3x3, check rank +// Obsv = [C; CA; CA^2] → 3x3, check rank +```