May 25, 2020

MMT servos - KSS - Output "6 Hz" notch

This is labeled notch6b in the model. This is the first of 4 filters that process servo output in what is called "flexible mode compensation". The routine that sets up these filters looks like this: 
void
flexmode_init ( void )
{
        flex_n567 = n567_init ();
        flex_n20 = n20_init ();
        flex_n35 = n35_init ();
        flex_ho = ho_init ();
}
Note that this calls the 6 Hz notch filter a 5.67 Hz notch, and is set up as follows: 
/* 5.67 Hz notch -- ORDER 2 */
/* Tested OK 1-23-2014 */
/* NEW for new model */
struct sos_info *
sos_n567_init ( void )
{
        static double a_data[2] = { 1.9810923803941254E+00, -9.8234996846355593E-01 };
        static double c_data[2] = { -1.5734854345754279E-02, 1.5744842756842625E-02 };
        static double d_data = 9.9205748580860009E-01;

        return sos_init ( a_data, c_data, d_data, 2 );
}
Newer versions of Matlab produce a DTF filter rather than an SOS filter for this.