LABORATRIO DE GUIAGEM, NAVEGAO E CONTROLE

RELATRIO 8 - DETERMINAO DE ATITUDE II

ANDRE MITSUO RAVAZZI RA: 11002511

GYSLLA DANIELLE BENTO DA SILVA RA: 11049811
VAGNER PASCUALINOTTO JUNIOR RA:11050907

Prof. Dr. Leandro Baroni

Santo Andr
2014

Sumrio

1. Introduo ............................................................................................................................................. 3
1.1 Determinao de atitude ................................................................................................................. 3
1.2 ngulos de Euler.............................................................................................................................. 5
1.3 Matriz de cossenos diretores ......................................................................................................... 7
1.4 Quaternions ...................................................................................................................................... 8
2. Metodologia e Resultados ................................................................................................................ 10
3. Concluses ......................................................................................................................................... 14
4. Bibliografia .......................................................................................................................................... 15
ANEXO A ................................................................................................................................................ 17

1. Introduo

A vida atual est repleta de atividades cotidianas que utilizam, de alguma forma, recursos
tecnolgicos espaciais. As telecomunicaes, a meteorologia, as pesquisas de recursos
naturais atravs de sensoriamento remoto, o monitoramento de atividades e sistemas terrestres
(agricultura, minerao, meio ambiente, etc.), o sistema de posicionamento global para
navegao de navios, avies e carros, todas essas atividades envolvem a utilizao de
satlites artificiais [SANTANA, 2008].

1.1 Determinao de atitude

A determinao da atitude o processo de computar a orientao do satlite em relao a

um sistema de referncia (inercial ou no-inercial). Como exemplo pode-se citar um sistema de
referncia a algum objeto de interesse, tal como a Terra. Isso normalmente envolve diversos
tipos de sensores em cada satlite e sofisticados procedimentos de processamento de dados
(WERTZ, 1978). O controle da atitude do satlite crucial para o adequado desempenho das
suas funes que podem ser sensoriamento remoto, meteorologia, comunicao, entre outras
aplicaes [WERTZ, 1978].
O problema da estabilizao do satlite est dividido em previso e controle da atitude. A
previso o processo de antever a orientao futura do satlite utilizando modelos dinmicos
do satlite estudado e conhecendo as foras perturbadoras que agem sobre o satlite. O
controle da atitude consiste em orientar ou manter o satlite em uma direo especfica
predeterminada [WERTZ, 1978, ARANTES JR, 2005]. Observa-se a importncia da
determinao da atitude uma vez que fornece a informao da atitude em relao ao sistema
de referncia considerado de acordo com o qual o movimento do satlite estabilizado
[SANTANA, 2008].
O controle de atitude pode ser visto por duas operaes: a operao de aquisio, que
consiste em levar o satlite para a atitude nominal a partir de uma atitude qualquer requerendo
manobras de grandes ngulos, e a operao de estabilizao, onde se deseja manter a atitude
de acordo com a atitude nominal, fazendo pequenas correes (manobras de pequenos
ngulos) quando necessrio [SANTANA, 2008].

A posio e a atitude de um satlite podem ser representadas de vrias formas, como o

referencial orbital e do corpo que sero discutidos a seguir. O referencial orbital (x ,y ,z ) tem
o

origem no centro de massa do satlite. O eixo z aponta para o centro da Terra, o eixo y
o

aponta na direo normal rbita. O eixo x encontrado usando a regra da mo direita,

o

completando o sistema [SANTANA, 2008].

O referencial do corpo, ou do satlite definido por (x,y,z) um sistema de coordenadas com
origem no centro de massa do satlite. Os eixos so escolhidos como sendo coincidentes com
os eixos principais de inrcia sendo x o eixo de roll, y o de pitch e z o de yaw definidos como
sendo nominalmente alinhados com x , y e z respectivamente [Wie, 1998, Moscati, 1992].
o

Figura 1.1 Sistemas de Referncia Inercial e do Satlite. [Arantes Jr, 2005]

1.2 ngulos de Euler

Transformaes entre sistemas de coordenadas sero muito importantes para resolver

