Combinar datos de giroscopio y acelerómetro

Estoy construyendo un robot de equilibrio con el sistema NXT de Lego Mindstorm. Estoy usando dos sensores de HiTechnic, el primero es un acelerómetro y el segundo es un giroscopio. He filtrado con éxito el ruido de los sensores y los ángulos derivados para ambos en un rango entre -90 y 90 grados, con 0 grados perfectamente equilibrados.

Mi próximo desafío es combinar ambos valores del sensor para corregir la deriva del giroscopio a lo largo del tiempo. A continuación se muestra un gráfico de ejemplo que creé a partir de datos reales para demostrar la deriva del giroscopio:

El enfoque más utilizado que he visto para hacer que la combinación de estos sensores sea sólida es mediante el uso de un filtro de Kalman. Sin embargo, no soy un experto en cálculo y realmente no entiendo los símbolos matemáticos, pero sí entiendo las matemáticas en el código fuente.

Estoy usando RobotC (que es como cualquier otro derivado de C) y realmente agradecería que alguien me diera ejemplos de cómo lograr esto en C.

¡Gracias por tu ayuda!

RESULTADOS DE LA SOLUCION:

Muy bien, Kersny resolvió mi problema presentándome filtros complementarios. Este es un gráfico que ilustra mis resultados:

Resultado # 1

Resultado # 2

Como puede ver, el filtro corrige la deriva giroscópica y combina ambas señales en una sola señal suave.

Editar: Como estaba arreglando las imágenes rotas de todos modos, pensé que sería divertido mostrar la plataforma que usé para generar estos datos:

Respuestas a la pregunta(2)

Su respuesta a la pregunta