Implementação do Filtro de Kalman Estendido

Transcrição

1 UNIVERSIDADE FEDERAL DA GRANDE DOURADOS UFGD FACULDADE DE CIÊNCIAS EXATAS E TECNOLOGIA FACET DISCIPLINA: Tópicos em Robótica Móvel PROFESSORA: Drª Valguima Odakura ACADÊMICOS: Aline Izida e Diogo Fernando Trevisan Implementação do Filtro de Kalman Estendido DOURADOS/MS 9/5/21 1

2 SUMÁRIO SUMÁRIO...2 INTRODUÇÃO...3 MODELO DE MOVIMENTO ODOMETRIA E DIREÇÃO SÍNCRONA...3 MODELO DE OBSERVAÇÃO - LASER...4 FILTRO DE KALMAN ESTENDIDO...4 FASES DO FKE...5 PREDIÇÃO...5 EMPARELHAMENTO...6 ATUALIZAÇÃO...7 AMBIENTES UTILIZADOS...8 EXPERIMENTOS...8 RESULTADOS...1 REFERÊNCIAS BIBLIOGRÁFICAS...18 ANEXO

3 Introdução Para resolver o problema de localização de robôs móveis é preciso estimar a posição e a orientação do robô, considerando as coordenadas dado um mapa de um ambiente. A postura do robô é representada por (x, y, Ѳ), onde x e y representam um ponto no qual o robô se encontra no mapa e Ѳ representa a orientação do robô. Para se localizar, o robô precisa utilizar de sensores internos e externos, respectivamente, para captar informações do ambiente. Porém, as informações dos sensores externos têm ruídos e são imprecisas, além de medirem apenas parte do ambiente. As informações do sensor interno também são imprecisas e ruidosas. Sendo assim, é necessário combinar essas informações vindas dos sensores para se obter a localização mais precisa do robô. No entanto, sabendo-se da imprecisão dos sensores, é necessária a utilização de um filtro que considere as incertezas associadas aos modelos de movimento e de observação para dar uma melhor estimativa da posição em que o robô, de fato, está localizado. Para isso, neste trabalho utilizou-se o filtro de Kalman [1], [2] e [3]. Modelo de Movimento Odometria e Direção Síncrona Através da odometria e da direção síncrona, é possível observar o movimento das rodas para determinação da localização do robô. A odometria utiliza de instrumentos que medem a distância percorrida a cada movimento, os odômetros, que por sua vez utilizam sensores, geralmente encoders localizados no eixo das rodas [4]. A direção síncrona trabalha com três rodas, conforme ilustra a Figura 1, de forma que o robô possa caminhar reto ou girar em torno de seu próprio eixo. As rodas trabalham todas juntas, viradas para a mesma direção e andando todas na mesma proporção, seja pra caminhar reto ou girar, sendo que quem controla essas proporções é o robô, além de poder também controlar sua orientação [5]. 3

4 Figura 1: Direção síncrona. O sistema de odometria acumula muitos erros ao longo do caminho percorrido pelo robô, adquiridos por problemas como solo escorregadio ou irregular, acelerações e rotações rápidas. Por isso, a odometria atua juntamente com o FKE para a correção destes erros a cada iteração do filtro [4]. Modelo de Observação - Laser Considerando que o robô conhece um mapa do ambiente, os sensores externos podem ser utilizados na localização do robô em um dado ambiente, uma vez que, através deles é possível localizar marcos, os quais o robô detecta e pode ter uma referência do lugar em que está e então estimar sua postura. O sensor utilizado é o laser, que é capaz de medir a distância até os marcos caracterizados como pontos, dado um mapa. Embora a observação através do laser tenha menos erros que a por sonar, ainda sim, como o modelo de movimento, também tem erro associado. Sendo assim, o FKE também considera esses erros para a correção das leituras a cada iteração do filtro, a fim de obter uma estimativa com a menor probabilidade de erro possível [6] e [8]. Filtro de Kalman Estendido Inicialmente, algumas informações são necessárias para a estimação do estado do robô por filtro de Kalman, ou seja, utiliza-se a postura inicial do robô, a incerteza inicial e o conhecimento sobre os modelos de movimento e de observação. Sabe-se que para resolver o problema de localização em robótica móvel necessita-se de um movimento não linear de mudança de postura e que a relação entre o estado do sistema e 4

