El tiro por la culata

Al cabo de un rato terminé encontrando el motivo por el que recibía esos valores negativos tan extraños. Aunque todo indicaba lo contrario el valor devuelto por la instrucción xhat[-1] es correcto para el vector xhat (que raro que yo me equivoque).

Al principio pensé que el problema radicaba en que yo solicitaba el valor del vector cuando este estaba siendo tratado por el filtro. Me explico: actualmente tengo dos clases, una llamada Kalman encargada de filtrar la información cuando se agreguen valores leídos por el mando, y la clase Wiimote que entre otras muchas cosas trata las "interrupciones" del mando, entregándosela al objeto kalman para que la filtre, un momento después se solicita el último dato del vector ya filtrado para así calcular velocidad y espacio en tiempo real.

Para evitar esto (un error de programación por mi parte) creé un vector nuevo, copia del anterior, que sólo se actualiza cuando toda la información ha sido filtrada, de forma que siempre devolverá un valor filtrado (aunque sea uno que ya ha sido entregado). Pero esto tampoco funciona, los valores extraños siguen apareciendo, y ahora mismo no se como resolverlo.

Calcularé velocidad y espacio una vez terminado un movimiento, no es igual de 'bonito' pero creo que es la última baza que me queda para utilizar el mando (al menos con el enfoque actual)