muitos problemas. Posio e velocidade podem ser expressos em mais de um sistema de
coordenada. Ser adotada nesse trabalho a parametrizao por ngulos de Euler e uma breve
apresentao desta feita a seguir [SANTANA, 2008].
Quando se define a orientao de um corpo em relao a um sistema de referncia uma
srie de rotaes puras utilizada, resultando em transformaes ortogonais. As rotaes
associadas so chamadas de ngulos de Euler e elas determinam a orientao de um corpo.
Inicialmente considera-se que o referencial do corpo e o inercial coincidem [SANTANA, 2008].
1 Rotao em torno do eixo Z de um ngulo que leva ao sistema ,,.
2 Rotao em torno do eixo de um ngulo que leva ao sistema , , .
3 Rotao em torno do eixo de um ngulo que leva ao sistema x ,y ,z .
Os ngulos de rotao so os ngulos de Euler (, , ) e os eixos de rotao de Euler
so os eixos instantneos de rotao que nesse caso foram (Z, , ) e que totalizam 12
conjuntos de rotaes possveis: 6 simtricas (1-2-1; 1-3-1; 2-3-2; 2-1-2; 3-1-3; 3-2-3) e 6
assimtricas (1-2-3; 1-3-2; 2-3-1; 2-1-3; 3-1-2; 3-2-1).
Para essa seqncia de rotaes 3 1 3 a matriz de atitude dada por
(KAPLAN,1976)

Onde

a matriz de rotao do referencial x para o referencial X e apresenta uma

singularidade em

Figura 1.2 Construo dos ngulos de Euler. (Arantes Jr, 2005)

Em problemas de estabilizao em trs eixos define-se:

ngulo de roll

o ngulo de rotao em torno do eixo de roll

ngulo de pitch () o ngulo de rotao em torno do eixo de pitch

ngulo de yaw () o ngulo de rotao em torno do eixo de yaw

A figura 1.3 apresenta os trs eixos do problema de estabilidade do veiculo espacial.

Figura 1.3 Descrio de atitude.(S ELLERS, 2005)

Existem duas classes bsicas de sensores de atitude. A primeira classe faz medies
absolutas e a segunda faz medies relativas. Sensores de medio absoluta so baseados no
fato de que, conhecendo a posio do satlite em sua rbita, torna-se possvel calcular as
direes dos vetores, com respeito a um sistema inercial, de certos objetos astronmicos e das
linhas de fora do campo magntico da Terra. (HALL, 2006)

Sensores de medies absolutas medem estas direes com respeito ao sistema de

referncias fixo ao corpo do satlite, e comparando as medies com referncias conhecidas
no sistema de referncia inercial determinam a orientao relativa do sistema do corpo do
satlite com respeito ao sistema inercial. Esta classe de sensores usada em algoritmos de
determinao de atitude esttica. [CASTRO, 2006].

A propagao no tempo pode ser obtida atravs de:

1.3 Matriz de cossenos diretores

A matriz de cossenos diretores uma matriz 3x3, onde suas coluna representam o vetor unitrio nos
eixos do corpo projetados nos eixos referenciais. O elemento na i-sima linha e na j-sima coluna
representa o cosseno do ngulo entre o i eixo de referencia e o j eixo do corpo. A transformao do
corpo para os eixos referenciais dado por: [PERSA, 2006].

Para pequenos ngulos de rotao sen= e assim para os demais ngulos, ignorando
termos de segunda ordem, podemos reescrever a matriz de cossenos diretores em termos ds
ngulos de Euler conforme abaixo:

Com propagao no tempo do tipo

1.4 Quaternions

Quaternions so tambm chamados de parmetros simtricos de Euler, so baseados no

teorema de Euler para dados dois sistemas de coordenadas, h um eixo invariante, ao longo
do qual as medidas so das mesmas nos dois sistemas de coordenadas e possvel mover de
um sistema para outro atravs de apenas uma rotao em torno do eixo invariante. Os
quaternions possuem uma espcie de generalizao do espao tridimensional, do conceito de
nmeros complexos.
A transformao com apenas uma rotao sobre o vetor definido no sistema de referencia
pode ser observado abaixo:

Representao na fora matricial:

A relao entre os quaternions e os ngulos de Euler definida atravs da matriz abaixo

[SCHLEPPE, 1996]

Com propagao no tempo do tipo:

2. Metodologia e Resultados

