Multi-State contraint Kalman filter for vision- aided inertial navigation with observability constain. Detect and track features in image sequence.