Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
410 lines (371 sloc) 9.93 KB
#ifndef SISO_H
#define SISO_H
#include "cplx.h"
#include "matrix.h"
#include <stdio.h>
#define SISO_MAX_STATES (32)
#ifdef SISO_TEST
#define SISO_UNUSED
#else
#define SISO_UNUSED COP_ATTR_UNUSED
#endif
struct ss_siso {
unsigned n;
double *a; /* n * n */
double *b; /* n */
double *c; /* n */
double d;
};
/* Given a transfer function of:
* Y(z) b0 + b1 z^-1 + b2 z^-2
* ---- = ----------------------
* X(z) 1.0 + a1 z^-1 + a2^-2
*
* Give a state space representation. The mapping is not unique and this
* implementation currently performs a pretty garbage direct-form mapping. */
static SISO_UNUSED void ss_siso_init(struct ss_siso *ss, double a1, double a2, double b0, double b1, double b2)
{
ss->n = 2;
ss->a[0] = -a1;
ss->a[1] = -a2;
ss->a[2] = 1.0;
ss->a[3] = 0.0;
ss->b[0] = 1.0;
ss->b[1] = 0.0;
ss->c[0] = b1 - a1 * b0;
ss->c[1] = b2 - a2 * b0;
ss->d = b0;
#if 1
{ /* Normalise input and output matrices. */
double norm_gain = sqrt(sqrt(ss->c[0] * ss->c[0] + ss->c[1] * ss->c[1]));
ss->b[0] = norm_gain;
ss->c[0] /= norm_gain;
ss->c[1] /= norm_gain;
}
#endif
}
static SISO_UNUSED void ss_siso_init1(struct ss_siso *ss, double a1, double b0, double b1)
{
ss->n = 1;
ss->a[0] = -a1;
ss->b[0] = 1.0;
ss->c[0] = b1 - a1 * b0;
ss->d = b0;
#if 1
{ /* Normalise input and output matrices. */
double norm_gain = sqrt(sqrt(ss->c[0] * ss->c[0] + ss->c[1] * ss->c[1]));
ss->b[0] = norm_gain;
ss->c[0] /= norm_gain;
ss->c[1] /= norm_gain;
}
#endif
}
static SISO_UNUSED void ss_siso_init_conj_pz(struct ss_siso *ss, struct cplx cpole, struct cplx czero, double gain)
{
double a1 = -cpole.re * 2.0;
double a2 = cpole.re * cpole.re + cpole.im * cpole.im;
double b1 = -czero.re * 2.0;
double b2 = czero.re * czero.re + czero.im * czero.im;
ss_siso_init
(ss
,a1
,a2
,1.0 * gain
,b1 * gain
,b2 * gain
);
}
/* Concatenate two state-space representations into one big matrix. This
* mapping will place the state-transition ("A") matrices of the input systems
* onto the diagonal of the new system. The upper-right elements off the
* block diagonal will be zero (i.e. the new state-transition matrix is lower
* block diagonal). */
static SISO_UNUSED void ss_siso_cat(struct ss_siso *dest, struct ss_siso *s0, struct ss_siso *s1)
{
unsigned i, j;
dest->n = s0->n + s1->n;
dest->d = s0->d * s1->d;
for (i = 0; i < s0->n; i++) {
dest->b[i] = s0->b[i];
dest->c[i] = s1->d * s0->c[i];
}
for (; i < dest->n; i++) {
dest->b[i] = s0->d * s1->b[i-s0->n];
dest->c[i] = s1->c[i-s0->n];
}
for (i = 0; i < s0->n; i++) {
for (j = 0; j < s0->n; j++)
dest->a[i*dest->n+j] = s0->a[i*s0->n+j];
for (; j < dest->n; j++)
dest->a[i*dest->n+j] = 0.0;
}
for (; i < dest->n; i++) {
for (j = 0; j < s0->n; j++)
dest->a[i*dest->n+j] = s0->c[j] * s1->b[i-s0->n];
for (; j < dest->n; j++)
dest->a[i*dest->n+j] = s1->a[(i-s0->n)*s1->n+j-s0->n];
}
}
static SISO_UNUSED void ss_siso_print(struct ss_siso *ss)
{
unsigned i, j;
for (i = 0; i < ss->n; i++) {
printf("| ");
for (j = 0; j < ss->n; j++) {
printf("%10.3e ", ss->a[i*ss->n+j]);
}
printf("| | %10.3e |\n", ss->b[i]);
}
printf("\n| ");
for (i = 0; i < ss->n; i++) {
printf("%10.3e ", ss->c[i]);
}
printf("| | %10.3e |\n", ss->d);
}
static SISO_UNUSED int ss_siso_cat_diag(struct ss_siso *dest, struct ss_siso *s0, struct ss_siso *s1) {
unsigned i, j;
double w10[128];
double w[128];
dest->n = s0->n + s1->n;
dest->d = s0->d * s1->d;
/* We want a block diagonal state transition matrix. This implies there
* are no eigenvalues of multiplicity > 1. We need to find a similarity
* transform (W) that maps the A matrix of the concatenated system to
* the block diagonal system A'.
*
* i.e.
*
* zQ = AQ + BX
* Y = CQ + DX
*
* Make the substituion Q=WT:
*
* zWT = A W T + B X
* zT = W^-1 A W T + W^-1 B X
* Y = C W T + D X
*
* So we want to find the matrix W where W^1 A W = A' where A' is block
* diagonal and A is known. i.e.
*
* WA' = AW
*
* Using block matrices where a00 represents the state matrix of s0 and
* a11 represents the state matrix of s1.
* | w00 w01 | | a00 0 | | a00 0 | | w00 w01 |
* | w10 w11 | | 0 a11 | = | a10 a11 | | w10 w11 |
*
* It turns out w00=I and w11=I and w01=0:
* | I 0 | | a00 0 | | a00 0 | | I 0 |
* | w10 I | | 0 a11 | = | a10 a11 | | w10 I |
*
* And:
* a11 w10 - w10 a00 = -a10
*
* This is a Sylvester equstion which we can solve for w10, which gives us
* the entire W which we can use to solve for the required B and C
* vectors.
*
* 1) Solve for W */
for (i = 0; i < s0->n; i++) {
for (j = 0; j < s0->n; j++)
dest->a[i*s0->n+j] = -s0->a[i*s0->n+j];
}
for (i = 0; i < s1->n; i++) {
for (j = 0; j < s0->n; j++) {
w[i*s0->n+j] = -s0->c[j] * s1->b[i];
}
}
if (mat_drm_sylv_rmrmrm
(w10 /* x=w10 */
,s1->a /* a=a11 */
,dest->a /* b=-a00 */
,w /* c=a10 */
,s1->n /* axc_rows_a_cols */
,s0->n /* b_rows_bxc_cols */
,s1->n /* a_rstride */
,s0->n /* bxc_rstride */
)) {
return -1;
}
for (i = 0; i < dest->n; i++) {
for (j = 0; j < dest->n; j++) {
if (i == j) {
w[i*dest->n+j] = 1.0;
} else if (i >= s0->n && j < s0->n) {
w[i*dest->n+j] = w10[(i-s0->n)*s0->n+j];
} else {
w[i*dest->n+j] = 0.0;
}
}
}
/* 2) Solve for C' */
for (i = 0; i < s0->n; i++) {
dest->a[i] = s1->d * s0->c[i];
}
for (; i < dest->n; i++) {
dest->a[i] = s1->c[i-s0->n];
}
mat_drm_mul_rmrm
(/* out */ dest->c /* C' */
,/* lhs */ dest->a /* C */
,/* rhs */ w /* W */
,/* lhs_rows_out_rows */ 1
,/* lhs_cols_rhs_rows */ dest->n
,/* rhs_cols_out_cols */ dest->n
,/* lhs_rstride */ 0 /* doesn't matter */
,/* rhsout_rstride */ dest->n
);
/* 3) Solve for B' */
if (mat_drm_inv_rm_naive
(/* out */ dest->a /* W^-1 */
,/* in */ w /* W */
,/* io_rstride */ dest->n
,/* io_nrow */ dest->n
,/* eps */ 1e-12
)) {
return -1;
}
for (i = 0; i < s0->n; i++) {
w[i] = s0->b[i];
}
for (; i < dest->n; i++) {
w[i] = s0->d * s1->b[i-s0->n];
}
mat_drm_mul_rmrm
(/* out */ dest->b /* B' */
,/* lhs */ dest->a /* W^-1 */
,/* rhs */ w /* B */
,/* lhs_rows_out_rows */ dest->n
,/* lhs_cols_rhs_rows */ dest->n
,/* rhs_cols_out_cols */ 1
,/* lhs_rstride */ dest->n
,/* rhsout_rstride */ 1 /* doesnt matter */
);
/* 4) Dump the block diagonal concatenation. */
for (i = 0; i < s0->n; i++) {
for (j = 0; j < s0->n; j++)
dest->a[i*dest->n+j] = s0->a[i*s0->n+j];
for (; j < dest->n; j++)
dest->a[i*dest->n+j] = 0.0;
}
for (; i < dest->n; i++) {
for (j = 0; j < s0->n; j++)
dest->a[i*dest->n+j] = 0.0;
for (; j < dest->n; j++)
dest->a[i*dest->n+j] = s1->a[(i-s0->n)*s1->n+j-s0->n];
}
return 0;
}
static SISO_UNUSED void ss_siso_copy(struct ss_siso *dest, struct ss_siso *src) {
unsigned i;
for (i = 0; i < src->n*src->n; i++) {
dest->a[i] = src->a[i];
}
for (i = 0; i < src->n; i++) {
dest->b[i] = src->b[i];
dest->c[i] = src->c[i];
}
dest->d = src->d;
dest->n = src->n;
}
static SISO_UNUSED double ss_siso_run(const struct ss_siso *ss, double *states, double x)
{
unsigned i, j;
double y;
double sn[SISO_MAX_STATES];
y = ss->d * x;
for (i = 0; i < ss->n; i++) {
double ns = ss->b[i] * x;
y += ss->c[i] * states[i];
for (j = 0; j < ss->n; j++) {
ns += ss->a[i*ss->n+j] * states[j];
}
sn[i] = ns;
}
for (i = 0; i < ss->n; i++) {
states[i] = sn[i];
}
return y;
}
#ifdef SISO_TEST
int main(int argc, char *argv[]) {
/* 5 pole, 5 zero system. */
const double pr0 = 0.999;
const double zr0 = 0.998;
const struct cplx p1 = cplx_make_p(0.995, 0.05);
const struct cplx z1 = cplx_make_p(0.990, 0.05);
const struct cplx p2 = cplx_make_p(0.990, 0.025);
const struct cplx z2 = cplx_make_p(0.980, 0.025);
/* Create coefficients for running test reference biquads. */
const double b1p1 = -2*p1.re;
const double b1p2 = p1.re*p1.re + p1.im*p1.im;
const double b1z1 = -2*z1.re;
const double b1z2 = z1.re*z1.re + z1.im*z1.im;
const double b2p1 = -2*p2.re;
const double b2p2 = p2.re*p2.re + p2.im*p2.im;
const double b2z1 = -2*z2.re;
const double b2z2 = z2.re*z2.re + z2.im*z2.im;
unsigned i;
printf("siso cat test...");
double ref_states[5];
double dut_states[5];
double storage[1024];
memset(ref_states, 0, sizeof(ref_states));
memset(dut_states, 0, sizeof(dut_states));
struct ss_siso system;
system.a = storage;
system.b = system.a + 8*8;
system.c = system.b + 8;
{
struct ss_siso tmp1;
struct ss_siso tmp2;
tmp1.a = system.c + 8;
tmp1.b = tmp1.a + 8*8;
tmp1.c = tmp1.b + 8;
tmp2.a = tmp1.c + 8;
tmp2.b = tmp2.a + 8*8;
tmp2.c = tmp2.b + 8;
ss_siso_init1(&system, -pr0, 1.0, -zr0);
ss_siso_init_conj_pz(&tmp1, p1, z1, 1.0);
ss_siso_cat_diag(&tmp2, &system, &tmp1);
ss_siso_init_conj_pz(&tmp1, p2, z2, 1.0);
ss_siso_cat_diag(&system, &tmp2, &tmp1);
}
#if 1 /* for debugging */
printf("\n");
ss_siso_print(&system);
#endif
for (i = 0; i < 200; i++) {
double y = (i == 0) ? 1.0 : 0.0;
double py;
double ppy;
double duty = ss_siso_run(&system, dut_states, y);
/* Run single pole section. */
py = ref_states[0];
y = y + py * pr0;
ref_states[0] = y;
y = y - py * zr0;
/* Run biquad 1 */
ppy = ref_states[1];
py = ref_states[2];
y = y - ppy * b1p2 - py * b1p1;
ref_states[1] = py;
ref_states[2] = y;
y = y + b1z1 * py + b1z2 * ppy;
/* Run biquad 2 */
ppy = ref_states[3];
py = ref_states[4];
y = y - ppy * b2p2 - py * b2p1;
ref_states[3] = py;
ref_states[4] = y;
y = y + b2z1 * py + b2z2 * ppy;
if (fabs(y - duty) > 1e-12) {
printf(" failed\n");
abort();
}
}
printf(" passed\n");
return 0;
}
#endif
#endif