matlab ss2tf函数 C实现

继上一篇matlab zp2ss函数 C实现

这一篇实现ss2tf(将状态空间系统转换为传递函数形式)

代码

void ss2tf(double *a, double *b, double *c, double *d, int n, double *num, double *den)
{
    poly(a, n, den);

    double *temp_a = malloc(sizeof(double) * n * n);

    trmul(b, c, n, 1, n, temp_a);
    
    for (size_t i = 0; i < n; i++)
    {
        for (size_t j = 0; j < n; j++)
        {
            temp_a[i * n + j] = a[i * n + j] - temp_a[i * n + j];
        }
    }

    poly(temp_a, n, num);//

    for (size_t i = 0; i < n + 1; i++)
    {
        num[i] += (*d - 1.0) * den[i];
    }
}

其中poly是矩阵求特征多项式的函数：

void poly(double *p, int np, double *d)
{
    double *temp = (double *)malloc(sizeof(double) * np * np);

    for (size_t i = 0; i < np; i++)
    {
        for (size_t j = 0; j < np; j++)
        {
            temp[i * np + j] = p[i * np + j];
        }
    }

    // eig(A)
    double *u_real = (double *)malloc(sizeof(double) * np);
    double *v_imag = (double *)malloc(sizeof(double) * np);
    int jt = 60;
    hhbg(temp, np);
    hhqr(temp, np, EPS, jt, u_real, v_imag);

    Complex *dt = (Complex *)malloc(sizeof(Complex) * (np + 1));
    Complex *dtt = (Complex *)malloc(sizeof(Complex) * (np + 1));
    dt[0].real = 1;
    dt[0].imaginary = 0;
    for (size_t i = 1; i < np + 1; i++)
    {
        dt[i].real = 0;
        dt[i].imaginary = 0;
    }
    for (size_t i = 0; i < np; i++)
    {
        for (size_t j = 1; j <= i + 1; j++)
        {
            dtt[j] = complex_subtract(dt[j], complex_multiply(create(u_real[i], v_imag[i]), dt[j - 1]));
        }
        for (size_t j = 1; j <= i + 1; j++)
        {
            dt[j].real = dtt[j].real;
            dt[j].imaginary = dtt[j].imaginary;
        }
    }

    for (size_t i = 0; i < np + 1; i++)
    {
        d[i] = dt[i].real;
    }

    free(dt);
    free(dtt);
}

trmul是两个矩阵相乘：

void trmul(double a[], double b[], int m, int n, int k, double c[])
{
    int i, j, l, u;
    for (i = 0; i <= m - 1; i++)
        for (j = 0; j <= k - 1; j++)
        {
            u = i * k + j;
            c[u] = 0.0;
            for (l = 0; l <= n - 1; l++)
                c[u] = c[u] + a[i * n + l] * b[l * k + j];
        }
}

测试

与matlab的ss2tf函数对比，基本一致，如果矩阵为病态条件矩阵则结果异常，需要注意。