root/applications/dual/vahala/kim/evolSys.m @ 1435

function [x_s, y_s] = evolSys(x, u, nQ, nR, noise)
    dt = 0.000125; 
    a = 0.9898;
    b = 0.0072;
    c = 0.0361;
    d = 1.0;
    e = 0.0149;

    x_s(1) = a*x(1) + b*x(3)*sin(x(4)) + c*u(1) + noise*sqrt(nQ(1,1))*randn();
    x_s(2) = a*x(2) - b*x(3)*cos(x(4)) + c*u(2) + noise*sqrt(nQ(2,2))*randn();
    x_s(3) = d*x(3) + e*(x(2)*cos(x(4)) - x(1)*sin(x(4))) + noise*sqrt(nQ(3,3))*randn();
    x_s(4) = x(4) + dt*x(3) + noise*sqrt(nQ(4,4))*randn();

    y_s(1) = x_s(1) + noise*sqrt(nR(1,1))*randn();
    y_s(2) = x_s(2) + noise*sqrt(nR(2,2))*randn();
end