Data Bus Kalman Filter

% initialize
A=[1 dt; 0 1]; % state transition matrix; represents how we get from prior state to next state
C=[1 0; 0 1]; % the matrix that maps measurement to system state
P=[1000 0; 0 1000]; % sets the initial covariance to indicate initial uncertainty
Q=[0.01 0; 0 0.01]; % process noise; I just picked some numbers; you can play with these
R=[0.01 0; 0 0.01]; % measurement noise; I just picked some numbers here too

% loop the following every time you get a new measurement

% PREDICT
% we predict the next system state based on our knowledge (model) of the system
x = A*x;
% We also have to adjust certainty. If we're predicting system state
% without meausrements, certainty reduces
P = A*P*A' + Q; % adjust certainty with the state transition, too. Add process noise

% CORRECT
% With measurements, we correct the state estimate
z=[gpsHdg; gyroHdgRate]; % this is the measurement matrix
% First, find the Kalman Gain; how much we trust the estimate vs measurements
K = P*C'*inv(C*P*C' + R)
% Then we find out the error between prediction and measurement (the Innovation)
% z-C*x
% and correct the estimate -- but only a little at a time,
% as determined by the Kalman Gain
x = x + K*(z-C*x)
% Likewise, we correct (actually increase) certainty, because any time
% we have a measurement we can be a little more certain about our estimate
P = (I-K*C)*P