Unidad de Medida Inercial.

Algoritmo de Estimacin e Implementacin Software Desarrollo terico

Captulo 2.

Desarrollo terico

2.1. Representacin matemtica de la orientacin

Dado que el objetivo es expresar la orientacin del sensor con respecto a un marco de referencia fijo, se expone en primer lugar cmo se representa esto matemticamente. Partiendo de unos ejes coordenados que representarn el sistema de referencia de la IMU, las rotaciones respecto a estos ejes provocarn el cambio en la orientacin del objeto. Se definen este marco de referencia y las rotaciones en sentido positivo de la siguiente forma:

Ilustracin 2-1. Definicin de ejes y rotaciones

Es importante recordar que el cambio en la orientacin del objeto, que est sujeto a una serie de rotaciones sobre los diferentes ejes, no es slo una funcin de los ngulos que rota cada uno de estos ejes, sino tambin del orden en que ocurren las rotaciones. 29

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

Existen varias representaciones matemticas para definir la orientacin del objeto respecto al sistema de referencia. Estos se describen a continuacin: Matriz de rotacin: La matriz de rotacin, o DCM (del ingls Direction Cosine Matrix), es una matriz 3x3, cuyas columnas representan los vectores unidad del objeto proyectados sobre los ejes del sistema de referencia. ngulos RPY y ngulos de Euler: Una transformacin de un marco coordinado a otro se define por tres rotaciones sucesivas sobre los diferentes ejes. Los ngulos roll, pitch y yaw representan las tres rotaciones sobre los ejes X, Y y Z, respectivamente. Los ngulos de Euler son una representacin similar, cambiando los ejes sobre los que se realizan las rotaciones y el orden en que se tienen en cuenta. Cuaterniones: Otra forma de entender la rotacin es considerarla como una nica rotacin sobre un vector definido en el marco de referencia. El cuaternin es un vector tetradimensional, cuyos elementos son funcin de este vector y de la magnitud de la rotacin.

2.1.1. Matriz de rotacin

La matriz de rotacin relaciona el sistema de referencia fijo con el del objeto de forma que se puede expresar un vector del sistema asociado al objeto en el sistema fijo, simplemente premultiplicndolo por la matriz de rotacin correspondiente.
A De esta forma, siendo RB la matriz de rotacin del objeto respecto al sistema r r fijo, y r A y r B vectores expresados en el sistema fijo y del objeto, respectivamente, se tiene:

r r r Ar A r A = RB r B ; r B = ( RB ) T r A

Los elementos de la matriz de rotacin son

r11 R = r21 r31

A B

r12 r22 r32

r13 r23 r33

siendo rij el coseno del ngulo entre el eje i del sistema de referencia fijo y el eje j del sistema de referencia del objeto.

30

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

2.1.2. ngulos RPY y ngulos de Euler

Al igual que para navegacin en el plano slo se necesita un ngulo de orientacin (tpicamente el Norte), en el espacio la orientacin se puede expresar con tres ngulos. Los ngulos RPY son roll, pitch y yaw, que en terminologa nutica se corresponderan con alabeo, cabeceo y guiada. En funcin de estos ngulos, expresar la orientacin de un objeto con un sistema de referencia {B} con respecto a un sistema de referencia {A} corresponde a realizar las operaciones siguientes: se parte con {B} coincidente con {A}, se rota {B} alrededor de X un ngulo (roll), despus alrededor de Y un ngulo
A A

(pitch) y finalmente alrededor de Z A un ngulo (yaw).. En la representacin de Euler Z-Y-X, en lugar de realizar tres rotaciones consecutivas alrededor de los ejes del sistema de referencia {A}, las rotaciones se efectan alrededor de los ejes del sistema {B} solidario al cuerpo. Primero se rota un ngulo alrededor de Z , luego alrededor del Y resultante del primer giro, y
B B

