Data hosted with ♥ by Pastebin.com - Download Raw - See Original
  1. % initialize
  2. A=[1 dt; 0 1]; % state transition matrix; represents how we get from prior state to next state
  3. C=[1 0; 0 1]; % the matrix that maps measurement to system state
  4. P=[1000 0; 0 1000]; % sets the initial covariance to indicate initial uncertainty
  5. Q=[0.01 0; 0 0.01]; % process noise; I just picked some numbers; you can play with these
  6. R=[0.01 0; 0 0.01]; % measurement noise; I just picked some numbers here too
  7.  
  8. % loop the following every time you get a new measurement
  9.  
  10. % PREDICT
  11. % we predict the next system state based on our knowledge (model) of the system
  12. x = A*x;
  13. % We also have to adjust certainty. If we're predicting system state
  14. % without meausrements, certainty reduces
  15. P = A*P*A' + Q; % adjust certainty with the state transition, too. Add process noise
  16.  
  17. % CORRECT
  18. % With measurements, we correct the state estimate
  19. z=[gpsHdg; gyroHdgRate]; % this is the measurement matrix
  20. % First, find the Kalman Gain; how much we trust the estimate vs measurements
  21. K = P*C'*inv(C*P*C' + R)
  22. % Then we find out the error between prediction and measurement (the Innovation)
  23. % z-C*x
  24. % and correct the estimate -- but only a little at a time,
  25. % as determined by the Kalman Gain
  26. x = x + K*(z-C*x)
  27. % Likewise, we correct (actually increase) certainty, because any time
  28. % we have a measurement we can be a little more certain about our estimate
  29. P = (I-K*C)*P