EXTENDED KALMAN FILTER

Necesito implementar un Filtro de Kalman Extendido para fusionar el sensor inercial (IMU) y el GPS. La idea es que si en algún momento la señal del GPS se pierde o es de baja calidad (zonas de sombra) el vehículo pueda seguir su trayectoria con un mínimo margen de error a partir de los datos suministrados por la IMU. Además este filtro suavizaría la trayectoria con lo cual conseguiríamos reducir los movimientos del volante causados por imprecisiones del GPS o la actitud del vehículo. (Balanceo, alabeo y guiñada). Los datos que nos proporciona el GPS son la latitud, longitud, velocidad y rumbo (El rumbo oscila mucho, sobre todo con el vehículo parado. Es calculado a partir de la posición anterior) y los que nos proporciona el IMU son aceleración y giros en cada uno de sus ejes (XYZ). A pesar de haber leído mucho sobre este filtro en los últimos meses no acabo de entender cómo implementarlo en la aplicación (Visual Basic). Creo que con el Filtro de Kalman Extendido suavizaría todos las oscilaciones bruscas que me hace el vehículo y le daría estabilidad al rumbo. por favor dígame si puede ayudarme, y el precio Gracias