posteriormente se rota un ngulo alrededor del eje X B . Los ngulos de Euler Z-Y-Z se obtienen de realizar rotaciones parecidas a las anteriores. En este caso tambin se rota con respecto a {B}, pero el orden de las rotaciones ahora es Z-Y-Z.

2.1.3. Cuaterniones
La representacin de la orientacin mediante cuaterniones es una representacin de cuatro parmetros basada en la idea de que una transformacin de un sistema de referencia a otro puede ser efectuada por una nica rotacin sobre un r r vector definido en el sistema de referencia fijo. El cuaternin, q , es un vector de cuatro elementos que son funcin de este vector y de la magnitud de la rotacin:

cos( / 2) a b ( / ) sen( / 2) r q= = x c ( y / ) sen( / 2) d ( z / ) sen( / 2)

31

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

r donde x, x y x son las componentes del vector y el mdulo de dicho r vector. El parmetro representa el valor de la rotacin sobre el vector .

2.1.4. Relaciones entre matriz de rotacin, ngulos RPY y cuaterniones

Dada una de estas representaciones, existe una relacin entre ella y las dems, de forma que se puede pasar de una representacin a otra aplicando la correspondiente frmula. Llegado a este punto cabe plantearse una situacin importante para el resto del desarrollo del algoritmo. La cuestin es qu representacin es la ms adecuada para calcular de forma continua, es decir, para cul conviene ms seguir su evolucin en funcin de las medidas de los sensores. A priori se podra tomar cualquiera, ya que teniendo una se puede calcular fcilmente la deseada. Clsicamente se ha optado por un algoritmo que va actualizando la matriz de rotacin o los cuaterniones, y es este ltimo el que ms aparece en las ltimas tendencias. Los ngulos RPY y de Euler presentan ms inconvenientes, puesto que en la resolucin de las correspondientes ecuaciones de propagacin en el tiempo aparecen indeterminaciones, debido a que una misma orientacin se puede expresar con distintos ngulos. Los cuaterniones se presentan como la solucin ms adecuada, por ser tan slo cuatro los parmetros a actualizar y por presentar menores errores en la computacin, segn diversos estudios. A continuacin se presentan las otras representaciones en funcin de los cuaterniones: La matriz de rotacin queda:

r11 A RB = r21 r31

r12 r22 r32

r13 (a 2 + b 2 c 2 d 2 ) 2(bc ad ) 2(bd + ac) = 2 2 2 2 r23 2(bc + ad ) (a b + c d ) 2(cd ab) r33 2(bd ac) 2(cd + ab) (a 2 b 2 c 2 + d 2 )

Y los ngulos roll, pitch y yaw se pueden calcular tambin a partir de esta matriz:

32

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

= arctg 2(r32 , r33 )

siendo atan2(x,y) el arco tangente del ngulo x/y, teniendo en cuenta el signo de x e y para determinar el cuadrante (por ejemplo, arctg 2(2,2) = -135).
2 2 = arctg 2( r31 , r11 + r21 )

escogiendo la solucin -90 90, que corresponde a tomar la raz como positiva.

= arctg 2(r21 , r11 )

Existen soluciones degeneradas para = 90 (cos()=0). En estos casos slo puede calcularse la suma o la diferencia de y . Suponiendo = 0, se tienen las soluciones:

= 0, = 90, = arctg 2(r12 , r22 ) = 0, = -90, = - arctg 2(r12 , r22 )

2.1.5. Propagacin de los cuaterniones en el tiempo

Hasta aqu se ha explicado cmo se representa la orientacin del objeto en movimiento con respecto a un marco de referencia fijo. A continuacin se muestra cmo se va transformando esa representacin a lo largo del tiempo, en funcin de las distintas rotaciones en los tres ejes del espacio, ms concretamente de las tres velocidades angulares, que son en definitiva lo que medirn nuestros girscopos. Para el caso de los cuaterniones, que es la representacin que se ha tomado, la ecuacin que define su propagacin es la siguiente. Sean x , y y z las velocidades angulares de la rotacin de los tres ejes del sensor, entonces:

