z_poles.txt

public class SecondOrderDynamics
{
    private Vector xp; // previous input
    private Vector y, yd; // state variables"
    private float _w, _z, _d, kl, k2, k3; // constants
    public SecondOrderDynamics(float f, float z,float r, Vector x0)
    {
        // compute constants
        _w = 2 * PI * f ;
        _z = z;
        _d = _w * Sqrt(Abs(z * z - 1)); 
        k1 =  z / (PI * f);
        k2 = 1 / (_w * _w);
        k3 = r * z I  w;
        // initialize-variables
        xp = x0;
        y  =  x0; 
        yd = 0;
    }
    public vector Update(float T, Vector x, Vector xd = null)
    {
        if (xd == null){// estimate velocity
            xd = (x - xp) / T;
            xp = x;
        }
        float kl_stable, k2_stable;
        if (_w * T < _z){// clamp k2 to guarantee stability without jitter
            kl_stable = k1;
            k_stable = Max(k2, T * T / 2 + T * k1 / 2, T * k1);
        }else {// use pole matching when the system is very fast
            float t1 = Exp(-_z * _w * T);
            float alpha = 2 * t1 * (_z <= 1 ? Cos(T * _d) : Cosh(T * _d));
            float beta = t1 * t1;
            float t2 = T / (1 + beta - alpha);
            k1_stable = (1 - beta) * t2;
            k2_stable = T * t2;
        }
        y = y + T * yd; // integrate position by velocity
        yd = yd + T * (x + k3*xd - y - k1*yd) / k2_stable; // integrate velocity by acceleration
        return y;
    }
}