Unidad de Medida Inercial.

Algoritmo de Estimación e Implementación Software

Desarrollo teórico

Capítulo 2. Desarrollo teórico

2.1. Representación matemática de la orientación

Dado que el objetivo es expresar la orientación del sensor con respecto a un

marco de referencia fijo, se expone en primer lugar cómo se representa esto
matemáticamente.

Partiendo de unos ejes coordenados que representarán el sistema de

referencia de la IMU, las rotaciones respecto a estos ejes provocarán el cambio en la
orientación del objeto. Se definen este marco de referencia y las rotaciones en
sentido positivo de la siguiente forma:

Ilustración 2-1. Definición de ejes y rotaciones

Es importante recordar que el cambio en la orientación del objeto, que está

sujeto a una serie de rotaciones sobre los diferentes ejes, no es sólo una función de
los ángulos que rota cada uno de estos ejes, sino también del orden en que ocurren
las rotaciones.

29
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

Existen varias representaciones matemáticas para definir la orientación del

objeto respecto al sistema de referencia. Estos se describen a continuación:

Matriz de rotación: La matriz de rotación, o DCM (del inglés 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 transformación 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 representación 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 rotación es considerarla como una

única rotación sobre un vector definido en el marco de referencia. El cuaternión es
un vector tetradimensional, cuyos elementos son función de este vector y de la
magnitud de la rotación.

2.1.1. Matriz de rotación

La matriz de rotación 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 premultiplicándolo por la matriz de rotación
correspondiente.

De esta forma, siendo RBA la matriz de rotación 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 r
r A = RBA r B ; r B = ( RBA )T r A

Los elementos de la matriz de rotación son

 r11 r12 r13 

R = r21
A
B r22 r23 
r31 r32 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 Estimación e Implementación Software
Desarrollo teórico

2.1.2. Ángulos RPY y ángulos de Euler

Al igual que para navegación en el plano sólo se necesita un ángulo de

orientación (típicamente el Norte), en el espacio la orientación se puede expresar
con tres ángulos.

Los ángulos RPY son roll, pitch y yaw, que en terminología náutica se
corresponderían con alabeo, cabeceo y guiñada.

En función de estos ángulos, expresar la orientación 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), después alrededor de Yˆ un ángulo β
A A

(pitch) y finalmente alrededor de Ẑ A un ángulo α (yaw)..

En la representación de Euler Z-Y-X, en lugar de realizar tres rotaciones

consecutivas alrededor de los ejes del sistema de referencia {A}, las rotaciones se
efectúan alrededor de los ejes del sistema {B} solidario al cuerpo. Primero se rota un
ángulo α alrededor de Ẑ , 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 también se rota con respecto a {B}, pero el orden de las
rotaciones ahora es Z-Y-Z.

2.1.3. Cuaterniones

La representación de la orientación mediante cuaterniones es una

representación de cuatro parámetros basada en la idea de que una transformación de
un sistema de referencia a otro puede ser efectuada por una única rotación sobre un
r r
vector µ definido en el sistema de referencia fijo. El cuaternión, q , es un vector de
cuatro elementos que son función de este vector y de la magnitud de la rotación:

a  cos(θ / 2) 
 b  ( µ / µ ) sen(θ / 2) 
r
q= = x 
 c  ( µ y / µ ) sen(θ / 2)
   
d   ( µ z / µ ) sen(θ / 2) 

31
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

r
donde µx, µx y µx son las componentes del vector µ y µ el módulo de dicho
r
vector. El parámetro θ representa el valor de la rotación sobre el vector µ .

2.1.4. Relaciones entre matriz de rotación, ángulos RPY y

cuaterniones

Dada una de estas representaciones, existe una relación entre ella y las
demás, de forma que se puede pasar de una representación a otra aplicando la
correspondiente fórmula.

Llegado a este punto cabe plantearse una situación importante para el resto
del desarrollo del algoritmo. La cuestión es qué representación es la más adecuada
para calcular de forma continua, es decir, para cuál conviene más seguir su
evolución en función de las medidas de los sensores. A priori se podría tomar
cualquiera, ya que teniendo una se puede calcular fácilmente la deseada.

Clásicamente se ha optado por un algoritmo que va actualizando la matriz de

rotación o los cuaterniones, y es este último el que más aparece en las últimas
tendencias. Los ángulos RPY y de Euler presentan más inconvenientes, puesto que
en la resolución de las correspondientes ecuaciones de propagación en el tiempo
aparecen indeterminaciones, debido a que una misma orientación se puede expresar
con distintos ángulos.

