kalman.jl 330 Bytes
Newer Older
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
function kalman(R1,R2,theta, y, A, P)
    # This is really a Kalman filter, not RLS
    ATP = A'*P;
    K = (P*A)/(R2+ATP*A);
    P = P - (P*A*ATP)./(R2 + ATP*A) + R1;
    yp = A'*theta
    e = (y-yp)[1];
    red = 1;
    # if abs(e) > 0.025
    #     red = 0.2;
    # end
    theta = theta + K*e*red;

    theta, P, e, yp[1]
end