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 , "ó²" 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 ó² or certainty. The new location probability curve, µ_n, ó²_n is calculated from the old one µ_o, ó²_o by combining it with the new measurement, which has a center µ_m and probability ó²_m based on the sensors reading and accuracy, as follows:

µ_n = ( ó²_m * µ_o + ó²_o * µ_m ) / (ó²_m + ó²_o)

ó²_n = 1 / ( 1/ó²_m + 1/ó²_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

ó²_n = ó²_o + r²_m

Where u_m is the distance moved and r²_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