https://github.com/yglee/FastSLAM/blob/master/matlab/KF_cholesky_update.m