Commit c41cd248 authored by Fredrik Bagge Carlson's avatar Fredrik Bagge Carlson
Browse files

Poked kalman

parent 9810392d
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 = theta + K*e;
theta, P, e, yp[1]
end
end
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment