matlab zp2ss函数 C实现

zzzLeaon

已于 2024-01-31 09:18:41 修改

阅读量1.9k

点赞数 58

分类专栏：数字滤波器文章标签： matlab c语言信号处理

于 2023-12-11 15:43:36 首次发布

本文链接：https://blog.csdn.net/CSharp_Beginner/article/details/134928115

版权

数字滤波器专栏收录该内容

16 篇文章 1 订阅

订阅专栏

三大模型

状态空间模型(Space State model)：用于描述动态系统行为的一种数学模型。它在控制系统、信号处理和其他工程领域中广泛应用。状态空间模型的基本思想是通过描述系统内部的状态随时间的演变来表示整个系统的行为。
零极点增益模型(Zero-Pole-Gain model)：用于表示线性时不变系统（LTI system）的数学模型。这个模型描述了系统的传递函数，其中包括了零点（zeros）、极点（poles）和增益（gain）。
传递函数(Transfer Function)：一种用于描述线性时不变系统（LTI system）的数学模型。它是输入和输出之间关系的 Laplace 变换。

相互转换和Matlab函数

ss2zp:将状态空间系统转换为零极点形式的函数。使用ss2zp的一个常见场景是在控制系统设计中，特别是在根轨迹分析和频率响应设计中。了解系统的零点和极点分布可以帮助工程师评估系统的稳定性、动态响应和频率特性。这对于选择合适的控制器、调整控制器参数以及确保系统符合设计要求都是至关重要的。
ss2tf:用于将状态空间系统转换为传递函数形式的函数。将系统从状态空间表示转换为传递函数表示，这有助于使用传统的频域和时域分析方法进行系统分析和设计。传递函数更容易理解，并且与一般的控制系统理论和工具更为兼容。例如，在设计控制器时，通常会使用传递函数形式进行分析、合成和调整。
zp2tf:用于根据给定的零点（zeros）和极点（poles）生成相应的传递函数的函数。使用户能够方便地从零极点的描述中获得传递函数表示。这对于控制系统设计、分析和仿真非常有用。例如，当使用根轨迹分析或频率响应分析时，用户可能更喜欢以传递函数的形式来表示系统，而不是零极点形式。
zp2ss:用于将零极点形式的传递函数转换为状态空间形式的函数。状态空间表示提供了一种更灵活的框架，使得控制系统的分析和设计更加直观。状态空间模型可以用于多种控制系统设计工具和技术，包括线性时不变系统的稳定性分析、系统观测器和反馈控制器设计等。
tf2zp:用于将传递函数转换为零极点形式的函数。通过零极点分析，可以更直观地了解系统的特性。零点是传递函数分子为零的点，极点是分母为零的点。这些零点和极点的位置和分布影响系统的稳定性、振荡频率以及对不同频率的响应。通过分析零极点，工程师可以更好地理解系统的行为，从而更有效地设计和调整系统。
tf2ss:用于将传递函数转换为状态空间形式的函数。构建一个与给定系统行为相匹配的状态空间模型。状态空间表示更直观地描述了系统的动态行为，并提供了更灵活的分析和设计框架。

C实现

由于IIR滤波器中需要用到ss2zp，ss2tf,tf2zp以及zp2ss
这篇文章先先实现zp2ss，代码如下：

