Added code to implement listing 7.5.

This is the EKF fixed for the divergence problem.
Implementation of listing 2, chapter 8.