Cálculo passo a passo das matrizes de covariância em Ekf-slam
O Filtro Kalman Extended (EKF) Simultaneous Localization and Mapping (SLAM) é uma técnica usada na robótica para construir um mapa de um ambiente desconhecido, enquanto determina simultaneamente a posição do robô dentro dele. Um componente chave da matriz de covariância EKF-SLAM é a matriz de covariância, que representa a incerteza no estado estimado do robô e as características do mapa. Este artigo fornece um processo passo a passo para calcular matrizes de covariância dentro do EKF-SLAM.
Inicialização da Matriz de Covariância
O processo começa com a inicialização da matriz de covariância, tipicamente denotada como P. Esta matriz combina as incertezas da pose do robô e as características do mapa. A covariância inicial reflete a confiança inicial na posição inicial do robô e as características conhecidas.
Passo de Predição
Durante a fase de predição, o modelo de movimento do robô é utilizado para estimar o novo estado. A matriz de covariância é atualizada usando o modelo de movimento Jacobiano, denotado como F, e a covariância de ruído do processo, Q. A atualização segue:
Ppred = F P FT[ + Q
Atualizar passo com medições
Quando novas medições são recebidas, a matriz de covariância é atualizada para incorporar essas informações. As variáveis Jacobian, H e R do modelo de medição são utilizadas para calcular o ganho de Kalman, K:
K = PpredHT[ (H Ppred[H[T[ + R)[-1]
A matriz de covariância é então atualizada como:
Pupd = (I - K H) Ppred[]
Características do mapa incorporado
A matriz de covariância se expande para incluir características do mapa, aumentando em tamanho à medida que novas características são adicionadas. Cada atualização ajusta a incerteza associada tanto à pose do robô quanto às características, mantendo uma estimativa consistente da incerteza geral no mapa e localização.