Los cuaterniones se presentan como la solución más adecuada, por ser tan
sólo cuatro los parámetros a actualizar y por presentar menores errores en la
computación, según diversos estudios.

A continuación se presentan las otras representaciones en función de los

cuaterniones:

La matriz de rotación queda:

 r11 r12 r13  (a 2 + b 2 − c 2 − d 2 ) 2(bc − ad ) 2(bd + ac) 

 
RB = r21
A
r22 
r23  =  2(bc + ad ) (a − b + c − d )
2 2 2 2
2(cd − ab) 
r31 r32 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 también a partir de esta
matriz:

32
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

γ = 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º).

β = arctg 2(− r31 , r112 + r212 )

escogiendo la solución -90º ≤ β ≤ 90º, que corresponde a tomar la raíz como

positiva.

α = arctg 2(r21 , r11 )

Existen soluciones degeneradas para β = ±90º (cos(β)=0). En estos casos sólo

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. Propagación de los cuaterniones en el tiempo

Hasta aquí se ha explicado cómo se representa la orientación del objeto en

movimiento con respecto a un marco de referencia fijo. A continuación se muestra
cómo se va transformando esa representación a lo largo del tiempo, en función de las
distintas rotaciones en los tres ejes del espacio, más concretamente de las tres
velocidades angulares, que son en definitiva lo que medirán nuestros giróscopos.

Para el caso de los cuaterniones, que es la representación que se ha tomado,

la ecuación que define su propagación es la siguiente.

Sean ω x , ω y y ω z las velocidades angulares de la rotación de los tres ejes

del sensor, entonces:

 a&  a −b −c − d  0 
r
 b&  b a −d c  ω x 
q& =   = 0.5
 c&  c d a − b  ω y 
 &   
d  d −c b a  ω z 

33
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

O, lo que es lo mismo,

 a&  0 − ωx −ωy − ω z a  0 − ωx −ωy − ωz 

 b&  ω 0 ωz 
−ωy  b   ω 0 ωz − ω y  r
r&  
q= = 0 .5    = 0 .5  x
x
q
 c&  ω y − ωz 0 ωx  c  ω y − ωz 0 ωx 
 &     
d  ω z ωy − ωx 0  d  ω z ωy − ωx 0 

2.2. Modelo del sistema

Con la información anterior, se puede fácilmente desarrollar un modelo

dinámico del sistema en descripción interna:

r r
x& = A ⋅ x
r
donde x es el vector de estado y A la matriz dinámica del sistema.

Una primera aproximación sería tomar tal cual la ecuación de propagación de

los cuaterniones, haciendo coincidir al cuaternión con el vector de estado. El
resultado sería:

0 − ωx − ωy − ωz 
ω 0 ωz − ω y  r
r&
x = 0 .5 
x
x
ω y − ωz 0 ωx 
 
ω z ωy − ωx 0 

Sin embargo, el modelo no se queda aquí. El motivo de la realización 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 también en este modelo la
dinámica 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 / τ ω 0 0 
r& 
0 ω
r
ω= 0 1/τ ω
 0 0 1 / τ ω 

34
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

siendo

ω x 
ω = ω y 
r

ω z 

Se puede confeccionar, entonces, el vector de estado para incluir ambas

dinámicas 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 0  ω x 
ω&   0  ω 
 y  2 /τω 0 0  y 
ω& z   0 0 2 /τω  ω z 
r&   − ωz  a 
x =  a&  = 0.5 0 − ωx − ωy
 
 b&   ωx 0 ωz − ωy  b 
   0
ωy − ωz 0 ω x   c 
 c&   
 d& 
   ωz ωy − ωx 0   d 

Ahora el sistema es no lineal, dado que los ω i son parte del vector de estado,
y están multiplicando a otros estados.

El modelo del sistema quedará completo al definir la ecuación de medida.

Esto es la relación entre las medidas que se pueden obtener de los sensores, y el
vector de estado. En el caso de las velocidades angulares ωi esta relación es directa,
ya que están directamente incluidas en el vector de estado. Para las medidas de
compases magnéticos y acelerómetros, esta relación no es otra que la que existe
entre la orientación del objeto y las medidas que nos aportan estos sensores.

35
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

Se ha comentado anteriormente que la orientación del objeto será la relación

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 Geográfico, el eje Y apuntará hacia el Este, y el eje Z será hacia abajo,
tal como indica la siguiente figura:

N
x

z E
y

