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 octave-online.net
% to run this code, go to % http://octave-online.net/?s=MLTPlOyeOIkAORaNALAwDniftkKyYpfvpwgwjHscVkNnzHVr % 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 %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 end
|file: /Techref/method/kalmanfilter_localization.htm, 8KB, , updated: 2017/6/29 17:04, local time: 2017/11/21 23:17,
|©2017 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?|
<A HREF="http://www.piclist.com/techref/method/kalmanfilter_localization.htm"> Localization by Kalman Filter</A>
|Did you find what you needed?|
PICList 2017 contributors:
o List host: MIT, Site host massmind.org, Top posters @20171121 RussellMc, Van Horn, David, James Cameron, Sean Breheny, alan.b.pearce, IVP, Neil, David C Brown, Bob Blick, Denny Esterline,
* Page Editors: James Newton, David Cary, and YOU!
* Roman Black of Black Robotics donates from sales of Linistep stepper controller kits.
* Ashley Roll of Digital Nemesis donates from sales of RCL-1 RS232 to TTL converters.
* Monthly Subscribers: Gregg Rew. on-going support is MOST appreciated!
* Contributors: Richard Seriani, Sr.
Welcome to www.piclist.com!