void zp2ss(Complex *z, Complex *p, int nz, int np, double k0, double *a, double *b, double *c, double *d)
{
    bool oddPoles = false;
    bool oddZerosOnly = false;
    int nz1 = nz;
    int np1 = np;
    for (int i = 0; i < np * np; i++)
    {
        a[i] = 0.0;
    }
    for (int i = 0; i < np; i++)
    {
        b[i] = 0.0;
        c[i] = 0.0;
    }
    *d = 1;

    cplxpair(p, np);
    if(nz>0)
    {
        cplxpair(z, nz);
    }

    if (np % 2 && nz % 2)
    {
        a[0] = p[np1 - 1].real;
        b[0] = 1;
        c[0] = p[np1 - 1].real - z[nz1 - 1].real;
        *d = 1;
        np = np - 1;
        nz = nz - 1;
        oddPoles = true;
    }
    else if (np % 2)
    {
        a[0] = p[np1 - 1].real;
        b[0] = 1;
        c[0] = 1;
        *d = 0;
        np = np - 1;
        oddPoles = true;
    }
    else if (nz % 2)
    {
        double *num = complex_array_getreal(poly_complex(z[nz - 1]), 2);
        double *den = complex_array_getreal(poly_complex_array((Complex[]){p[np1 - 2], p[np1 - 1]}, 2.0), 3.0);
        double wn = sqrt(double_array_prod(complex_array_abs((Complex[]){p[np1 - 2], p[np1 - 1]}, 2.0), 2.0));
        if (wn = 0)
        {
            wn = 1.0;
        }

        double t[4] = {1, 0, 1, 1.0 / wn};
        double t_inv[4] = {1.0, 0.0, 1.0, 1.0 / wn};
        rinv(t_inv, 2);
        double temp[4];
        double temp1[4];
        trmul(t_inv, (double[]){-den[1], -den[2], 1, 0}, 2, 2, 2, temp);
        trmul(temp, t, 2, 2, 2, temp1);
        for (size_t i = 0; i < 2; i++)
        {
            for (size_t j = 0; j < 2; j++)
            {
                a[i * np1 + j] = temp1[i * 2 + j];
            }
        }
        trmul(t_inv, (double[]){1, 0}, 2, 2, 1, temp);
        for (size_t i = 0; i < 2; i++)
        {
            b[i] = temp[i];
        }
        trmul((double[]){1, num[1]}, t, 1, 2, 2, temp);
        for (size_t i = 0; i < 2; i++)
        {
            c[i] = temp[i];
        }
        d = 0;
        nz = nz - 1;
        np = np - 2;
        oddZerosOnly = true;
        free(num);
        free(den);
    }

    Complex p_temp[2];
    Complex z_temp[2];
    Complex c_temp[3];
    double den[3], num[3], wn;
    double t[2 * 2] = {0.0, 0.0, 0.0, 0.0};
    double t_rinv[2 * 2] = {0.0, 0.0, 0.0, 0.0};
    double tr_den[2 * 2] = {0.0, 0.0, 0.0, 0.0};

    double a1_temp[2 * 2] = {0.0, 0.0, 0.0, 0.0};
    double a1[2 * 2] = {0.0, 0.0, 0.0, 0.0};
    double b1[2 * 2] = {0.0, 0.0, 0.0, 0.0};
    double c1[2 * 2] = {0.0, 0.0, 0.0, 0.0};
    double d1 = 0;

    int i = 0;
    for (i = 0; i < nz1 - 1; i += 2)
    {
        for (size_t m = 0; m < 2; m++)
        {
            z_temp[m] = z[i + m * 1];
            p_temp[m] = p[i + m * 1];
        }
        complex_poly(z_temp, 2, c_temp);
        for (int j = 0; j < 3; j++)
        {
            num[j] = get_real(c_temp[j]);
        }

        complex_poly(p_temp, 2, c_temp);
        for (int j = 0; j < 3; j++)
        {
            den[j] = get_real(c_temp[j]);
        }
        wn = sqrt(get_modulus(p_temp[0]) * get_modulus(p_temp[1]));
        if (wn == 0)
            wn = 1;
        double t[4] = {1.0, 0.0, 0.0, 1.0 / wn};
        double t_inv[4] = {1.0, 0.0, 0.0, 1.0 / wn};
        double a1[4], a_temp[4];
        double b1[2];
        double c1[2];
        double d1 = 1.0;

        rinv(t_inv, 2);
        trmul(t_inv, (double[]){-den[1], -den[2], 1.0, 0.0}, 2, 2, 2, a_temp);
        trmul(a_temp, t, 2, 2, 2, a1);
        trmul(t_inv, (double[]){1.0, 0.0}, 2, 2, 1, b1);
        trmul((double[]){num[1] - den[1], num[2] - den[2]}, t, 1, 2, 2, c1);

        int j;
        if (oddPoles)
        {
            j = i;
        }
        else if (oddZerosOnly)
        {
            j = i + 1;
        }
        else
        {
            j = i - 1;
        }

        if (j == -1)
        {
            for (size_t m = 0; m < 2; m++)
            {
                for (size_t k = 0; k < 2; k++)
                {
                    a[m * np + k] = a1[m * 2 + k];
                }
            }
        }
        else
        {
            double temp_aa[(j + 1) * 2];
            double temp_c[j + 1];
            for (size_t m = 0; m <= j; m++)
            {
                temp_c[m] = c[m];
            }

            trmul(b1, temp_c, 2, 1, j + 1, temp_aa);

            for (size_t m = j + 1; m <= j + 2; m++)
            {
                for (size_t k = 0; k < j + 1; k++)
                {
                    a[m * np1 + k] = temp_aa[(m - j - 1) * (j + 1) + k];
                }
                for (size_t k = j + 1; k <= j + 2; k++)
                {
                    a[m * np1 + k] = a1[(m - j - 1) * 2 + (k - j - 1)];
                }
            }
        }

        // Assign b(j+2:j+3) = b1*d and c(j+2:j+3) = c1
        b[j + 1] = b1[0] * *d;
        b[j + 2] = b1[1] * *d;
        c[j + 1] = c1[0];
        c[j + 2] = c1[1];

        for (size_t m = 1; m <= 2; m++)
        {
            b[j + m] = b1[m - 1] * *d;
            c[j + m] = c1[m - 1];
        }
        // Update d
        *d = d1 * *d;
    }

    for (i; i < np1 - 1; i += 2) // 一般当z为空时执行
    {
        for (size_t m = 0; m < 2; m++)
        {
            p_temp[m] = p[i + m * 1];
        }

        complex_poly(p_temp, 2, c_temp);
        for (int j = 0; j < 3; j++)
        {
            den[j] = get_real(c_temp[j]);
        }
        wn = sqrt(get_modulus(p_temp[0]) * get_modulus(p_temp[1]));
        if (wn == 0)
            wn = 1;
        double t[4] = {1.0, 0.0, 0.0, 1.0 / wn};
        double t_inv[4] = {1.0, 0.0, 0.0, 1.0 / wn};
        double a1[4], a_temp[4];
        double b1[2];
        double c1[2];
        double d1 = 0.0;

        rinv(t_inv, 2);
        trmul(t_inv, (double[]){-den[1], -den[2], 1.0, 0.0}, 2, 2, 2, a_temp);
        trmul(a_temp, t, 2, 2, 2, a1);
        trmul(t_inv, (double[]){1.0, 0.0}, 2, 2, 1, b1);
        trmul((double[]){0.0, 1.0}, t, 1, 2, 2, c1);

        int j;
        if (oddPoles)
        {
            j = i;
        }
        else if (oddZerosOnly)
        {
            j = i + 1;
        }
        else
        {
            j = i - 1;
        }

        if (j == -1)
        {
            for (size_t m = 0; m < 2; m++)
            {
                for (size_t k = 0; k < 2; k++)
                {
                    a[m * np + k] = a1[m * 2 + k];
                }
            }
            for (size_t m = 0; m < 2; m++)
            {
                c[m] = c1[m];
                c[m] = c1[m];
            }
        }
        else
        {
            double temp_aa[(j + 1) * 2];
            double temp_c[j + 1];
            for (size_t m = 0; m <= j; m++)
            {
                temp_c[m] = c[m];
            }

            trmul(b1, temp_c, 2, 1, j + 1, temp_aa);

            for (size_t m = j + 1; m <= j + 2; m++)
            {
                for (size_t k = 0; k < j + 1; k++)
                {
                    a[m * np1 + k] = temp_aa[(m - j - 1) * (j + 1) + k];
                }
                for (size_t k = j + 1; k <= j + 2; k++)
                {
                    a[m * np1 + k] = a1[(m - j - 1) * 2 + (k - j - 1)];
                }
            }

            // c(1:j+1) = d1*c(1:j+1); d1=0;
            for (size_t m = 0; m < j + 1; m++)
            {
                c[m] = 0;
            }
            // c(j+2:j+3) = c1;
            for (size_t m = j + 1; m <= j + 2; m++)
            {
                c[m] = c1[m - j - 1];
            }
        }
        // Assign b(j+2:j+3) = b1*d and c(j+2:j+3) = c1
        for (size_t m = 1; m <= 2; m++)
        {
            b[j + m] = b1[m - 1] * *d;
        }
        // Update d
        *d = d1 * *d;
    }

    for (int i = 0; i < np1; i++)
    {
        c[i] *= k0;
    }
    (*d) = k0 * (*d);
}