Ilustración 2-2. Sistema de referencia fijo

En este sistema de referencia se definen dos vectores fijos, correspondientes

al campo gravitatorio y magnético terrestres. Según el IGRF (International
Geomagnetic Reference Field), las fuerzas gravitatoria y magnética de la Tierra
forman los siguientes vectores en nuestra posición geográfica, expresados en el
marco de referencia que se acaba de exponer:

 0 
g =  0  m / s 2
r 

9,82
 26,8503
m = − 1,3929 nT
r 

 33,7885 

Dado que el objetivo es encontrar la orientación del objeto, sólo resulta de

utilidad la orientación de estos vectores, resultando irrelevante el valor de su
módulo. Por tanto, se tomará una versión normalizada de los mismos:

36
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

0 
rA  
g = 0 
1 
 0,62182 
m = − 0,03226
rA 

 0,78249 

Los acelerómetros son capaces de medir el vector de aceleración 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 magnéticos se
obtiene el vector del campo magnético terrestre expresado en {B}.

Con esto, el vector de medidas será:

 y1   ω x 
 y  ω 
 2  y 
 y3   ω z 
   B
 y 4   g1   g1B  m1B 
r r   r  
y =  y5  =  g 2B  , siendo g B =  g 2B  y m B = m2B 
   B
 y6   g 3   g 3B  m3B 
   
 y  m B 
 7   1B 
 y8   m 2 
 y  m B 
 9  3 

r r
Los vectores g B y m B serán vectores normalizados, para poder relacionarlos
con los vectores de referencia.
r r
Como se ha visto en el apartado 2.1.1, la relación entre los vectores g y m
expresados en uno y otro sistema de referencia viene dada por:
r r r r
g A = RBA g B ; g B = ( RBA )T g A

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

Y la matriz R se puede expresar en función de los cuaterniones, de forma que

queda:

37
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

( a 2 + b 2 − c 2 − d 2 ) 2(bc + ad ) 2(bd − ac) 

rB  rA
g = 2(bc − ad ) (a − b + c − d )
2 2 2 2
2(cd + ab) g
 2(bd + ac) 2(cd − ab) (a − b − c + d )
2 2 2 2


