debugging EKF. More detailed Initialization member function is necessary
removed some lines to make the initialization clear
debugged bugs caused by initializations
a little bit code-cleaning