O Giroscpio ser usado para propagao da atitude do IMU. Essa propagao envolve o
acompanhamento das orientaes de um ponto conhecido.
Atravs da medio da mudana de orientao possvel o conhecimento da atitude
propagada.
O giroscpio contido no IMU tem como sada o valor de taxa de variao angular que deve ser
integrado para determinar a orientao do IMU.
Para implementao dos mtodos, inicialmente foi obtido o grfico dos valores de gyr , que
contm as medidas do giroscpio coletadas do IMU.

regio de repouso IMU

Figura 2 - Medidas obtidas do giroscpio armazenados no vetor gyr

Atravs do grfico da figura 2 possvel observar que as medidas possuem um offset ou bias
na condio que deveria ser "zero" pois o IMU estava em repouso. Logo a calibrao do sensor
foi necessria para corrigir o bias da ordem de 7,3.
Para calibrao foram calculadas as mdias dos valores de gyr para o giroscpio para a
condio de repouso consiste nas posies entre 0 a ~800.

10

O grfico com os valores corrigidos pode ser observado na figura 3

Figura 3 - Valores calibrados do giroscpio

A figura 4 apresenta os valores calibrados normalizados

Figura 4 - Valores calibrados normalizados do giroscpio

11

Aps a primeira etapa de calibrao foram implementados as 3 formas de propagao de

atitude conforme descrito no capitulo anterior.
A primeira implementao utilizou os ngulos de Euler como forma de propagao.
Um contador foi definido para gerar o vetor de tempo responsvel tambm pelo clculo do vetor
de velocidade angular w . A condio inicial para os ngulos de Euler foram retiradas dos
vetores phi_imu, psi_imu e theta_imu , que estavam disponveis no conjunto de dados
extrados do IMU. Essas condies inicias definidas, o mtodo de ngulos de Euler foi utilizado
para o integrao das taxas de variao angular propagados no tempo, retornando os ngulos
de roll, pitch e yaw, armazenados em euler_E
Para o mtodo MCD, foi implementada a matriz de cossenos diretores de acordo com as
condies iniciais dos ngulos de Euler que em conjunto com o vetor de tempo dt e velocidade
angular w, formam as condies iniciais para a integrao gerando a matriz e cossenos
diretores propagadas. Essa condio apresenta a forma de matriz, a funo do Matlab
dcm2angle foi implementada para obteno dos ngulos de Euler que foram armazenados em
euler_M.
O mtodos de quaternions, foi implementado atravs vetor de quaternions q1,q2,q3 e q4
utilizando as condies iniciais dos ngulos de Euler, os vetores q , dt e w, foram utilizados
para integrao das condies propagadas. A funo do Matlab quat2angle foi implementada
para obteno dos ngulos de Euler que foram armazenados em euler_Q.
Abaixo temos os resultados das propagaes para cada um dos mtodos para uma dada
trajetria.

12

-Representao de atitude por ngulos de Euler

Figura 5 - Representao de atitude por ngulos de Euler

- Representao de atitude pelo mtodo da matriz de cossenos diretores

Figura 6 - Representao de atitude pelo mtodo da matriz de cossenos diretores

13

- Representao de atitude pelo mtodo dos quaternions

Figura 7 - Representao de atitude pelo mtodo dos quaternions

3. Concluses
O comparativo dos resultados mostra que a vantagem da representao de atitude atravs dos
ngulos de Euler est em utilizar apenas trs parmetros. Por outro lado, esta representao
tem a desvantagem da ocorrncia de singularidades na modelagem matemtica do movimento
do IMU. A ocorrncia de singularidades pode ser observada para s 3 ngulos. (eixos
principais). Nota-se uma distoro do perfil da trajetria prximo as regies singulares.
A representao atravs da matriz de cossenos diretores mostra que nas regies transientes
h uma distoro na representao da trajetria. Variaes rpidas na velocidade angular
carregam erros composio da propagao da trajetria. Para visualizao dos ngulos de
Euler devem ser extrados do mtodo.
A representao por quaternions foi a mais complexa na implementao dado seu carter mais
abstrato, porm no apresentou singularidades como na representao pelos ngulos de Euler
e apresentou a melhor resposta nas regies transientes.

14

A vantagem com relao ao algoritmo TRIAD que para as 3 representaes desenvolvidas

no houve a necessidade de um sistema inercial fora do corpo. Somente com as medies
feitas pelo giroscpio solidrio ao IMU foi possvel a representao da trajetria.

4. Bibliografia

[SANTANA, 2008]. Santana, A. C. . Controle de Atitude de Satlites Artificiais. Monografia