( 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(bd + ac) 2(cd − ab) (a − b − c + d )
2 2 2 2


Nótese 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 relación entre las medidas y los
elementos del vector de estado. Nuevamente, esta relación no es lineal con respecto
al vector de estado.

El modelo completo del sistema queda de esta forma:

x&1 = (1 / τ ω ) x1
x& 2 = (1 / τ ω ) x2
x&3 = (1 / τ ω ) x3
x& 4 = (− x1 x5 − x2 x6 − x3 x7 ) / 2
x&5 = ( x1 x4 + x3 x6 − x2 x7 ) / 2
x&6 = ( x2 x4 − x3 x5 + x1 x7 ) / 2
x&7 = ( 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 )
y6 = x42 − x52 − x62 + x72
y7 = ( x42 + x52 − x62 − x72 )0,62182 − 2( x5 x6 + x4 x7 )0,03226 + 2( x5 x7 − x4 x6 )0,78249
y8 = 2( x5 x6 − x4 x7 )0,62182 − ( x42 − x52 + x62 − x72 )0,03226 + 2( x6 x7 + x4 x5 )0,78249
y9 = 2( x5 x7 + x4 x6 )0,62182 − 2( x6 x7 − x4 x5 )0,03226 + ( x42 − x52 − x62 + x72 )0,78249

38
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

2.3. Filtro de Kalman

La orientación del objeto es la información que se quiere obtener del sistema.

Esta información se encuentra en el vector de estados, concretamente en sus cuatro
últimas componentes, que es el cuaternión que define dicha orientación.

Para conocer el valor de ese vector de estado a lo largo de la evolución 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
cuadrático medio del error cometido en esa estimación.

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 ecuación en diferencias lineal estocástica
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 distribución
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 también
la matriz de covarianza del error del estado, Pk. Esta matriz si evolucionará a lo
largo de las sucesivas iteraciones.

[
Pk = E ( xk − xˆ k )( xk − xˆ k )T ]
siendo x̂k la estimación 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 Estimación e Implementación Software
Desarrollo teórico

error, a partir de la ecuación dinámica del sistema. En el segundo se corrige esta

predicción y se actualiza el vector de estado y la covarianza de su error, teniendo en
cuenta la ecuación de medida.

Para cada iteración k, se sigue el siguiente algoritmo:

r
xˆk− = Axˆk −1 + Bu k −1

Pk− = APk −1 AT + Q

K k = Pk− H T ( HPk− H T + R ) −1
r
xˆk = xˆk− + K k ( yk − Hxˆk− )
Pk = ( I − K k H ) Pk−

2.3.2. Filtro de Kalman Extendido (EKF)

En el apartado anterior se suponía 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 versión del filtro hace uso del teorema de Taylor y es válida
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:

∂f [i ] r
A[i , j ] = ( xˆk −1 , uk −1 ,0)
∂x[ j ]
∂f [i ] r
W[i , j ] = ( xˆk −1 , u k −1 ,0)
∂w[ j ]
∂h[i ] −
H [i , j ] = ( xˆk ,0)
∂x[ j ]
∂h[i ]
V[i , j ] = ( xˆk− ,0)
∂v[ j ]

Y ahora el algoritmo queda como sigue:

40
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

r
xˆk− = f ( xˆk −1 , uk −1 ,0)

Pk− = Ak Pk −1 AkT + Wk Qk −1WkT

K k = Pk− H kT ( H k Pk− H kT + Vk RkVkT ) −1

r
xˆk = xˆk− + K k ( yk − h( xˆk− ,0))
Pk = ( I − K k H k ) Pk−

2.4. Algoritmo de estimación

Con la información anterior ya se puede proceder a detallar el algoritmo que

será capaz de seguir en tiempo real los cambios en la orientación 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 aproximación de Euler hacia adelante
(Forward Euler), que consiste en aproximar la derivada según la relación siguiente:

∂f ( x) xk +1 − xk
x& = ≈
∂t T

Se ha introducido aquí un parámetro que muy importante: el tiempo de

muestreo T. Este tiempo es el que transcurre entre una iteración y otra del algoritmo,
y debe ser lo suficientemente pequeño como para que la aproximación sea válida,
pero lo suficientemente grande como para que al procesador le dé tiempo a realizar
todos los cálculos, además de actualizar todas las medidas de los sensores.
Finalmente, el tiempo de muestreo será de 20 milisegundos. Esto proporciona una
tasa de actualización de la orientación de 50 Hz, que es lo mínimo requerido por el
DSP de control del helicóptero, ya que es esa la frecuencia con la que va
actualizando su estado.

Una vez pasado a ecuaciones en diferencias y añadido el modelo de ruido, el

sistema queda así:

r r r
Ecuación dinámica xk = f ( xk −1 , wk −1 ) :

41
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

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
Ecuación 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
y6k = x42k − x52k − x62k + x72k + v6k
y7k = ( x42k + x52k − x62k − x72k )0,62182 − 2( x5k x6k + x4k x7k )0,03226 + 2( x5k x7k − x4k x6k )0,78249 + v7k
y8k = 2( x5k x6k − x4k x7k )0,62182 − ( x42k − x52k + x62k − x72k )0,03226 + 2( x6k x7k + x4k x5k )0,78249 + v8k
y9k = 2( x5k x7k + x4k x6k )0,62182 − 2( x6k x7k − x4k x5k )0,03226 + ( x42k − x52k − x62k + x72k )0,78249 + v9k

r r
Las covarianzas de los ruidos wk y vk se definen en base a las desviaciones
típicas de las señales de los sensores. A priori, estas desviaciones típicas vendrán
dadas por las especificaciones de los fabricantes de los sensores.

- Giróscopo: Según 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 desviación típica del ruido en la
velocidad angular. Esto es, ajustado a las unidades usadas en el modelo,
π/360 rad/s.

- Acelerómetro: El dato teórico sitúa el espectro de ruido en el acelerómetro en

0.225 mg / Hz . Para nuestro ancho de banda de 100 Hz queda una
desviación típica de 2.25 mg.

42
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

- Compás magnético: El fabricante no proporciona un dato exacto al respecto,

pero podemos hacer una estimación. Suponiendo un error de entre 1 y 2
grados en la medida, la desviación típica del ruido en las componentes del
vector magnético asociado se puede suponer en primera aproximación del
orden de 0.03 (no tiene unidades porque el vector se encuentra normalizado).
La relación entre el error en el ángulo que nos da el compás magnético y el
error sobre los ejes del triedro de referencia no es lineal, ya que se basa en
senos y cosenos. Esta aproximación no es más que una media entre el mejor
y el peor caso.

A continuación se detalla cómo queda finalmente el algoritmo de estimación.

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 iteración.

∂f [i ]
W[i , j ] = ( xˆk −1 ,0) ⇒ W = T ⋅ I 7 x 7
∂w[ j ]
∂h[i ] −
V[i , j ] = ( xˆk ,0) ⇒ V = I 9 x 9
∂v[ j ]

Se puede definir también 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 actualización
del estado usando la ecuación dinámica del sistema. En principio se le asignará el
siguiente valor:

 0 .1 0 0 0 0 0 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 0.01 0 
0 0.01
 0 0 0 0 0

La matriz R modela el error en la medida, y se puede definir en función de

las varianzas de las diferentes componentes del vector de medidas. Llamando v, r y s
a las desviaciones típicas comentadas anteriormente, en el mismo orden, queda la
siguiente matriz:

43
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

v 2 0 0 0 0 0 0 0 0
 2 
0 v 0 0 0 0 0 0 0
0 0 v2 0 0 0 0 0 0
 
0 0 0 r2 0 0 0 0 0
R = 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 iteración k se hace lo

siguiente:
r
- Actualización de las medidas: se actualiza el vector yk con las medidas de
giróscopos, acelerómetros y compases magnéticos.

- Cálculo de la matriz Ak.

∂f [i ]
A[i , j ] = ( xˆk −1 ,0) ⇒
∂x[ j ]

 T 
 1+ τ 0 0 0 0 0 0 
ω
 
 0 T
1+ 0 0 0 0 0 
 τω 
 T 
 0 0 1+ 0 0 0 0 
 τω 
Ak = − T x −
T
x6 −
T
x7 1 −
T
x1 −
T
x2
T
− x3k −1 

 2 5k −1 2 k −1 2 k −1 2 k −1 2 k −1 2
T T T T T T 
 x4k −1 − x7k −1 x6 x1 1 x3 − x2k −1 
 2 2 2 k −1 2 k −1 2 k −1 2 
Tx T
x4
T
− x5k −1
T
x2 −
T
x3 1
T
x1k −1 
 2 7k −1 2 k −1 2 2 k −1 2 k −1 2 
 T T T T T T 
− x6k −1 x5 x4 x3 x2 − x1 1 
 2 2 k −1 2 k −1 2 k −1 2 k −1 2 k −1 

44
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

- Cálculo del vector de estado a priori, x̂k− , usando el modelo dinámico del
sistema.

xˆk− = f ( xˆk −1 ,0)

- Cálculo de la matriz Hk.

∂h[i ] −
H [i , j ] = ( xˆk ,0) ⇒
∂x[ j ]

1 0 0 0 0 0 0 
0 1 0 0 0 0 0 
 
0 0 1 0 0 0 0 
 − − 
0 0 0 − 2x 6k 2x 7k − 2 x4−k −
2 x5k 
0 0 0 2x −
5k 2x −
4k 2 x7−k 2 x6−k 
 − − 
0 0 0 2x 4k − 2x 5k − 2 x6−k −
2 x7k 
Hk =  2 x4k m1 + 2 x7−k m2A
− A
2 x5k m1A + 2 x6−k m2A
−
− 2 x6−k m1A + 2 x5−k m2A − −
− 2 x7k m1 + 2 x4k m2
A A
0 0 0 
 − 2 x6−k m3A + 2 x7−k m3A − 2 x4−k m3A + 2 x5−k m3A 
 A
 − 2 x7−k m1A + 2 x4−k m2A 2 x6−k m1A − 2 x5−k m2A 2 x5−k m1A + 2 x6−k m2A − A −
− 2 x4k m1 − 2 x7k m2 
0 0 0 
 + 2 x5−k m3A 2 x4−k m3A + 2 x7−k m3A + 2 x6−k m3A 
 2 x6−k m1A − 2 x5−k m2A 2 x7−k m1A − 2 x4−k m2A 2 x4−k m1A + 2 x7−k m2A 2 x5−k m1A + 2 x6−k m2A 
0 0 0 
 + 2 x4−k m3A − 2 x5−k m3A − 2 x6−k m3A + 2 x7−k m3A 

- Cálculo de la covarianza del vector de estado a priori, Pk-.

Pk− = Ak Pk −1 AkT + WQW T

- Cálculo de la ganancia de Kalman, Kk.

45
 Unidad de Medida Inercial. Algoritmo de Estimación e Implementación Software
Desarrollo teórico

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

r
- Actualización de x y de su matriz de covarianza.

r
xˆk = xˆk− + K k ( yk − h( xˆk− ,0))

Pk = ( I − K k H k ) Pk−

46