其中函数```cplxpair````实现对复数数组进行配对的方法，代码如下：

// 对一组复数进行配对
void cplxpair(Complex *arr, int n)
{
    // 按照幅度升序排列
    qsort(arr, n, sizeof(Complex), compare_abs);

    for (int i = 0; i < n; ++i)
    {
        if (is_real(arr[i]))
        {
            // 找到共轭并交换位置
            for (int j = i + 1; j < n; ++j)
            {
                if (!is_real(arr[j]))
                {
                    swap(arr[i], arr[j]);
                    break;
                }
                else if (fabs(arr[i].real) < arr[j].real ||
                         (fabs(arr[i].real - arr[j].real) < EPS && arr[j].real < EPS))
                {
                    swap(arr[i], arr[j]);
                    break;
                }
            }
        }
    }
}

// 比较两个复数的幅度
static int compare_abs(const void *a, const void *b)
{
    const Complex *c1 = (const Complex *)a;
    const Complex *c2 = (const Complex *)b;

    // 先按实部排序,从小到大
    if (fabs(c1->real - c2->real) > EPS)
    {
        if (c1->real < c2->real)
            return -1;
        else
            return 1;
    }
    else
    {
        // 实部相同，按虚部绝对值排序，由大到小
        if (fabs(fabs(c1->imaginary) - fabs(c2->imaginary)) > EPS)
        {
            if (fabs(c1->imaginary) > fabs(c2->imaginary))
                return -1;
            else
                return 1;
        }
        else
        {
            // 虚部绝对值相同，负号在前
            if (c1->imaginary< 0 && c2->imaginary>= 0)
                return -1;
            else if (c1->imaginary>= 0 && c2->imaginary< 0)
                return 1;
        }
    }

    return 0;
}

// 检查是否是实数
static int is_real(Complex c)
{
    return fabs(c.imaginary) < EPS100;
}

// 计算复数的幅度
static double cplx_abs(Complex c)
{
    return sqrt(c.real * c.real + c.imaginary * c.imaginary);
}