Add CST parity functions and optimize FRD workflows

README.md

@@ -174,7 +174,6 @@ func main() {
 |----------|-------------|
 | `H2Norm` | H2 norm (RMS gain) |
 | `HinfNorm` | H-infinity norm (peak gain) |
-| `Norm` | General norm computation |
 
 ### Lyapunov Equations
 
@@ -222,8 +221,7 @@ func main() {
 |-----------------|-------------|
 | `Step` | Unit step response |
 | `Impulse` | Unit impulse response |
-| `InitialCondition` | Free response to initial state |
-| `Lsim` | Response to arbitrary input signal |
+| `Initial` | Free response to initial state |
 | `Simulate` | Discrete-time simulation |
 | `GenSig` | Generate test signals (step, sine, square, pulse) |
 

augstate.go

@@ -0,0 +1,73 @@
+package controlsys
+
+import "gonum.org/v1/gonum/mat"
+
+func Augstate(sys *System) (*System, error) {
+	n, m, p := sys.Dims()
+	if n == 0 {
+		return sys.Copy(), nil
+	}
+
+	pNew := p + n
+
+	cNew := mat.NewDense(pNew, n, nil)
+	cRaw := cNew.RawMatrix()
+	origC := sys.C.RawMatrix()
+	for i := 0; i < p; i++ {
+		copy(cRaw.Data[i*cRaw.Stride:i*cRaw.Stride+n], origC.Data[i*origC.Stride:i*origC.Stride+n])
+	}
+	for i := 0; i < n; i++ {
+		cRaw.Data[(p+i)*cRaw.Stride+i] = 1
+	}
+
+	dNew := mat.NewDense(pNew, m, nil)
+	dRaw := dNew.RawMatrix()
+	origD := sys.D.RawMatrix()
+	for i := 0; i < p; i++ {
+		copy(dRaw.Data[i*dRaw.Stride:i*dRaw.Stride+m], origD.Data[i*origD.Stride:i*origD.Stride+m])
+	}
+
+	result := &System{
+		A:  denseCopy(sys.A),
+		B:  denseCopy(sys.B),
+		C:  cNew,
+		D:  dNew,
+		Dt: sys.Dt,
+	}
+
+	if sys.Delay != nil {
+		result.Delay = copyDelayOrNil(sys.Delay)
+	}
+	if sys.InputDelay != nil {
+		result.InputDelay = make([]float64, len(sys.InputDelay))
+		copy(result.InputDelay, sys.InputDelay)
+	}
+	if sys.OutputDelay != nil {
+		result.OutputDelay = make([]float64, len(sys.OutputDelay))
+		copy(result.OutputDelay, sys.OutputDelay)
+	}
+	if sys.LFT != nil {
+		result.LFT = &LFTDelay{
+			Tau: append([]float64(nil), sys.LFT.Tau...),
+			B2:  copyDelayOrNil(sys.LFT.B2),
+			C2:  copyDelayOrNil(sys.LFT.C2),
+			D12: copyDelayOrNil(sys.LFT.D12),
+			D21: copyDelayOrNil(sys.LFT.D21),
+			D22: copyDelayOrNil(sys.LFT.D22),
+		}
+	}
+
+	result.InputName = copyStringSlice(sys.InputName)
+	var outNames []string
+	if sys.OutputName != nil {
+		outNames = append(outNames, sys.OutputName...)
+	} else {
+		outNames = make([]string, p)
+	}
+	stNames := sys.stateLabels()
+	outNames = append(outNames, stNames...)
+	result.OutputName = outNames
+	result.StateName = copyStringSlice(sys.StateName)
+
+	return result, nil
+}