Estou tentando implementar um sistema de navegação inercial usando umFiltro indireto de Kalman. Encontrei muitas publicações e teses sobre esse tópico, mas não muito código como exemplo. Para minha implementação, estou usando a Dissertação de Mestrado disponível no seguinte link:

https://fenix.tecnico.ulisboa.pt/downloadFile/395137332405/dissertacao.pdf

Conforme relatado na página 47, os valores medidos dos sensores inerciais são iguais aos valores reais mais uma série de outros termos (viés, fatores de escala, ...). Para minha pergunta, vamos considerar apenas o viés.

Assim:

Wmeas = Wtrue + BiasW   (Gyro meas)
Ameas = Atrue + BiasA.  (Accelerometer meas)

Portanto,

quando propagar as equações de mecanização (equações 3-29, 3-37 e 3-41), devo usar os valores "true", ou melhor:

Wmeas - BiasW
Ameas - BiasA

onde BiasW e BiasA são a última estimativa disponível do viés. Direito?

Em relação à fase de atualização do EKF, se a equação de medição for

dzV = VelGPS_est - VelGPS_meas

a matriz H deve ter uma matriz de identidade em concordância com as variáveis de estado de erro de velocidade dx (VEL) e 0 em outro lugar. Direito?

Disse que não tenho certeza de como tenho que propagar a variável state após a fase de atualização. A propagação da variável state deve ser (na minha opinião):

POSk|k = POSk|k-1 + dx(POS);
VELk|k = VELk|k-1 + dx(VEL);
...

Mas isso não funcionou. Portanto, eu tentei:

POSk|k = POSk|k-1 - dx(POS);
VELk|k = VELk|k-1 - dx(VEL);

isso também não funcionou ... Tentei as duas soluções, mesmo que, na minha opinião, o "+" deva ser usado. Mas como os dois não funcionam (tenho outro erro em outro lugar), pergunto se você tem alguma sugestão.

Você pode ver um trecho de código no seguinte link:http://pastebin.com/aGhKh2ck.

Obrigado.