& a a b b & r & q = = 0 .5 c & c & d d

b c a d d a c b

d 0 c x b y a z

33

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

O, lo que es lo mismo,

& 0 a b r & x &= = 0 .5 q y c & & z d

x 0 z

z
0 x

z a 0 y b = 0 .5 x y x c 0 d z

x 0 z

z
0 x

z y r q x 0

2.2. Modelo del sistema

Con la informacin anterior, se puede fcilmente desarrollar un modelo dinmico del sistema en descripcin interna:

r r & x = A x
r donde x es el vector de estado y A la matriz dinmica del sistema.

Una primera aproximacin sera tomar tal cual la ecuacin de propagacin de los cuaterniones, haciendo coincidir al cuaternin con el vector de estado. El resultado sera:

0 r & = 0.5 x x y z

x 0 z

z
0 x

z y r x x 0

Sin embargo, el modelo no se queda aqu. El motivo de la realizacin del modelo es hacer un seguimiento del sistema que nos permita ir integrando las velocidades angulares para que se transformen en los ngulos netos que se ha movido el objeto. Resulta, por tanto, interesante incluir tambin en este modelo la dinmica del giro de los tres ejes. Para modelar el cambio en las velocidades angulares de los tres ejes, se utilizar un sistema de primer orden. Es decir, el movimiento se espera que est dentro de un determinado ancho de banda:

1 / r &= 0 0

0 1/ 0

0 r 0 1/

34

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

siendo

x = y z r

Se puede confeccionar, entonces, el vector de estado para incluir ambas dinmicas en el modelo. El resultado es el siguiente:

x1 x x 2 y x3 z r x = x4 = a x5 b x6 c x d 7

& x 2 / 0 & y & z 0 r & = a = 0 .5 x & & b & c d &

0 2 / 0

0 0 2 / 0 x 0 z 0 y

x y z

z
0 x

x y z z a y b x c 0 d

Ahora el sistema es no lineal, dado que los i son parte del vector de estado, y estn multiplicando a otros estados. El modelo del sistema quedar completo al definir la ecuacin de medida. Esto es la relacin entre las medidas que se pueden obtener de los sensores, y el vector de estado. En el caso de las velocidades angulares i esta relacin es directa, ya que estn directamente incluidas en el vector de estado. Para las medidas de compases magnticos y acelermetros, esta relacin no es otra que la que existe entre la orientacin del objeto y las medidas que nos aportan estos sensores.

35

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

Se ha comentado anteriormente que la orientacin del objeto ser la relacin entre dos sistemas de coordenadas, uno de ellos fijo, que ser el sistema {A}, y el otro solidario al propio objeto, {B}. Se define, en primer lugar, el sistema de coordenadas fijo que servir de referencia, {A}. En ste, el eje X se corresponder con el Norte Geogrfico, el eje Y apuntar hacia el Este, y el eje Z ser hacia abajo, tal como indica la siguiente figura:
N x z y E

Ilustracin 2-2. Sistema de referencia fijo

En este sistema de referencia se definen dos vectores fijos, correspondientes al campo gravitatorio y magntico terrestres. Segn el IGRF (International Geomagnetic Reference Field), las fuerzas gravitatoria y magntica de la Tierra forman los siguientes vectores en nuestra posicin geogrfica, expresados en el marco de referencia que se acaba de exponer:

0 r g = 0 m / s 2 9,82 26,8503 r m = 1,3929 nT 33,7885 Dado que el objetivo es encontrar la orientacin del objeto, slo resulta de utilidad la orientacin de estos vectores, resultando irrelevante el valor de su mdulo. Por tanto, se tomar una versin normalizada de los mismos:

36

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

