modelos matematicos de un acelerometro de navegacion inercial con giroscopio

7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio

1/18

Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software

Desarrollo terico

29

Captulo 2. Desarrollo terico

2.1. Representacin matemtica de la orientacinDado 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.


2/18


Desarrollo terico

30

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 DirectionCosine 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 marcocoordinado 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 unanica 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.

De esta forma, siendo ABR la matriz de rotacin del objeto respecto al sistema

fijo, y Arr

y Brr

vectores expresados en el sistema fijo y del objeto, respectivamente,

se tiene:

;BABA rRr

rr= ATAB

B rRrrr

)(=

Los elementos de la matriz de rotacin son

A

BR =

333231

232221

131211

rrr

rrr

rrr

siendoij

r el coseno del ngulo entre el eje i del sistema de referencia fijo y el

ejej del sistema de referencia del objeto.


3/18


Desarrollo terico

31

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 expresarcon 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 deA

X un ngulo (roll), despus alrededor de AY un ngulo

(pitch) y finalmente alrededor de AZ 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 BZ , luego alrededor del BY

resultante del primer giro, y

posteriormente se rota un ngulo alrededor del eje BX .

Los ngulos de Euler Z-Y-Z se obtienen de realizar rotaciones parecidas a lasanteriores. 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

vector r definido en el sistema de referencia fijo. El cuaternin, qr , es un vector de

cuatro elementos que son funcin de este vector y de la magnitud de la rotacin:

=

=

)2/()/(

)2/()/(

)2/()/(

)2/cos(

sen

sen

sen

d

c

b

a

q

z

y

xr


4/18


Desarrollo terico

32

donde x, x y x son las componentes del vector r

y el mdulo de dicho

vector. El parmetro representa el valor de la rotacin sobre el vector r

.

2.1.4. Relaciones entre matriz de rotacin, ngulos RPY ycuaterniones

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 restodel 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:

++

++

++

=

=

)()(2)(2

)(2)()(2

)(2)(2)(

2222

2222

2222

333231

232221

131211

dcbaabcdacbd

abcddcbaadbc

acbdadbcdcba

rrr

rrr

rrr

RA

B

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

matriz:


5/18


Desarrollo terico

33

= ),(2 3332 rrarctg

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, )2,2(2 arctg = -135).

