modelos matematicos de un acelerometro de navegacion inercial con giroscopio
-
Upload
aguilasblancas -
Category
Documents
-
view
220 -
download
0
Transcript of 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.
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
2/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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.
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
3/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
4/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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:
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
5/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
6/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
7/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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.
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
8/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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:
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
9/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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:
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
10/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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
+++=
+++=
+++=
+=
+=
=
=
=
=
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
11/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
12/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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:
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
13/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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
:
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
14/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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.
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
15/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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:
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
16/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
17/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
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.
-
7/30/2019 modelos matematicos de un acelerometro de navegacion inercial con giroscopio
18/18
Unidad de Medida Inercial. Algoritmo de Estimacin e Implementacin Software
Desarrollo terico
46
1)( += TTkkk
T
kkk VRVHPHHPK
- Actualizacin de xr y de su matriz de covarianza.
))0,((
+=kkkkk xhyKxx
r
=
kkkk PHKIP )(