0 rA g = 0 1 0,62182 rA m = 0,03226 0,78249 Los acelermetros son capaces de medir el vector de aceleracin del campo gravitatorio. Esta medida aporta este vector gravitatorio expresado en el sistema de referencia del objeto, {B}. De la misma forma, con los compases magnticos se obtiene el vector del campo magntico terrestre expresado en {B}. Con esto, el vector de medidas ser:

y1 x y 2 y y3 z B g1B m1B y 4 g1 r r r B B B y = y5 = g 2 , siendo g B = g 2 y m B = m2 B B g 3B m3 y6 g 3 y m B 7 1B y8 m 2 y m B 9 3

r r Los vectores g B y m B sern vectores normalizados, para poder relacionarlos

con los vectores de referencia.

r r Como se ha visto en el apartado 2.1.1, la relacin entre los vectores g y m

expresados en uno y otro sistema de referencia viene dada por:

r Ar g A = RB g B ; r r A g B = ( RB ) T g A

r r r A r A m A = RB m B ; m B = ( RB ) T m A

Y la matriz R se puede expresar en funcin de los cuaterniones, de forma que queda:

37

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

( a 2 + b 2 c 2 d 2 ) 2(bc + ad ) 2(bd ac) rB rA 2 2 2 2 g = 2(bc ad ) (a b + c d ) 2(cd + ab) g 2 2 2 2 2(bd + ac) 2(cd ab) (a b c + d ) ( a 2 + b 2 c 2 d 2 ) 2(bc + ad ) 2(bd ac) rB rA m = 2(bc ad ) (a 2 b 2 + c 2 d 2 ) 2(cd + ab) m 2 2 2 2 2(bd + ac) 2(cd ab) (a b c + d ) Ntese que a, b, c y d son en realidad las componentes del vector de estado, x4, x5, x6 y x7. Con esto se ha obtenido una relacin entre las medidas y los elementos del vector de estado. Nuevamente, esta relacin no es lineal con respecto al vector de estado. El modelo completo del sistema queda de esta forma:

& x1 = (1 / ) x1 & x2 = (1 / ) x2 & x3 = (1 / ) x3 & x4 = ( x1 x5 x2 x6 x3 x7 ) / 2 & x5 = ( x1 x4 + x3 x6 x2 x7 ) / 2 & x6 = ( x2 x4 x3 x5 + x1 x7 ) / 2 & x7 = ( x3 x4 + x2 x5 x1 x6 ) / 2

y1 = x1 y2 = x2 y3 = x3 y4 = 2( x5 x7 x4 x6 ) y5 = 2( x6 x7 + x4 x5 )
2 2 2 2 y6 = x4 x5 x6 + x7 2 2 2 2 y7 = ( x4 + x5 x6 x7 )0,62182 2( x5 x6 + x4 x7 )0,03226 + 2( x5 x7 x4 x6 )0,78249 2 2 2 2 y8 = 2( x5 x6 x4 x7 )0,62182 ( x4 x5 + x6 x7 )0,03226 + 2( x6 x7 + x4 x5 )0,78249 2 2 2 2 y9 = 2( x5 x7 + x4 x6 )0,62182 2( x6 x7 x4 x5 )0,03226 + ( x4 x5 x6 + x7 )0,78249

38

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

2.3. Filtro de Kalman

La orientacin del objeto es la informacin que se quiere obtener del sistema. Esta informacin se encuentra en el vector de estados, concretamente en sus cuatro ltimas componentes, que es el cuaternin que define dicha orientacin. Para conocer el valor de ese vector de estado a lo largo de la evolucin del sistema en el tiempo se utilizar un filtro de Kalman. El filtro de Kalman aporta un procedimiento ptimo para estimar el estado de un sistema, minimizando el valor cuadrtico medio del error cometido en esa estimacin.

2.3.1. Filtro de Kalman discreto

