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}, *ó*^{2}_{n} is calculated
from the old one µ_{o},
*ó*^{2}_{o} by combining it with the new
measurement, which has a center µ_{m} and probability
*ó*^{2}_{m} based on the sensors reading and
accuracy, as follows:

µ_{n} = ( *ó*^{2}_{m} *
µ_{o} + *ó*^{2}_{o} *
µ_{m} ) / (*ó*^{2}_{m} +
*ó*^{2}_{o})

*ó*^{2}_{n} = 1 / (
1/*ó*^{2}_{m} +
1/*ó*^{2}_{o} )

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} + u_{m}

*ó*^{2}_{n} =
*ó*^{2}_{o} +
*r*^{2}_{m}

Where u_{m} is the distance moved and
*r*^{2}_{m} 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.'

See also:

- http://www.ilectureonline.com/lectures/subject/SPECIAL%20TOPICS/26/190 A good series of lectures explaining the Kalman Filter.+
- http://greg.czerniak.info/guides/kalman1/ (note: they use "A" as the name of the matrix we called "F")
- http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
- https://www.alanzucconi.com/2022/07/24/kalman-gain/ "Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation" (Simple and Intuitive for Math majors)

file: /Techref/method/kalmanfilter_localization.htm, 8KB, , updated: 2024/3/1 13:08, local time: 2024/4/18 17:13, |

©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?<A HREF="http://www.piclist.com/techref/method/kalmanfilter_localization.htm"> Localization by Kalman Filter</A> |

Did you find what you needed? |

PICList 2024 contributors:
o List host: MIT, Site host massmind.org, Top posters @none found - 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! |

.