Implemented UKF in test_lsb.py and fixed bug in EKF implementation.