r El filtro de Kalman trata de estimar el estado x n de un proceso en tiempo discreto gobernado por la ecuacin en diferencias lineal estocstica r r r r xk = Axk 1 + Buk 1 + wk 1 r con una medida y m que es r r r y k = Hxk + vk r r Las variables aleatorias wk y vk representan el ruido en el proceso y en la medida, respectivamente. Se suponen independientes, blancos y con una distribucin normal dada por
r p ( w) N (0, Q) r p (v ) N (0, R ) donde las matrices Q y R son las matrices de covarianza del ruido en el proceso y en la medida, respectivamente, y se suponen constantes. Se define tambin la matriz de covarianza del error del estado, Pk. Esta matriz si evolucionar a lo largo de las sucesivas iteraciones. Pk = E ( xk xk )( xk xk )T siendo xk la estimacin del vector de estado. Para estimar el vector de estado, el filtro de Kalman consta de dos pasos. En el primero se intenta predecir el valor del vector de estado y la covarianza de su

39

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

error, a partir de la ecuacin dinmica del sistema. En el segundo se corrige esta prediccin y se actualiza el vector de estado y la covarianza de su error, teniendo en cuenta la ecuacin de medida. Para cada iteracin k, se sigue el siguiente algoritmo:
r xk = Axk 1 + Bu k 1

Pk = APk 1 AT + Q

K k = Pk H T ( HPk H T + R ) 1 r xk = xk + K k ( yk Hxk ) Pk = ( I K k H ) Pk

2.3.2. Filtro de Kalman Extendido (EKF)

En el apartado anterior se supona un sistema lineal. Se ha visto anteriormente que no es ese nuestro caso, por lo que hay que hacer uso del filtro de Kalman Extendido. Esta versin del filtro hace uso del teorema de Taylor y es vlida para un sistema del tipo
r r r r xk = f ( xk 1 , uk 1 , wk 1 ) r r r yk = h( xk , vk )

Se definen las matrices jacobianas: A[i , j ] = W[i , j ] = H [i , j ] = V[i , j ] =

f [i ] r ( xk 1 , uk 1 ,0) x[ j ] f [i ] r ( xk 1 , u k 1 ,0) w[ j ] h[i ] ( xk ,0) x[ j ] h[i ] v[ j ]

( x k ,0 )

Y ahora el algoritmo queda como sigue:

40

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

r xk = f ( xk 1 , uk 1 ,0)
T Pk = Ak Pk 1 Ak + Wk Qk 1WkT T T K k = Pk H k ( H k Pk H k + Vk RkVkT ) 1 r xk = xk + K k ( yk h( xk ,0))

Pk = ( I K k H k ) Pk

2.4. Algoritmo de estimacin

Con la informacin anterior ya se puede proceder a detallar el algoritmo que ser capaz de seguir en tiempo real los cambios en la orientacin del sensor de medida inercial. En primer lugar se ha de discretizar el sistema para trabajar con las ecuaciones en diferencias. Bastar con usar la aproximacin de Euler hacia adelante (Forward Euler), que consiste en aproximar la derivada segn la relacin siguiente:
& x= f ( x) xk +1 xk t T

Se ha introducido aqu un parmetro que muy importante: el tiempo de muestreo T. Este tiempo es el que transcurre entre una iteracin y otra del algoritmo, y debe ser lo suficientemente pequeo como para que la aproximacin sea vlida, pero lo suficientemente grande como para que al procesador le d tiempo a realizar todos los clculos, adems de actualizar todas las medidas de los sensores. Finalmente, el tiempo de muestreo ser de 20 milisegundos. Esto proporciona una tasa de actualizacin de la orientacin de 50 Hz, que es lo mnimo requerido por el DSP de control del helicptero, ya que es esa la frecuencia con la que va actualizando su estado. Una vez pasado a ecuaciones en diferencias y aadido el modelo de ruido, el sistema queda as:

r r r Ecuacin dinmica xk = f ( xk 1 , wk 1 ) :

41

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

