marginalization

Sample code for marginalization of SLAM, Visual-Inertial Odometry. The main reference is code form OKVIS and VINS_MONO. I will try to show how marginlaization works and optimize the performance at the same time.