function xdot = threelqrrhs(t,x)
    k = 2;
    A = [0 1 0 0 0 0;
        -k 0 k 0 0 0;
        0 0 0 1 0 0;
        k 0 -2*k 0 k 0;
        0 0 0 0 0 1;
        0 0 k 0 -k 0];
    B = [0 1 0 0 0 0]';
    f = -[5.6734    3.5138   -2.5242    3.1151    3.3316    3.0058]*x;
    xdot = A*x + B*f;
end