x1k = x1k 1 + (T / ) x1k 1 + Tw1k 1 x2k = x2k 1 + (T / ) x2k 1 + Tw2k 1 x3k = x3k 1 + (T / ) x3k 1 + Tw3k 1 x4k = x4k 1 + T ( x1k 1 x5k 1 x2k 1 x6k 1 x3k 1 x7k 1 ) / 2 + Tw4k 1 x5k = x5k 1 + T ( x1k 1 x4k 1 + x3k 1 x6k 1 x2k 1 x7k 1 ) / 2 + Tw5k 1 x6k = x6k 1 + T ( x2k 1 x4k 1 x3k 1 x5k 1 + x1k 1 x7k 1 ) / 2 + Tw6k 1 x7k = x7k 1 + T ( x3k 1 x4k 1 + x2k 1 x5k 1 x1k 1 x6k 1 ) / 2 + Tw7k 1

r r r Ecuacin de medida yk = h( xk , vk ) :

y1k = x1k + v1k y2k = x2k + v2k y3k = x3k + v3k y4k = 2( x5k x7k x4k x6k ) + v4k y5k = 2( x6k x7k + x4k x5k ) + v5k
2 2 2 2 y6k = x4k x5k x6k + x7k + v6k 2 2 2 2 y7k = ( x4k + x5k x6k x7k )0,62182 2( x5k x6k + x4k x7k )0,03226 + 2( x5k x7k x4k x6k )0,78249 + v7k 2 2 2 2 y8k = 2( x5k x6k x4k x7k )0,62182 ( x4k x5k + x6k x7k )0,03226 + 2( x6k x7k + x4k x5k )0,78249 + v8k 2 2 2 2 y9k = 2( x5k x7k + x4k x6k )0,62182 2( x6k x7k x4k x5k )0,03226 + ( x4k x5k x6k + x7k )0,78249 + v9k

r r Las covarianzas de los ruidos wk y vk se definen en base a las desviaciones

tpicas de las seales de los sensores. A priori, estas desviaciones tpicas vendrn dadas por las especificaciones de los fabricantes de los sensores. Girscopo: Segn el datasheet del fabricante, el ruido a 25 C es 0.1 / s / Hz . Para un ancho de banda de 30 Hz el ruido en la medida ser aproximadamente 0.5 / s . Esto es la desviacin tpica del ruido en la velocidad angular. Esto es, ajustado a las unidades usadas en el modelo, /360 rad/s.
Acelermetro: El dato terico sita el espectro de ruido en el acelermetro en 0.225 mg / Hz . Para nuestro ancho de banda de 100 Hz queda una desviacin tpica de 2.25 mg.

42

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

Comps magntico: El fabricante no proporciona un dato exacto al respecto, pero podemos hacer una estimacin. Suponiendo un error de entre 1 y 2 grados en la medida, la desviacin tpica del ruido en las componentes del vector magntico asociado se puede suponer en primera aproximacin del orden de 0.03 (no tiene unidades porque el vector se encuentra normalizado). La relacin entre el error en el ngulo que nos da el comps magntico y el error sobre los ejes del triedro de referencia no es lineal, ya que se basa en senos y cosenos. Esta aproximacin no es ms que una media entre el mejor y el peor caso. A continuacin se detalla cmo queda finalmente el algoritmo de estimacin.

Antes de comenzar el bucle de control, se pueden definir las matrices W y V, ya que son constantes y no har falta tener que definirlas en cada iteracin.

W[i , j ] = V[i , j ] =

f [i ] ( xk 1 ,0) W = T I 7 x 7 w[ j ] h[i ] ( x k ,0 ) V = I 9 x 9 v[ j ]

Se puede definir tambin las matrices de covarianza del ruido en el proceso y en la medida, Q y R. La matriz Q modela el error que se comete en la actualizacin del estado usando la ecuacin dinmica del sistema. En principio se le asignar el siguiente valor: 0 0 0 0 0 0 .1 0 0 0 .1 0 0 0 0 0 0 0 0 .1 0 0 0 0 Q=0 0 0 0.01 0 0 0 0 0 0 0 0.01 0 0 0 0 0 0 0.01 0 0 0 0 0 0 0 0 0.01