= ),(2 2212

1131 rrrarctg +

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

positiva.

= ),(2 1121 rrarctg

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, = ),(2 2212 rrarctg

= 0, = -90, = - ),(2 2212 rrarctg

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:

=

=

z

y

x

abcd

badc

cdab

dcba

d

c

b

a

q

0

5.0

&

&

&

&

&r


6/18


Desarrollo terico

34

O, lo que es lo mismo,

q

d

c

b

a

d

c

b

a

q

xyz

xzy

yzx

zyx

xyz

xzy

yzx

zyx

r

&

&

&

&

&r

=

=

=

0

0

0

0

5.0

0

0

0

0

5.0

2.2. Modelo del sistema

Con la informacin anterior, se puede fcilmente desarrollar un modelo

dinmico del sistema en descripcin interna:

xAxr&r =

donde xr

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:

xx

xyz

xzy

yzx

zyx

r&r

=

0

0

0

0

5.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:

r&r

=

/100

0/10

00/1


7/18


Desarrollo terico

35

siendo

=

z

y

x

r

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

dinmicas en el modelo. El resultado es el siguiente:

=

=

d

c

b

a

x

x

x

x

x

x

x

x

z

y

x

7

6

5

4

3

2

1

r

=

=

d

c

b

a

d

c

b

ax

z

y

x

xyz

xzy

yzx

zyx

z

y

x

0

0

0

0

0

0

/200

0/20

00/2

5.0

&

&

&

&

&

&

&

&r

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.


8/18


Desarrollo terico

36

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

E

x

yz

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 elmarco de referencia que se acaba de exponer:

nTm

smg

=

=

7885,33

3929,1

8503,26

/

82,9

0

02

r

r

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:


9/18


Desarrollo terico

37

=

=

78249,0

03226,0

62182,0

1

0

0

A

A

m

g

r

r

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:

=

=

B

B

B

B

B

B

z

y

x

m

m

m

g

g

g

y

y

y

y

y

y

y

y

y

y

3

2

1

3

2

1

9

8

7

6

5

4

3

2

1

r, siendo

=

B

B

B

B

g

g

g

g

3

2

1r

y

=

B

B

B

B

m

m

m

m

3

2

1r

Los vectores Bgr

y Bmr

sern vectores normalizados, para poder relacionarlos

con los vectores de referencia.

Como se ha visto en el apartado 2.1.1, la relacin entre los vectores gr

y mr

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

BA

B

A gRgrr

= ; ATAB

B gRgrr

)(=

BA

B

A mRmrr

= ; ATABB mRm

rr)(=

Y la matrizR se puede expresar en funcin de los cuaterniones, de forma que

queda:


10/18


Desarrollo terico

38

ABg

dcbaabcdacbd

abcddcbaadbc

acbdadbcdcba

grr

++

++

++

=

)()(2)(2

)(2)()(2

)(2)(2)(

2222

2222

2222

ABm

dcbaabcdacbd

abcddcbaadbc

acbdadbcdcba

mrr

++

++

++

=

)()(2)(2

)(2)()(2

)(2)(2)(

2222

2222

2222

Ntese que a, b, c y dson 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:

2/)(

2/)(

2/)(

2/)(

)/1(

)/1(

)/1(

6152437

7153426

7263415

7362514

33

22

11

xxxxxxx

xxxxxxx

xxxxxxx

xxxxxxx

xx

xx

xx

+=

+=

+=

=

=

=

=

&

&

&

&

&

&

&

78249,0)(03226,0)(262182,0)(2

78249,0)(203226,0)(62182,0)(2

78249,0)(203226,0)(262182,0)(

)(2

)(2

2

7

2

6

2

5

2

4547664759

5476

2

7

2

6

2

5

2

474658

64757465

2

7

2

6

2

5

2

47

2

7

2

6

2

5

2

46

54765

64754

33

22

11

xxxxxxxxxxxxy

xxxxxxxxxxxxy

xxxxxxxxxxxxy

xxxxy

xxxxy

xxxxy

xy

xy

xy

+++=

+++=

+++=

+=

+=

=

=

=

=


11/18


Desarrollo terico

39

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

El filtro de Kalman trata de estimar el estado nx r

de un proceso en tiempo

discreto gobernado por la ecuacin en diferencias lineal estocstica

111 ++= kkkk wuBxAxrrrr

con una medida my r

que es

kkk vxHyrrr

+=

Las variables aleatorias kwr

y kvr

representan el ruido en el proceso y en la

medida, respectivamente. Se suponen independientes, blancos y con una distribucin

normal dada por

),0()(

),0()(

RNvp

QNwp

r

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 tambinla matriz de covarianza del error del estado, Pk. Esta matriz si evolucionar a lo

largo de las sucesivas iteraciones.

T

kkkkk xxxxEP ))(( =

siendo kx 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


12/18


Desarrollo terico

40

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:

11

+=

kkk uBxAxr

QAAPPT

kk+=

1

=

+=

+=

kkk

kkkkk

T

k

T

kk

PHKIP

xHyKxx

RHHPHPK

)(

)(

)(1

r

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

),,( 111 = kkkk wuxfxrrrr

),( kkk vxhyrrr

=

Se definen las matrices jacobianas:

[ ][ ]

[ ]

[ ][ ]

[ ]

[ ][ ]

[ ]

[ ][ ]

[ ]

)0,(

)0,(

)0,,(

)0,,(

,

,

11,

11,

=

=

=

=

k

j

i

ji

k

j

i

ji

kk

j

i

ji

kk

j

i

ji

xv

hV

xx

hH

uxw

fW

uxx

fA

r

r

Y ahora el algoritmo queda como sigue:


13/18


Desarrollo terico

41

)0,,( 11 =

kkk uxfxr

T

kkk

T

kkkk WQWAPAP 11

+=

=

+=

+=

kkkk

kkkkk

T

kkk

T

kkk

T

kkk

PHKIP

xhyKxx

VRVHPHHPK

)(

))0,((

)(1

r

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:

Txx

txfx kk

= +1)(&

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 elDSP 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:

Ecuacin dinmica ),( 11 = kkk wxfxrrr

:


14/18


Desarrollo terico

42

11111111

11111111

11111111

11111111

111

111

111

761524377

671534266

572634155

473625144

3333

2222

1111

2/)(

2/)(

2/)(

2/)(

)/(

)/(

)/(

+++=

+++=

+++=

++=

++=

++=

++=

kkkkkkkkk

kkkkkkkkk

kkkkkkkkk

kkkkkkkkk

kkkk

kkkk

kkkk

TwxxxxxxTxx

TwxxxxxxTxx

TwxxxxxxTxx

TwxxxxxxTxx

TwxTxx

TwxTxx

TwxTxx

Ecuacin de medida ),( kkk vxhyrrr

= :

kkkkkkkkkkkkkk

kkkkkkkkkkkkkk

kkkkkkkkkkkkkk

kkkkkk

kkkkkk

kkkkkk

kkk

kkk

kkk

vxxxxxxxxxxxxy

vxxxxxxxxxxxxy

vxxxxxxxxxxxxy

vxxxxy

vxxxxy

vxxxxy

vxy

vxy

vxy

9

2

7

2

6

2

5

2

4547664759

85476

2

7

2

6

2

5

2

474658

764757465

2

7

2

6

2

5

2

47

6

2

7

2

6

2

5

2

46

554765

464754

333

222

111

78249,0)(03226,0)(262182,0)(2

78249,0)(203226,0)(62182,0)(2

78249,0)(203226,0)(262182,0)(

)(2

)(2

++++=

++++=

++++=

++=

++=

+=

+=

+=

+=

Las covarianzas de los ruidos kwr

y kvr

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 es0.1 Hzs// . 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 en0.225 Hzmg/ . Para nuestro ancho de banda de 100 Hz queda una

desviacin tpica de 2.25 mg.


15/18


Desarrollo terico

43

- 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 Wy V,

ya que son constantes y no har falta tener que definirlas en cada iteracin.

[ ][ ]

[ ]

[ ][ ]

[ ]99,

771,

)0,(

)0,(

xk

j

i

ji

xk

j

i

ji

IVxv

hV

ITWxw

fW

=

=

=

=

Se puede definir tambin las matrices de covarianza del ruido en el proceso y

en la medida, Q yR. 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:

=

01.0000000

001.000000

0001.00000

00001.0000

00001.000

000001.00

0000001.0

Q

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:


16/18


Desarrollo terico

44

=

2

2

2

2

2

2

2

2

2

00000000

00000000

00000000

00000000

00000000

00000000

00000000

00000000

00000000

s

s

s

r

r

r

v

v

v

R

Finalmente, se procede a comenzar el bucle. Para cada iteracin kse hace lo

siguiente:

- Actualizacin de las medidas: se actualiza el vectorky

rcon las medidas de

girscopos, acelermetros y compases magnticos.

- Clculo de la matriz Ak.

[ ][ ]

[ ]

=

)0,( 1, kj

i

ji xxfA

+

+

+

=

1222222

21

22222

221

2222

2221

222

0000100

0000010

0000001

111111

111111

111111

111111

123456

132547

231674

321765

kkkkkk

kkkkkk

kkkkkk

kkkkkk

xT

xT

xT

xT

xT

xT

xT

xT

xT

xT

xT

xT

xT

xT

xT

xT

xT

xT

xTxTxTxTxTxT

T

T

T

Ak


17/18


Desarrollo terico

45

- Clculo del vector de estado a priori, kx , usando el modelo dinmico delsistema.

)0,( 1=

kk xfx

- Clculo de la matrizHk.

[ ]

[ ]

[ ]

=

)0,

(, kj

i

ji xx

h

H

+

+

+

+

+

+

+

+

+

+

+

+

+

+

+

=

A

AA

A

AA

A

AA

A

AA

A

AA

A

AA

A

AA

A

AA

A

AA

A

AA

A

AA

A

AA

k

mx

mxmx

mx

mxmx

mx

mxmx

mx

mxmx

mx

mxmx

mx

mxmx

mx

mxmx

mx

mxmx

mx

mxmx

mx

mxmx

mx

mxmx

mx

mxmx

xxxx

xxxx

xxxx

H

k

kk

k

kk

k

kk

k

kk

k

kk

k

kk

k

kk

k

kk

k

kk

k

kk

k

kk

k

kk

kkkk

kkkk

kkkk

37

2615

36

2714

35

2417

34

2516

36

2714

37

2615

34

2516

35

2417

35

2417

34

2516

37

2615

36

2714

7654

6745

5476

2

22

2

22

2

22

2

22000

2

22

2

22

2

22

2

22000

222

222

222

222000

2222000

2222000

2222000

0000100

0000010

0000001

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

TT

kkkk WQWAPAP +=

1

- Clculo de la ganancia de Kalman, Kk.


18/18


Desarrollo terico

46

1)( += TTkkk

T

kkk VRVHPHHPK

- Actualizacin de xr y de su matriz de covarianza.

))0,((

+=kkkkk xhyKxx

r

=

kkkk PHKIP )(

modelos matematicos de un acelerometro de navegacion inercial con giroscopio

Documents

Transcript of modelos matematicos de un acelerometro de navegacion inercial con giroscopio