https://123dok.net/document/zx538wmd-ekf-based-slam-fusing-heterogeneous-landmarks.html