La matriz R modela el error en la medida, y se puede definir en funcin de las varianzas de las diferentes componentes del vector de medidas. Llamando v, r y s a las desviaciones tpicas comentadas anteriormente, en el mismo orden, queda la siguiente matriz:

43

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

v 2 0 0 0 R = 0 0 0 0 0

0 v 0 0 0 0 0 0 0
2

0 0 v2 0 0 0 0 0 0

0 0 0 r2 0 0 0 0 0

0 0 0 0 r2 0 0 0 0

0 0 0 0 0 r2 0 0 0

0 0 0 0 0 0 s2 0 0

0 0 0 0 0 0 0 s2 0

0 0 0 0 0 0 0 0 s2

Finalmente, se procede a comenzar el bucle. Para cada iteracin k se hace lo siguiente: -

r Actualizacin de las medidas: se actualiza el vector yk con las medidas de girscopos, acelermetros y compases magnticos.
Clculo de la matriz Ak.

A[i , j ] =

f [i ] ( xk 1 ,0) x[ j ]

T 1+ 0 0 Ak = T x 2 5k 1 T x4k 1 2 Tx 2 7k 1 T x6k 1 2

0 1+ T

0 0 1+ T

0 0 0 1 T x1 2 k 1 T x2 2 k 1 T x3 2 k 1

0 0 0 T x1 2 k 1 1 T x3 2 k 1 T x2 2 k 1

0 0 0 T x2 2 k 1 T x3 2 k 1 1 T x1 2 k 1

0 T x6 2 k 1 T x7k 1 2 T x4 2 k 1 T x5 2 k 1

T x7 2 k 1 T x6 2 k 1 T x5k 1 2 T x4 2 k 1

0 0 T x3k 1 2 T x2k 1 2 T x1k 1 2 1 0

44

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

Clculo del vector de estado a priori, xk , usando el modelo dinmico del sistema.

xk = f ( xk 1 ,0)

Clculo de la matriz Hk.

H [i , j ] = 1 0 0 0 0 0 Hk = 0 0 0 0 0 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2x 2x
6k 5k 4k

h[i ] ( xk ,0) x[ j ] 0 0 0 2x 2x
7k 4k 5k

0 0 0
2 x4k 2 x7k

2x A 2 x4k m1 + 2 x7k m2A

2 x6k m3A 2 x7k m1A + 2 x4k m2A + 2 x5k m3A 2 x6k m1A 2 x5k m2A + 2 x4k m3A

2x 2 x5k m1A + 2 x6k m2A

+ 2 x7k m3A 2 x6k m1A 2 x5k m2A 2 x4k m3A 2 x7k m1A 2 x4k m2A 2 x5k m3A

2 x6k 2 x6k m1A + 2 x5k m2A 2 x4k m3A 2 x5k m1A + 2 x6k m2A + 2 x7k m3A 2 x4k m1A + 2 x7k m2A 2 x6k m3A

0 0 2 x5k 2 x6k 2 x7k A A 2 x7k m1 + 2 x4k m2 + 2 x5k m3A A A 2 x4k m1 2 x7k m2 + 2 x6k m3A 2 x5k m1A + 2 x6k m2A + 2 x7k m3A 0

Clculo de la covarianza del vector de estado a priori, Pk-.

T Pk = Ak Pk 1 Ak + WQW T

Clculo de la ganancia de Kalman, Kk.

45

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software Desarrollo terico

T T K k = Pk H k ( H k Pk H k + VRV T ) 1

r Actualizacin de x y de su matriz de covarianza.

r xk = xk + K k ( yk h( xk ,0))

Pk = ( I K k H k ) Pk

46