5 as medidas são não lineares. Uma vez que o filtro de Kalman foi desenvolvido para sistemas lineares, para o problema de localização de robôs este não se aplica. No entanto, pode-se utilizar a sua versão estendida, ou seja, o filtro de Kalman adaptado a um modelo não linear do sistema o Filtro de Kalman Estendido (FKE) [1] e [2]. Fases do FKE Predição Dadas as entradas de controle u(, que são obtidas através da odometria e X(, que é o vetor de estado do robô no tempo k, pode-se calcular o x estimado, ou seja, a posição estimada do robô no tempo k [2]: x( + D( * cos[ θ ( ] xˆ ( k + 1 = y( + D( * sin[ θ ( ] θ ( + θ ( Sendo u( representado por [2]: D( u( = θ ( Onde, D( é o deslocamento efetuado pelo robô e Ө( é a mudança de direção. E X(, representado por [2]: x( X( = y( θ ( Onde, x e y são as coordenadas dado o mapa do ambiente e Ѳ é a orientação do robô. Calcula-se também a incerteza (P(k+1 ) associada à postura estimada do robô [2]: T P ( k + 1 = F*P( *F + C V Sendo F a matriz do modelo de movimento linearizada [2]: 5

6 1 F = 1 D( *sinθ ( D( *cosθ ( 1 As matrizes 3x3 de covariância do erro P( e Cv são fornecidas com valores que determinam a probabilidade de erro associada à postura estimada do robô. Emparelhamento Dados a incerteza P(k+1), o modelo de observação linearizado H e a covariância da inovação S, calcula-se o ganho de Kalman, que irá possibilitar a comparação dos valores preditos com os observados, de modo que seja possível analisar uma medida do quanto a observação está compatível com a postura estimada do robô, calculada na predição [2] e [7]. O ganho de Kalman é dado por [2]: K( k + 1) = P( k + 1)*H T -1 *(S) Sendo a inovação a diferença entre observações dada por [2]: v( k + 1) = z(k + 1) h[xˆ ( k + 1, ξ ] Onde, z é a distancia lida do robô em relação ao marco e h é a distância observada, dada por [2]: h[x(, ξ ] = [( x( x ) + ( y( 2 m y m ) 2 ] 1 2 Quando a inovação resulta no valor ou próximo de, considera-se válida a informação observada. Para calcular o ganho de Kalman, é preciso calcular também a covariância da inovação S [2]: T S = H*P( k + 1 *H + Onde, Cw corresponde ao um valor de probabilidade de erro associado ao modelo de observação do laser (h). C ω 6

7 E o modelo de observação linearizado H é dado por [2]: 2 * x( *[ x( = x H = 2 * y( *[ y( = y A área de validação é calculada para ignorar valores inválidos e saber se é necessário executar a atualização, dada por [1]: v( k + 1)*S -1 m m ] ] * v(k + 1) g Onde, g² é um limiar escolhido empiricamente. Dessa forma, o emparelhamento tem o objetivo de averiguar as informações preditas com as observadas e dizer se o processo de atualização será executado ou não, pois apenas as observações válidas são usadas na atualização, ou seja, ela só será executada caso sejam encontrados erros de predição, caso não, a próxima iteração do filtro será executada. 2 T Atualização Tem-se como base os cálculos da fase de emparelhamento para, na fase de atualização, corrigir a estimação do estado e a covariância do erro associada, para então executar a próxima iteração do filtro com informações mais próximas o possível da real localização do robô no ambiente o qual está inserido [2]. A nova postura estimada é dada por [2]: xˆ ( k + 1) = xˆ ( k K( k + 1)*v( k + 1) E a nova incerteza associada à postura atualizada é dada por [1]: P( k + 1) = P( k + 1 K( k + 1)*S( k + 1)*K( k + 1) T 7

8 Ambientes utilizados Figura 2: Representação do ambiente 1 com marcos e da trajetória do robô Experimentos O FKE, como descrito anteriormente, foi testado num ambiente simulado, utilizando MATLAB, de acordo com as especificações posteriores. Foram feitos três testes para covariâncias de erro da posição inicial, do modelo de movimento e do modelo de observação, sendo cada um feito com o mesmo ambiente e para as mesmas entradas de controle. As covariâncias do erro são apresentadas na Tabela 1. Teste 1 Teste 2 Teste 3 P ().5 =.5.5 P ().6 =.6.6 P ().7 = Cv = Cv = Cv =.6.6 8

9 C ω = [.1] C ω = [.2] C ω = [.4] Tabela 1: Covariâncias de erro em relação a posição do robô, ao modelo de movimento e ao modelo de observação. As entradas de controle estão representadas na Tabela 2. A posição dos marcos e a trajetória do robô podem ser visualizados na Figura 2. TEMPO Comando Movimento Z lido Posição do marco 1 [2; ] 1 (5,2) 2 [; 9] 4 (4,6) 3 [1; ] 3 (4,6) 4 [2; ] 1 (4,6) 5 [; -9] 5 (9,5) 6 [2; ] 3 (9,5) 7 [1; ] 2 (9,5) 8 [; -9] 3 (7,2) 9 [2; ] 1 (7,2) 1 [; 9] 5 (9,5) 11 [3; ] 3 (13,3) 12 [1; ] 2 (13,3) 13 [1; ] 1 (13,3) 14 [; 9] 5 (12,8) 15 [2; ] 3 (12,8) 16 [1; ] 2 (12,8) 17 [1; ] 1 (12,8) 18 [; 9] 6 (6,7) 19 [2; ] 4 (6,7) 9

10 2 [3; ] 1 (6,7) 21 [; -9] 5 (7,12) 22 [2; ] 3 (7,12) 23 [1; ] 2 (7,12) 24 [1; ] 1 (7,12) 25 [; 9] 4 (3,14) Tabela 2: Entradas de controle. Resultados Os teste 1, 2 e 3 resultaram em uma única estimação ótima para as entradas de controle da Tabela 2. Resultado este que é apresentado na Tabela 1, 2 e 3, respectivamente, alterando as covariâncias de erro dadas na Tabela 1. Teste 1 Iteração Posição Real Comando Movimento Posição Predita Pelo FKE Covariância Z ESTIMADO Z Lido Posição Atualiza da Pelo FKE Covariância [2; 2; ] 1 [4; 2; ] [2; ] [4; 2; ] [.7;2.7;.7] [4; 2; ] [.16;2.7;.7] 2 [4; 2; 9] [; 9] [4; 2; 9] [.216;2.9;.9] 4 4 [4; 2; 9] [.216;.4; ] 3 [4; 3; 9] [1; ] [4; 3; 9] [.9;.24;.7552] [4; 3; 9] [.9568;.3;.75 52] 4 [4; 5; 9] [2; ] [4; 5; 9] [6.3985;.23;.9552] [4; 5; 9] [6.3985;.1;.5 1

11 5 ] 5 [6.5985;.21;1.1552] 5 5 [4; 5; ] [; -9] [4; 5; ] [4; 5; ] [.1;.21;.5 86] 6 [6; 5; ] [2; ] [6; 5; ] [.21;2.4353;.786] [6; 5; ] [.1;2.4353;.7 866] 7 [.21;5.3782;.986] [7; 5; ] [1; ] [7; 5; ] [7; 5; ] [.1;5.3782;.9 86] 8 [7; 5; - 9] [; -9] [7; 5; - 9] [.21;5.5782;1.186] [7; 5; -9] [.21;.1;.57 47] 9 [7; 3; - 9] [2; ] [7; 3; - 9] [2.6989;.21;.7747] [7; 3; -9] [2.6989;.27;.77 47] 1 [7; 3; ] [; 9] [7; 3; ] [2.8989;.227;.9747] 6 6 [7; 3; ] [;.227;.519] 11 [1; 3; ] [3; ] [1; 3; ] [.2;5.735;.719] [1; 3; ] [;5.735;.719] 12 [11; 3; ] [1; ] [11; 3; ] [.2;9.163;.919] [11; 3; ] [.1;9.163;.91 9] 13 [12; 3; ] [1; ] [12; 3; ] [.21; ;1.119] [12; 3; ] [.2; ;1.1 19] 14 [12; 3; 9] [; 9] [12; 3; 9] [.22; ;1.319] 5 5 [12; 3; 9] [.22;.1;.63 75] 15 [12; 5; 9] [2; ] [12; 5; 9] [2.95;.21;.8375] [12; 5; 9] [2.95;.1;.83 75] 16 [12; 6; [1; ] [12; 6; [12; 6; 11

12 9] 9] [6.5373;.21;1.375] 9] [6.5373;.2;1.3 75] 17 [12; 7; 9] [1; ] [12; 7; 9] [ ;.22;1.2375] [12; 7; 9] [ ;.5; ] 18 [12; 7; 18] [; 9] [12; 7; 18] [ ;.25;1.4375] 6 6 [12; 7; 18] [;.25;.6242] 19 [1; 7; 18] [2; ] [1; 7; 18] [.2;2.8973;.8242] 4 4 [1; 7; 18] [;2.8973;.8242] 2 [7; 7; 18] [3; ] [7; 7; 18] [.2;18.53;1.242] [7; 7; 18] [.5;18.53;1. 242] 21 [7; 7; 9] [; -9] [7; 7; 9] [.25;18.253;1.2242] 5 5 [7; 7; 9] [.25;;.4637] 22 [7; 9; 9] [2; ] [7; 9; 9] [2.2552;.2;.6637] [7; 9; 9] [2.2552;;.6637] 23 [7; 1; 9] [1; ] [7; 1; 9] [4.9735;.2;.8637] [7; 1; 9] [4.9735;.1;.86 37] 24 [7; 11; 9] [1; ] [7; 11; 9] [9.2192;.21;1.637] [7; 11; 9] [9.2192;.2;1.6 37] 25 [5; 11; 18] [; 9] [5; 11; 18] [9.4192;.22;1.2637] 4 4 [5; 11; 18] [;.22;.624] Teste 2 Iteração Posição Real Comando Movimento Posição Predita Pelo FKE Covariância Z ESTIM ADO Z Lido Posiçã o Atualiz ada Pelo FKE Covariância [2; 2; ] 12

13 1 [4; 2; ] [2; ] [4; 2; ] [1.;3.4;1.] [4; 2; ] [.31;3.4;1.] 2 [4; 2; 9] [; 9] [4; 2; 9] [.431;3.8;1.4] 4 4 [4; 2; 9] [.431;.8;1.211] 3 [4; 3; 9] [1; ] [4; 3; 9] [1.8242;.48; ] [4; 3; 9] [1.8242;.6;1.4211] 4 [4; 5; 9] [2; ] [4; 5; 9] [ ;.46;1.8211] [4; 5; 9] [ ;.2; ] 5 [4; 5; ] [; -9] [4; 5; ] [ ;.42;2.2211] 5 5 [4; 5; ] [.1;.42;1.168] 6 [6; 5; ] [2; ] [6; 5; ] [.41;4.8692; ] [6; 5; ] [.2;4.8692;1.4168] 7 [7; 5; ] [1; ] [7; 5; ] [.42;1.7532;1.8168] [7; 5; ] [.3;1.7532; ] 8 [7; 5; -9] [; -9] [7; 5; -9] [.43; ;2.2168] [7; 5; - 9] [.43;.2;1.1494] 9 [7; 3; -9] [2; ] [7; 3; -9] [5.3978;.42; ] [7; 3; - 9] [5.3978;.42;1.5494] 1 [7; 3; ] [; 9] [7; 3; ] [5.7978;.455; ] 6 6 [7; 3; ] [;.455;1.379] 11 [1; 3; ] [3; ] [1; 3; ] [.4;1.147;1.4379] [1; 3; ] [.1;1.147; ] 12 [11; 3; ] [1; ] [11; 3; ] [.41; ;1.8379] [11; 3; ] [.1; ; ] 13 [12; 3; ] [1; ] [12; 3; ] [.41; ;2 [12; 3; ] [.3; ;

14 .2379] 9] 14 [12; 3; 9] [; 9] [12; 3; 9] [.43; ;2.6379] 5 5 [12; 3; 9] [.43;.2;1.2749] 15 [12; 5; 9] [2; ] [12; 5; 9] [5.9;.42; ] [12; 5; 9] [5.9;.2;1.6749] 16 [12; 6; 9] [1; ] [12; 6; 9] [13.746;.42;2.749] [12; 6; 9] [13.746;.3;2.749] 17 [12; 7; 9] [1; ] [12; 7; 9] [23.999;.43;2.4749] [12; 7; 9] [23.999;.1; ] 18 [12; 7; 18] [; 9] [12; 7; 18] [24.399;.41;2.8749] 6 6 [12; 7; 18] [;.41;1.2484] 19 [1; 7; 18] [2; ] [1; 7; 18] [.4;5.7946; ;1.6484] 4 4 [1; 7; 18] [;5.7946;1.6484] 2 [7; 7; 18] [3; ] [7; 7; 18] [.4;36.16; ] [7; 7; 18] [.1;3616;2.484 ] 21 [7; 7; 9] [; -9] [7; 7; 9] [.4;36.16;2.4 84] 5 5 [7; 7; 9] [.41;;.9273] 22 [7; 9; 9] [2; ] [7; 9; 9] [4.514;.4; ] [7; 9; 9] [4.514;.1;1.3273] 23 [7; 1; 9] [1; ] [7; 1; 9] [9.9471;.41; [7; 1; 9] [9.9471;.1;1.7273] 24 [7; 11; 9] [1; ] [7; 11; 9] [ ;.41;2 [7; 11; 9] [ ;.4;2.1273] 14

15 .1273] 25 [5; 11; 18] [; 9] [5; 11; 18] [ ;.44;2.5273] 4 4 [5; 11; 18] [.1;.44;1.248] Teste 3 Iteração Posição Real Comando Movimento Posição Predita Pelo FKE Covariância Z ESTIM ADO Z Lido Posiçã o Atualiz ada Pelo FKE Covariância [2; 2; ] 1 [4; 2; ] [2; ] [4; 2; ] [1.3;4.1;1.3] [4; 2; ] [.62;4.1;1.3] 2 [4; 2; 9] [; 9] [4; 2; 9] [.662;4.7;1.9] 4 4 [4; 2; 9] [.662;.16;1.4831] 3 [4; 3; 9] [1; ] [4; 3; 9] [2.6893;.616;2. 831] [4; 3; 9] [2.6893;.12;2.831] 4 [4; 5; 9] [2; ] [4; 5; 9] [ ;.612;2.6831] [4; 5; 9] [ ;.4; ] 5 [4; 5; ] [; -9] [4; 5; ] [ ;.64; ] 5 5 [4; 5; ] [.2;.64;1.5251] 6 [6; 5; ] [2; ] [6; 5; ] [.62;7.345; ] [6; 5; ] [.3;7.345;2.1251] 7 [7; 5; ] [1; ] [7; 5; ] [.63;16.123;2.7251] [7; 5; ] [.5;16.133; ] 8 [7; 5; -9] [; -9] [7; 5; -9] [.65;16.733;3.3251] [7; 5; - 9] [.65;.4;1.7242] 15

16 9 [7; 3; -9] [2; ] [7; 3; -9] [8.973;.64;2. 342] [7; 3; - 9] [8.973;.19;2.3242] 1 [7; 3; ] [; 9] [7; 3; ] [8.6973;.619; ] 6 6 [7; 3; ] [.1;.69;1.557] 11 [1; 3; ] [3; ] [1; 3; ] [.61; ;2.157] [1; 3; ] [.1; ;2.157 ] 12 [11; 3; ] [1; ] [11; 3; ] [.61; ;2.757] [11; 3; ] [.2; ;2.757 ] 13 [12; 3; ] [1; ] [12; 3; ] [.62; ;3.357] [12; 3; ] [.7; ; ] 14 [12; 3; 9] [; 9] [12; 3; 9] [.67; ;3.957] 5 5 [12; 3; 9] [.67;.4;1.9125] 15 [12; 5; 9] [2; ] [12; 5; 9] [8.857;.64; ] [12; 5; 9] [8.857;.4;2.5125] 16 [12; 6; 9] [1; ] [12; 6; 9] [ ;.7;3.1125] [12; 6; 9] [ ;.7; ] 17 [12; 7; 9] [1; ] [12; 7; 9] [36.7;.67;3.7125] [12; 7; 9] [36.7;.2; ] 18 [12; 7; 18] [; 9] [12; 7; 18] [36.67;.62;4.3125] 6 6 [12; 7; 18] [;.62;1.8726] 19 [1; 7; 18] [2; ] [1; 7; 18] [.6;8.6924; ] 4 4 [1; 7; 18] [.1;8.6924;2.4726] 16

17 2 [7; 7; 18] [3; ] [7; 7; 18] [.61;54.167;3.726] [7; 7; 18] [.2;54.167;3.72 6] 21 [7; 7; 9] [; -9] [7; 7; 9] [.62; ;3.6726] 5 5 [7; 7; 9] [.62;.1;1.391] 22 [7; 9; 9] [2; ] [7; 9; 9] [6.7662;.61;1. 991] [7; 9; 9] [6.7662;.1;1.991] 23 [7; 1; 9] [1; ] [7; 1; 9] [ ;.61;2.51] [7; 1; 9] [ ;.2;2.591 ] 24 [7; 11; 9] [1; ] [7; 11; 9] [ ;.62;3.191] [7; 11; 9] [ ;.8;3.191 ] 25 [5; 11; 18] [; 9] [5; 11; 18] [.1;.68;1. 872] 4 4 [5; 11; 18] [.1;.44;1.248] 17

18 Referências Bibliográficas [1] Odakura, Valguima Victoria Viana Aguiar. Aula 16 - Filtro de Kalman, UFGD, 21. [2] Odakura, Valguima Victoria Viana Aguiar. Aula 18 Filtro de Kalman Estendido, UFGD, 21. [3] SANTANA, A. M.; AIRES, K. R. T.; MEDEIROS, A. A. D.. Localização Relativa de um Robô Humanóide Utilizando Filtro de Kalman Estendido. In: ERCEMAPI - Escola Regional de Computação Ceará - Maranhão - Piauí, 29, Parnaíba, PI. Anais do III ERCEMAPI. Teresina, PI : EDUFPI, 29. v. 1. [4] Odakura, Valguima Victoria Viana Aguiar. Aula 1 Localização de Robôs Móveis, UFGD, 21. [5] Odakura, Valguima Victoria Viana Aguiar. Aula 9 - Cinemática, UFGD, 21. [6] WAGNER, E. R.; HEMERLY, E. M. Estimação de Postura de Robôs Móveis via Filtro de Kalman Usando Odometria e Scanner a laser. Tese (Mestrado) Instituto Tecnológico de Aeronáutica - ITA (24). [7] RUSSEL, Stuart J, RUSSEL, Peter Norvig. Inteligência Artificial. 2 Edição. Rio de Janeiro. Elsevier, 24. [8] SILVA, Claudiney Calixto da; PEREIRA, Juliano Augusto. Implementação e Análise de Localização em Robótica Móvel por Filtro de Kalman. In: Encontro de Iniciação Científica e Pós-Graduação do ITA (ENCITA), 23, São José dos Campos,

19 Anexo CÓDIGO-FONTE O código MATLAB para a função de predição pode ser visto na Listagem 1. function [Xe, Pe] = predicao(xk, Pe, Cv, u %O robô se move e usa parâmetros de controle para estimar sua postura no estado k+1 orient= deg2rad(xk(3,1)); Xe = [xk(1,1) + (uk(1,1)*cos(orient));xk(2,1) + (uk(1,1)*sin(orient)); xk(3,1) + uk(2,1)]; %Versão linearizada do modelo de movimento é gerada na estimativa do estado atual F = [1,, -uk(1,1)*(sin(orient));,1,uk(1,1)*(cos(orient));,,1]; %A incerteza do estado é gerada atualizando a matriz de covariância usando os movimentos obtidos e o tempo k Pe = ((F*Pe)*F')+Cv; Listagem 2. Listagem 1: código MATLAB da função de predição do FKE. O código MATLAB para a função de emparelhamento pode ser visto na function [H,h, Kganho, VInov, Valid,S] = emparelhamento(xk, pm, Pe, Cw, z) %Modelo de observação linearizado H = [2*xk(1,1)*(xk(1,1) - pm(1,1)) (2*xk(2,1))*(xk(2,1) - pm(1,2)) ]; %Modelo de observação do laser h = sqrt([xk(1,1) - pm(1,1)]^2 + [xk(2,1) - pm(1,2)]^2); %Inovação VInov = z - h; %Covariância da inovação S = ((H*Pe)*H') + Cw; %Ganho de Kalman Kganho = (Pe*H')*inv(S); % Cálculo da área de validação Valid = (VInov*inv(S))*VInov'; Listagem 2: código MATLAB da função de emparelhamento do FKE. 19

20 O código MATLAB para a função de atualização pode ser visto na Listagem 3. function [Xe, Pe] = atualizacao(xe, Pe, Kganho, VInov, S) %x estimado atualizado Xe = Xe + (Kganho*VInov); %Covariância do erro atualizada Pe = Pe - ((Kganho*S)*Kganho'); Listagem 3: código MATLAB da função de atualização. A Listagem 4 mostra o código do teste 1. Nos outros dois testes as únicas informações que se alteram são os valores das matrizes de covariância do erro P(k+1), Cv e Cw. 2

21 function [xk, Cv,uk, Cw, z, Pk, pm] = teste1() %Posição inicial Xe = [2;2;]; %Matrizes de coraviância do erro Cv = [.2,,;,.2,;,,.2]; Cw = [.1]; Pe = [.5,,;,.5,;,,.5]; %Entradas de controle para o tempo 1 uk = [2;]; z = 1; pm = [5 2]; %Vetores de entradas de controle, do tempo 2 ao 25 Z=[4,3,1,5,3,2,3,1,6,3,2,1,5,3,2,1,6,4,1,5,3,2,1,4]; Dk = [ ]; orientacao = [ ]; pos_marco = [4 6;4 6;4 6;9 5; 9 5; 9 5;7 2;7 2;1;1;1;1;12 8;12 8;12 8;12 8;6 7;6 7;6 7;7 12;7 12;7 12;7 12;3 11]; %Percorre os vetores de entradas de controle, do tempo 2 ao 25 for i=1:1:25 disp('tempo '); disp(i); if(i > 1) uk(1,1) = Dk(i-1); uk(2,1) = orientacao(i-1); z = Z(i-1); pm(1,1) = pos_marco(i-1,1); pm(1,2) = pos_marco(i-1,2); end %Predição [Xe, Pe] = predicao(xe, Pe, Cv, u; disp('x estimado: '); disp(xe); disp('covariância X estimado: '); disp(pe); %Emparelhamento [H,h, Kganho, VInov, Valid,S] = emparelhamento(xe, pm, Pe, Cw, z); disp('z lido: '); disp(z); disp('z estimado: '); disp(h); end end %Atualização if(valid <=8) [Xe, Pe] = atualizacao(xe, Pe, Kganho, Valid, S); disp('x estimado atualizado: '); disp(xe); disp('covariância X estimado atualizado: '); disp(pe); end Listagem 4: código MATLAB do teste 1. 21