Tom's MMT Pages

May 25, 2020

MMT servos - KSS - Output "HO" filter

This is labeled "Newest HO filter" in the model. This is the last of 4 filters that process servo output The routine that sets up these four filters looks like this:

void
flexmode_init ( void )
{
        flex_n567 = n567_init ();
        flex_n20 = n20_init ();
        flex_n35 = n35_init ();
        flex_ho = ho_init ();
}

The HO filter is set up as follows:

/* Tested AOK 8-11-2010 */
/* This filter is a low-pass filter with a cutoff around 36 Hz */
/* same filter in old and new el model.
 * parked in the new model */

struct ho_info *
ho_init ( void )
{
        struct ho_info *hp;
        int i;

        hp = (struct ho_info *) malloc ( sizeof(struct ho_info) );
        if ( ! hp )
            return hp;

        for ( i=0; i<10; i++ )
            hp->state[i] = 0.0;

        hp->s1 = 0.06835314745957749;

        hp->a[0] = -1.0714680349992194;
        hp->a[1] = 0.32120675863960513;
        hp->a[2] = -1.5456006425916522;
        hp->a[3] = 0.67723598996970558;
        hp->a[4] = -1.7796949495985039;
        hp->a[5] = 0.8544789988971333;
        hp->a[6] = -1.8819352062893109;
        hp->a[7] = 0.93442399131598086;
        hp->a[8] = -1.9360470180601386;
        hp->a[9] = 0.98075403075671552;

        hp->b[0] = -0.67012066314606944;
        hp->b[1] = 0.99999999999999878;
        hp->b[2] = -1.7766628742891879;
        hp->b[3] = 0.99999999999999933;
        hp->b[4] = -1.904813250331264;
        hp->b[5] = 0.99999999999999989;
        hp->b[6] = -1.939517806548595;
        hp->b[7] = 1.0000000000000022;
        hp->b[8] = -1.950640147644068;
        hp->b[9] = 0.99999999999999989;

        return hp;
}

The calculation performed at each time step is as follows:

#define NSTAGES 5

double
ho_calc ( struct ho_info *hp, double input )
{
        double out;
        double ss;
        int k;

        if ( ! hp )
            return 0.0;

        out = input * hp->s1;

        for ( k=0; k<(NSTAGES*2); k += 2 ) {
            ss = out - hp->a[k] * hp->state[k] - hp->a[k+1] * hp->state[k+1];
            out = ss + hp->b[k] * hp->state[k] + hp->b[k+1] * hp->state[k+1];
            hp->state[k+1] = hp->state[k];
            hp->state[k] = ss;
        }

        return out;
}