please dont rip this site

Localization by Kalman Filter

Provides a continuous uni-modal tracking of position; providing one "best guess" prediction, tempered with an certainty which increases with measurement, and decreases with movement. We update position via Bayes Rule products and make predictions about location with a total probability convolution. Normalized Gaussian functions are used in both. The mean, "µ", or center position of the peak of the Gaussian curve is our best guess at the location, and the distribution , "ó2" or width of the peak represents our certainty. A narrow peak indicates high confidence, and a wide peak indicates low confidence.

Each new measurement updates the µ or mean of the location and increases ó2 or certainty. The new location probability curve, µn, ó2n is calculated from the old one µo, ó2o by combining it with the new measurement, which has a center µm and probability ó2m based on the sensors reading and accuracy, as follows:

µn = ( ó2m * µo + ó2o * µm ) / (ó2m + ó2o)

ó2n = 1 / ( 1/ó2m + 1/ó2o )

Note that the new certainty does not depend on the µ or center of the curves at all. So an old measurement and new measurement can be far distant from each other, and the result is a far more confident prediction that the location is between the two. Confidence always increases with new measurements and decreases with motion. Note also that the new position does depend on the certainty of the old and new measurements. The center of the new curve will be nearer the old or new measurements with the higher confidence. If old and new are equally confident, it will be at the midpoint between them.

For movement, the calculations are very simple

µn = µo + um

ó2n = ó2o + r2m

Where um is the distance moved and r2m is the confidence that the movement was exact. Note that the subscript "m" here is "motion" not "measurement" i.e. they are not related to the values for measurements.

The Kalman Filter can also be used to make predictions about what to expect in the future. e.g., given a series of position readings, it can infer velocity.

In Matrix form: Octave Programming Language is available as

% to run this code, go to
% click "kalman.m" on the left, then "RUN>"

location_variance = 1000; %Location uncertainty (high) aka variance of location with itself.
correlation = 0; %Correlation of velocity with location aka covariance
velocity_variance = 1000; %Velocity uncertainty (high) aka variance of velocity with itself.
location = velocity = 0; % Assume at the start, not moving
location_change = velocity_change = 0; %motors initially off, stopped
sensor_accuracy = 1; % 1 assuming perfect sensors, .9 or .8 more likely
measurement = [1 2 3]; % measurements 

x = [location ; velocity]; %State containing the current location and velocity
P = [location_variance correlation ; correlation velocity_variance]; % Covariance matrix^ 
% Note: The covariance of a variable with itself is its variance or uncertainty.
F = [1 1 ; 0 1]; % Next state function. new location = old + velocity. new velocity = old
% this assumes a time interval of 1. F = [1 dt ; 0 1]
H = [1 0]; % measurement function. Only measuring location
R = [sensor_accuracy]; % measurement uncertainty
I = [1 0; 0 1]; % identity matrix

for z=measurement
  y = [z] - H*x; %error between new measure and prior location
   % Note: In octave, .' is transpose (NOT just ')
  S = H*P*H.' + R; %sum uncertainty and sensor accuracy
   % H*P*H.' just extracts location_variance from P. 
   % H*P extracts [location_variance correlation] and *H.' cuts out correlation
  K = P*H.'*inv(S); %generalization of single dimension gain factor
   % P*H.' just extracts [location_variance correlation]
   % *inv(S) basically divides by S
  x = x + (K*y); %update state with error times certainty
  P = (I - K*H)*P; % 

  %movement prediction
  u = [location_change ; velocity_change]; % Commanded motion (expected)
  x = F*x + u; % F*x gives [location+velocity ; velocity], then add [location_change ; velocity_change]
  P = F*P*F.'; % This is the covariance of F*x

See also:

file: /Techref/method/kalmanfilter_localization.htm, 8KB, , updated: 2024/3/1 13:08, local time: 2024/7/15 05:07,

 ©2024 These pages are served without commercial sponsorship. (No popup ads, etc...).Bandwidth abuse increases hosting cost forcing sponsorship or shutdown. This server aggressively defends against automated copying for any reason including offline viewing, duplication, etc... Please respect this requirement and DO NOT RIP THIS SITE. Questions?
Please DO link to this page! Digg it! / MAKE!

<A HREF=""> Localization by Kalman Filter</A>

After you find an appropriate page, you are invited to your to this massmind site! (posts will be visible only to you before review) Just type a nice message (short messages are blocked as spam) in the box and press the Post button. (HTML welcomed, but not the <A tag: Instead, use the link box to link to another page. A tutorial is available Members can login to post directly, become page editors, and be credited for their posts.

Link? Put it here: 
if you want a response, please enter your email address: 
Attn spammers: All posts are reviewed before being made visible to anyone other than the poster.
Did you find what you needed?


Welcome to!


Welcome to!