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

poked kalman

parent ca562212
"""
One dimensional Kalman filter
`kalman(R1,R2,theta, y, A, P)`
"""
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]
ATP = A'P
K = (P*A)/(R2+ATP*A)
P = P - (P*A*ATP)./(R2 + ATP*A) + R1
yp = (A'theta)[1]
e = y-yp
theta = theta + K*e
return theta, P, e, yp
end
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