sos2ss proof of concept

sos2ss.py

import numpy as np
import scipy.signal

def sscascade(sslist):

    # The cascaded state-space representation can be constructed as block
    # matrices of the following form (example for 4 sub-systems):
    # A = [                A1 ,              ,         ,    ]
    #     [           B2 @ C1 ,           A2 ,         ,    ]
    #     [      B3 @ D2 @ C1 ,      B3 @ C2 ,      A3 ,    ]
    #     [ B4 @ D3 @ D2 @ C1 , B4 @ D3 @ C2 , B4 @ C3 , A4 ]
    # B = [                B1 ]
    #     [           B2 @ D1 ]
    #     [      B3 @ D2 @ D1 ]
    #     [ B4 @ D3 @ D2 @ D1 ]
    # C = [ D4 @ D3 @ D2 @ C1 , D4 @ D3 @ C2 , D4 @ C3 , C4 ]
    # D = [ D4 @ D3 @ D2 @ D1 ]

    # normalize state-space representation list
    sslist = [scipy.signal.abcd_normalize(*ss) for ss in sslist]

    # check consistency of number of inputs/outputs between stages
    for i in range(len(sslist) - 1):
        n1 = sslist[i][3].shape[0]
        n2 = sslist[i + 1][3].shape[1]
        assert n1 == n2, f'Input/output mismatch: Stage {i} has {n1} outputs, stage {i + 1} has {n2} inputs'

    # get number of inputs and number of outputs
    ni = sslist[0][3].shape[1]
    no = sslist[-1][3].shape[0]

    # get number of states per stage, and calculate cumulative sum
    ns = np.array([ss[0].shape[0] for ss in sslist])
    nsc = np.r_[0, np.cumsum(ns)]

    # construct A
    A = np.zeros((nsc[-1], nsc[-1]))
    for i in range(len(sslist)):
        sl1 = np.s_[nsc[i] : nsc[i + 1]]
        A[sl1, sl1] = sslist[i][0]
        temp = np.eye(sslist[i][3].shape[0])
        for j in range(i + 1, len(sslist)):
            sl2 = np.s_[nsc[j] : nsc[j + 1]]
            A[sl2, sl1] = sslist[j][1] @ temp @ sslist[i][2]
            if j != len(sslist) - 1:
                temp = sslist[j][3] @ temp

    # construct B
    B = np.zeros((nsc[-1], ni))
    temp = np.eye(ni)
    for i in range(len(sslist)):
        sl = np.s_[nsc[i] : nsc[i + 1]]
        B[sl, :] = sslist[i][1] @ temp
        if i != len(sslist) - 1:
            temp = sslist[i][3] @ temp

    # construct C and D
    C = np.zeros((no, nsc[-1]))
    temp = np.eye(no)
    for i in reversed(range(len(sslist))):
        sl = np.s_[nsc[i] : nsc[i + 1]]
        C[:, sl] = temp @ sslist[i][2]
        temp = temp @ sslist[i][3]
    D = temp

    return (A, B, C, D)

def sos2ss(sos):
    sslist = []
    for row in sos[::-1]:
        if row[3] == 0:
            # first order section
            assert row[0] == 0 and row[4] == 1, 'Invalid SOS format'
            A = np.array([[-row[5]]])
            B = np.array([[1]])
            C = np.array([[row[2] - row[5] * row[1]]])
            D = np.array([[row[1]]])
        else:
            # second order section
            assert row[3] == 1, 'Invalid SOS format'
            A = np.array([[-row[4], -row[5]], [1, 0]])
            B = np.array([[1], [0]])
            C = np.array([[row[1] - row[4] * row[0], row[2] - row[5] * row[0]]])
            D = np.array([[row[0]]])
        sslist.append((A, B, C, D))
    return sscascade(sslist)

if __name__ == '__main__':

    import matplotlib.pyplot as plt

    zpk = scipy.signal.ellip(41, 0.01, 60, 0.8, analog=True, output='zpk')
    sos = scipy.signal.zpk2sos(*zpk, analog=True)
    ss = sos2ss(sos)

    t = np.linspace(0, 1000, 1001)
    res1 = scipy.signal.impulse(zpk, T=t)
    res2 = scipy.signal.impulse(ss, T=t)

    plt.close('all')

    plt.figure('SOS to SS', figsize=(8, 6))
    plt.subplot(2, 1, 1)
    plt.plot(res1[0], res1[1], '-', label='ZPK->TF->SS impulse')
    plt.grid()
    plt.ylim(-0.3, 0.3)
    plt.legend(loc='upper right')
    plt.subplot(2, 1, 2)
    plt.plot(res2[0], res2[1], '-', label='ZPK->SOS->SS impulse')
    plt.grid()
    plt.ylim(-0.3, 0.3)
    plt.legend(loc='upper right')
    plt.tight_layout()
    plt.show()