I've always found that determining where to put outputs from disparate sensors as opposed to just filtering a single observation like the GPS output in your example is challenging. Have you tried extending this to include input from other sensors (e.g. accelerometer, gyroscope, magnetometer, etc.)?