Kalman_Filter_Example ---------------------------------

Simple KF example with simplified dynamics and noise + error, good for reference, teaching, and as a template. Models for Kalman Filter and Extended Kalman Filter

Proof these examples demonstrate working KF algorithm implementations:

Kalman Filter for Position and Velocity Tracking

^Actual velocity is 60 m/s but initial estimate is set to 20. Clearly, it rises towars and sustains an estimate velocity near 60 m/s after only a few updates.

^Kalman Gains move towards a steady state as estimation process starts to converge, Velocity is only a little in favor of the estimate since it is 0.5 and close to an even weight of estimate and measurement whereas the position is significantly more favoring of the estimate due to the lower gain.

^Shows general filtering of measurement position error noise.

Extended Kalman Figure Example ----------------------------

^Actual velocity in x direction was set to be around 20 m/s with some added noise. Initial estimate was 0 m/s so it does approach that. What makes this an EKF example is that the state to measurement matrix is not linear. It is jacobian of polar to cartesian conversion. This would be a good example of a radar (polar sensor) being a the sensor that would update the filter. (y direction actual velocity set to around zero.

^these show that position error is mostly contained within 3-sigma (standard devations) for this example. Didn't spend time tuning Q matrix etc since this is just an example. However this code should be relatively easy to digest.

Credit is due to Franklin's Book: A Kalman Filter Made Easy, highly recommend the read for those who want to learn how to use and have intution for using a KF in relativley little time.