de graduao em Enegenharia de Controle e Automao. Universidade Federal de Ouro Preto
(UFOP). 2008.

[ARANTES JR., 2005]. Arantes Jr., G. . Estudo Comparativo de Tcnicas de Controle de

Atitude em 3 Eixos para Satlites Artificiais. 2005. 187 f. Dissertao (Mestrado em
Engenharia a Tecnologia Espacias / Mecnica Espacial e Controle), Instituto Nacional de
Pesquisas Espaciais, So Jos dos Campos, So Paulo, Brasil, 2005.
[CASTRO, 2006]. Castro, J. C. V. . Desenvolvimento de um Dispositivo para Determinao
de Atitude de Satlites Artificiais, Baseados em Magnetmetro de Estado Slido.
Monografia de Graduao em Engenharia de Controle e Automao. Universidade Federal de
Ouro Preto (UFOP). 2006.

[HALL, 2006]. Hall, C. D, Spacecraft attitude dynamics and control: Lectures notes posted
on Handouts page. Disponvel em: <http://www.aoe.vt.edu/~cdhall/courses/aoe4140>.
Acessado em: 29/04/2006.
[MATLAB]. MATLAB - Manual e ajuda do programa (HELP)
[MOSCATI, 1992]. Moscati, N. R. . Projeto de um Sistema de Controle de Atitude (trs eixos)
de Satlites Utilizando a Metodologia LQR/LTR. 1992. Dissertao (Mestrado em Cincia
Espacial) INPE-5473-TDI/504, INPE, So Jos dos Campos, Brasil, 1992.

[SELLERS, 2005]. SELLERS , Jerry Jon. Understanding Space: An Introduction to

Astronautics (Third Edition). McGraw-Hill, 2005. 642 p.

15

[SCHLEPPE, 1996]. SCHLEPPE , J. B.. Development of a Real-Time Attitude System

Using a Quaternion Parameterization and Non-Dedicated GPS Receivers, CALGARY,
ALBERTA, July, 1996.
[PERSA, 2006].PERSA, S.F. Sensor Fusion in Head Pose Tracking for Augmented Reality,
2006
[WERTZ, 1978]. WERTZ, J. R., Spacecraft Attitude Determination and Control. London, D.
Reidel, 1978.
[WIE, 1989]. Wie, H. W. B. ; Arapostathis, A. . Quaternion Feedback Regulator for Spacedraft
Eigenaxis Rotations. Journal of Guidance Control and Dynamics, Vol. 12, No. 3, p. 375-380,
1989.
[BARONI, 2014], Baroni, L., Notas de aulas da disciplina Laboratrio de guagem, navegao e
controle - Determinao de atitude II (pdf). UFABC, 2014.

16

ANEXO A
atitude.m clear all
clc
% Abre o arquivo de dados para leitura
arq = fopen('Data_2014-07-24.txt', 'r');
% Formato:
% tempo - temperatura - altitude - taxa de subida - acc(3) - mag(3) - ...
% gyr(3) - acc_cal(3) - mag_cal(3) - gyr_cal(3) - phi_imu - theta_imu psi_imu
% L o arquivo e armazena os dados na clula dados{:}
dados = textscan(arq,
'%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f');
% Organiza os dados
tempo = dados{1};
temperatura = dados{2};
altitude = dados{3};
taxa_de_subida = dados{4};
acc = [dados{5} dados{6} dados{7}] * (9.81 / 819);
% [m/s^2]
mag = [dados{8} -dados{9} -dados{10}] * 0.001;
% [gauss]
gyr = [dados{11} dados{12} dados{13}] * 0.003550881453439; % [rad/s]
acc_cal = [dados{14} dados{15} dados{16}];
mag_cal = [dados{17} dados{18} dados{19}];
gyr_cal = [dados{20} dados{21} dados{22}];
phi_imu = dados{23};
theta_imu = dados{24};
psi_imu = dados{25};
% Calibrao dos giroscpios
Mediagyr1 = mean (gyr(1:500,1)); %clculo Mdia enquanto o giroscopio est em
repouso (ver grafico plot (gyr))
Mediagyr2 = mean (gyr(1:500,2));
Mediagyr3 = mean (gyr(1:500,3));
%
%
%
%
%
%

figure
plot (gyr);
title('Medidas giroscpio')
xlabel('pos vetor')
ylabel('rad/s')
hold on

