Suponiendo que esté muestreando en un paso de tiempo fijo, denominado \ $ \ Delta \ $, y \ $ t_n \ $ denote sus tiempos de muestreo (es decir, \ $ t_n = n \ Delta \ $), entonces una estimación tradicional de la velocidad es
$$ v (t_ {n}) \ sim \ sum_ {k = 0} ^ {n-1} a (t_k) \ Delta $$
donde \ $ a (t_k) \ $ es el valor de aceleración en el tiempo \ $ t_k \ $ según lo muestrea su acelerómetro. Parece que también estás asumiendo las condiciones iniciales de que \ $ v (t_0) = 0 \ $ y \ $ p (t_0) = 0 \ $.
Aplicando esta idea nuevamente para obtener la posición mediante la integración de la velocidad calculada que obtenemos
$$ p (t_ {n}) \ sim \ sum_ {k = 0} ^ {n-1} v (t_k) \ Delta \ sim \ sum_ {k = 0} ^ {n-1} \ sum_ {i = 0} ^ {k-1} a (t_i) \ Delta ^ 2. $$
Esta última suma simplifica un poco a
$$ \ sum_ {k = 0} ^ {n-1} \ sum_ {i = 0} ^ {k-1} a (t_i) \ Delta ^ 2 = \ sum_ {i = 0} ^ {n-2} (n-1-i) a (t_i) \ Delta ^ 2 = \ left (\ sum_ {i = 0} ^ {n-3} (n-2-i) a (t_i) \ Delta ^ 2 \ right) + \ sum_ {i = 0} ^ {n-2} a (t_i) \ Delta ^ 2. $$
Ahora, si observa que el bit entre paréntesis es nuestra estimación de \ $ p (t_ {n-1}) \ $, verá que obtenemos la fórmula recursiva \ $ p (t_ {n}) = p (t_ { n-1}) + \ sum_ {i = 0} ^ {n-2} a (t_i) \ Delta ^ 2 \ $. Esto nos permite calcular fácilmente \ $ p (t_ {n}) \ $ mediante el seguimiento de dos (y solo dos) valores:
\ $ p (t_ {n-1}) \ $ y una variable auxiliar \ $ s_n = \ sum_ {i = 0} ^ {n-2} a (t_i) \ Delta ^ 2 \ $. Tenga en cuenta que tenemos las fórmulas recursivas $$ s_n = s_ {n-1} + a (t_ {n-2}) \ Delta ^ 2 $$
y
$$ p (t_n) = p (t_ {n-1}) + s_n. $$
Su algoritmo de cálculo será algo como esto:
1) Inicial: \ $ p (t_0) = p (t_1) = 0 \ $. \ $ s_2 = a (t_0) \ Delta ^ 2 \ $.
2) En la etapa \ $ n \ $: \ $ s_n = a (t_ {n-2}) \ Delta ^ 2 + s_ {n-1} \ $; libere la memoria que contiene \ $ s_ {n-1} \ $; \ $ p (t_ {n}) = p (t_ {n-1}) + s_ {n} \ $.
3) Repita.
Algunos comentarios pequeños: probablemente querrás retrasar la multiplicación de todo por \ $ \ Delta ^ 2 \ $ hasta el final. Tendrá que experimentar con el tamaño del tipo de datos de punto flotante que necesita para no desbordarlo. No necesita usar variables separadas para \ $ s_n \ $; en XC tendría una declaración similar a \ $ s \ hspace {5pt} + \ hspace {-5pt} = a (t_ {n-2}) \ Delta ^ 2 \ $.
