Fix segfault  & sensors bounds check + some documentation refactoring + work on unit tests