Filtro Kalman indirecto para sistema de navegación inercial

Estoy tratando de implementar un sistema de navegación inercial usando unFiltro indirecto de Kalman. He encontrado muchas publicaciones y tesis sobre este tema, pero no demasiado código como ejemplo. Para mi implementación estoy usando la Tesis ...