subplot(311);
plot(phi_imu);
xlabel('Medidas de phi (rad)')
ylabel('pos vetor')
subplot(312);
plot(theta_imu);
xlabel('Medidas de theta (rad)')
ylabel('pos vetor');

17

subplot(313);
plot(psi_imu);
xlabel('Medidas de psi (rad)')
ylabel('pos vetor')
hold on

% ---------------Giroscpio-----------------------------------------for i = 1 : length(gyr)
gyr_ref1(:, i) = (gyr(i, 1)' - Mediagyr1);
% Mtodo do campo de
referncia
G1_ref(i) = norm(gyr_ref1(:, i)); % norma do vetor
end
gyr_ref1 = gyr_ref1';
G1_ref=G1_ref';
for i = 1 : length(gyr)
gyr_ref2(:, i) = (gyr(i, 2)' - Mediagyr2);
% Mtodo do campo de
referncia
G2_ref(i) = norm(gyr_ref2(:, i)); % norma do vetor
end
gyr_ref2 = gyr_ref2';
G2_ref=G2_ref';
for i = 1 : length(gyr)
gyr_ref3(:, i) = (gyr(i, 3)' - Mediagyr3);
% Mtodo do campo de
referncia
G3_ref(i) = norm(gyr_ref3(:, i)); % norma do vetor
end
gyr_ref3 = gyr_ref3';
G3_ref=G3_ref';
%Plotar valores calibrados(BIAS)
gyr_ref = [gyr_ref1 gyr_ref2 gyr_ref3]
gyr_ref2 = [G1_ref G2_ref G3_ref]
%
%
%
%
%
%
%
%
%
%
%
%
%

figure
plot (gyr_ref2)
title('Valores calibrados normalizados - Giroscpio')
xlabel('pos vetor')
ylabel('-')
%hold on
figure
plot (gyr_ref)
title('Valores calibrados - Giroscpio')
xlabel('pos vetor')
ylabel('rad/s')
hold on

% pr alocao de memria
euler_E = zeros(length(gyr), 3); euler_C = euler_E; euler_Q = euler_E;

18

% Valores iniciais para euler_E, euler_C, euler_Q

% OBS: O MATLAB define as funes:
% euler2dcm
% euler2quat
% Use o comando 'help euler2dcm' para mais detalhes.
for i = 2 : length(gyr)
dt = tempo(i) - tempo(i - 1); % intervalo de tempo entre as medidas
% Propagao dos ngulos de Euler
euler=[phi_imu(1,1) theta_imu(1,1) psi_imu(1,1)] ; % Valores iniciais
para integracao
w= [(phi_imu(i) - phi_imu(i-1)/dt) (theta_imu(i) - theta_imu(i-1)/dt)
(psi_imu(i) - psi_imu(i-1)/dt)]
%

euler_E(i,:,:) = IntegraEuler(dt,euler,w)

% Propagao da matriz de cossenos diretores

C1=[cos(theta_imu(1,1))*cos(psi_imu(1,1)) cos(phi_imu(1,1))*sin(psi_imu(1,1))+sin(phi_imu(1,1))*sin(theta_imu(1,1))*cos
(psi_imu(1,1))
sin(phi_imu(1,1))*sin(psi_imu(1,1))+cos(phi_imu(1,1))*sin(theta_imu(1,1))*cos
(psi_imu(1,1))]
C2=[cos(theta_imu(1,1))*sin(psi_imu(1,1))
cos(phi_imu(1,1))*cos(psi_imu(1,1))+sin(phi_imu(1,1))*sin(theta_imu(1,1))*sin
(psi_imu(1,1)) sin(phi_imu(1,1))*cos(psi_imu(1,1))+cos(phi_imu(1,1))*sin(theta_imu(1,1))*sin
(psi_imu(1,1))]
C3=[-sin(theta_imu(1,1)) sin(phi_imu(1,1))*cos(theta_imu(1,1))
cos(phi_imu(1,1))*cos(theta_imu(1,1))]
C=[C1;C2;C3]
dcm = IntegraMCD (dt, C, w)
euler_C(i,:,:) = dcm2angle(dcm)
%
% Propagao dos quaternions
q1=[cos(psi_imu(1,1)/2)*cos(phi_imu(1,1)/2)*sin(theta_imu(1,1)/2)sin(psi_imu(1,1)/2)*sin(phi_imu(1,1)/2)*cos(theta_imu(1,1)/2)]
q2=[sin(psi_imu(1,1)/2)*cos(phi_imu(1,1)/2)*sin(theta_imu(1,1)/2)+cos(psi_imu
(1,1)/2)*sin(phi_imu(1,1)/2)*cos(theta_imu(1,1)/2)]
q3=[sin(psi_imu(1,1)/2)*cos(phi_imu(1,1)/2)*cos(theta_imu(1,1)/2)+cos(psi_imu
(1,1)/2)*sin(phi_imu(1,1)/2)*sin(theta_imu(1,1)/2)]

19

q4=[cos(psi_imu(1,1)/2)*cos(phi_imu(1,1)/2)*cos(theta_imu(1,1)/2)sin(psi_imu(1,1)/2)*sin(phi_imu(1,1)/2)*sin(theta_imu(1,1)/2)]
q_=[q1;q2;q3;q4]
q=q_'
quat = IntegraQ ( dt, q, w )
euler_Q(i,:,:) = quat2angle(quat)
end
%Plote os grficos
% Mtodo Euler
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%

subplot(311);
plot(euler_E(:, 1));
title('ngulos de Euler')
ylabel('phi (rad)')
xlabel('tempo (s)')
hold on
subplot(312);
plot(euler_E(:, 2));
ylabel('theta (rad)')
xlabel('tempo (s)');
hold on
subplot(313);
plot(euler_E(:, 3));
ylabel('psi (rad)')
xlabel('tempo (s)')
hold on

% Mtodo MCD
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%

subplot(311);
plot(euler_C(:, 1));
title('Matriz de cossenos diretores')
ylabel('phi (rad)')
xlabel('tempo (s)')
hold on
subplot(312);
plot(euler_C(:, 2));
ylabel('theta (rad)')
xlabel('tempo (s)');
hold on
subplot(313);
plot(euler_C(:, 3));
ylabel('psi (rad)')
xlabel('tempo (s)')
hold on

20

% Mtodo Quaternions
subplot(311);
plot(euler_Q(:, 1));
title('Quaternions')
ylabel('phi (rad)')
xlabel('tempo (s)')
subplot(312);
plot(euler_Q(:, 2));
ylabel('theta (rad)')
xlabel('tempo (s)');
subplot(313);
plot(euler_Q(:, 3));
ylabel('psi (rad)')
xlabel('tempo (s)')
hold on
fclose all;
----------------------------------------------------------------------------IntegraEuler.m
function [ novo_euler ] = IntegraEuler( dt, euler, w )
% dt - intervalo de tempo entre as medidas, em s
% euler - vetor com os ngulos de Euler [roll pitch yaw], em rad
% w - vetor com as medidas de velocidade angular, em rad/s
eulerp(1) = (w(2) * sin(euler(1)) + w(3) * cos(euler(1))) * tan(euler(2)) +
w(1);
eulerp(2) = w(2) * cos(euler(1)) - w(3) * sin(euler(1));
eulerp(3) = (w(2) * sin(euler(1)) + w(3) * cos(euler(1))) / cos(euler(2));
novo_euler = euler + eulerp * dt; % ngulos de euler propagado
end
----------------------------------------------------------------------------IntegraMCD.m
function [ nova_C ] = IntegraMCD (dt, C, w)
% dt - intervalo de tempo entre as medidas, em s
% C - matriz de cossenos diretores
% w - vetor com as medidas de velocidade angular, em rad/s
W = [

0 -w(3) w(2) ;
w(3)
0 -w(1) ;
-w(2) w(1)
0];

Cp = C * W;
nova_C = C + Cp * dt; % matriz de cossenos diretores propagada

21

----------------------------------------------------------------------------IntegraQ.m
function [ novo_q ] = IntegraQ ( dt, q, w )
% dt - intervalo de tempo entre as medidas, em s
% q - vetor com quaternions
% w - vetor com as medidas de velocidade angular, em rad/s
Q = [q(1) -q(2) -q(3) -q(4) ;
q(2) q(1) -q(4) q(3) ;
q(3) q(4) q(1) -q(2) ;
q(4) -q(3) q(2) q(1)];
W = [0 w]';
qp = 0.5 * Q * W;
novo_q = q + qp' * dt; % quaternions propagados

22