Processo Gauss-Markov de 1º ordem

Processo de Gauss-Markov

Estou estudando pelo livro "Fundamentals of Inertial Navigation, Satellite-based Positioning and their Integration", do Noureldin, no que tange, em específico, a parte de modelagem de erros aleatórios dos sensores como girômetro e acelerômetros. Estou trabalhando com o MPU6050 para entender melhor como funciona esses pormenores e entender a modelagem que empregarei para a fusão de dados de navegação com as informações de um receptor GPS (mais detalhes oportunamente).

Na seção 6.1.4 do livro ele aborda algumas condicionantes referentes aos erros de sensores, levando à característica de erros não determinísticos, ou seja, aleatórios, e modelados por modelos estocásticos. Os erros de modelos são usualmente correlacionados no tempo e podem ser modeloados de diversas formas, tais como: processo de caminhada aleatória (random walk process), processo de 1º ordem Gauss-Markov (GM) e processos autoregressivos (AR). Abordarei aqui o processo GM. Os demais eu não estudei ainda, mas assim o farei oportunamente.

O processo Gauss-Markov de 1º ordem é bem utilizado para a modelagem de erros estocásticos de sensores, e possui a seguinte formulação algébrica:

em que:

1. x é o processo aleatório;
2. β é o recíproco do tempo de correlação do processo;
3. w é o ruído gaussiano de média zero e variância unitária; e
4. é a variância do ruído branco associada com o processo randômico.

Para fins de visualização de certos itens da formulação GM, observe a figura abaixo:

A variável é o parâmetro de autocorrelação da variável x por ela mesma. Ou seja, de posse de uma sequência numérica coletada em uma data frequência de amostragem conhecida, a gente pode determinar os parâmetros da função de autocorrelação . A função de autocorreção mede a correlação entre uma série e , para é um processo estocástico. A série representa a série defasada temportalmente de τ.

A autocorreçalação para um lag τ é dada por:

onde:

• , é o coeficiente de autocorrelação com defasagem τ.
• é a variância da série temporal.

Para os parâmetros acima, tem-se que:

• é a média temporal da série, calculada por meio de ; e
• N é o número total de observações da série.

Dá para se criar uma função em C++ ou mesmo em Matlab que execute esses cálculos, um a um. Eu optei, inicialmente, por utilizar a função autocorr presente no matlab para realizar essa tarefa para mim. Para maiores informações sobre essa função, consultar o seguinte link.

Os meus dados foram coletados na frequência de 100Hz. São dados oriundos diretamente do meu MPU6050 acoplado num ESP32 DevKit v1 de 30 pinos. Os dados são coletados via I2C, passam por uma transformação para dados de engenharia e foram impressos na serial. Estou utilizando o FreeRTOS para me possibilitra executar uma task no intervalo de 10ms. O período de tempo entre a coleta de dados, conversão para unidades de engenharia e impressão na serial demora cerca de 2~2,2ms. Após isso, o ESP32 espera até o time de 10ms para executar essa rotina novamente. Construi um programa em python que faz a leitura da serial e salva os parâmetros em um arquivo "*.txt". No Matlab eu já li e separei, integralmente, os dados recebidos, e eles foram, para mim, acessados por meio do seguinte código:

clear all; close all; clc;

 

% Carrega os dados salvos.

load("dados_processados.mat");

 

% Frequência de amostragem.

Fs = 100;

Os dados presentes no meu programa são referentes aos três acelerômetros e três girômetros do MPU6050. Eles, após carregados, podem ser acessados pelos parâmetros fx, fy e fz, para os acelerômetros, e wx, wy e wz, para os giros. Utilizarei os dados do acelerômetro do eixo x para análise (fx).

plot(fx);

grid on;

ylabel('acc eixo x');

xlabel('');

xlabel('qnt. de dados');

A primeira vez que olhei esse gráfico, eu particularmente estranhei, pois acreditei piamente que havia deixado o meu sensor MPU6050 parado durante cerca de 8 horas. Eu particularmente tive de reduzir a minha análise, de modo a buscar uma região onde eu pudesse ter dados sem "interferências". Eu vi que até aproximadamente a coleta 850000 eu tinha parâmetros característicos do sensor parado.

fx_cut = fx(1:850000);

hold on;

plot(fx_cut, 'r');

legend('Dados integrais', 'Dados cortados');

Achei, no visual, os dados interessantes e trabalharei com esse corte.

% Estimativa inicial do desvio padrão e variância

disp(['Desvio Padrão (m/s^2)......: ', num2str(std(fx_cut))]);

Desvio Padrão (m/s^2)......: 0.034331

disp(['Variância (m^2/s^4)........: ', num2str(var(fx_cut))]);

Variância (m^2/s^4)........: 0.0011786

Obtenção do gráfico de função autocorrelação (normalizada).

% Obtenção da autocorrelação.

[funcao_auto_correlacao, lags] = autocorr(fx_cut);

tau = lags * (1/Fs);

figure;

plot(tau, funcao_auto_correlacao);

grid;

xlabel("\tau");

ylabel("R_{xx}(\tau)");

Para encontrar o parâmetro β eu estou utilizando uma função de otimização, descrita a seguir:

function [out] = custo(dados_in, beta)

% ***********************************************

% Esta função busca encontrar um 'beta' de

% uma curva com a seguinte expressão algébrica:

%

% -beta * |x|

% f(x) = (sigma^2) * e

%

% quando comparada com os parâmetros de uma curva

% cujos parâmetros 'x', 'y' e 'sigma2', vêm por

% meio do array 'dados_in'.

%

% Importante destacar que a função autocorrelação

% acima NÃO está normalizada. Isso implica que

% teremos que normalizada, o que em termos prá-

% ticos indica fazer a seguinte normalização em

% sigma:

%

% -beta * |x|

% f(x) = e

% ***********************************************

 

% Extrai os dados da curva referência.

x = dados_in(:,1);

y = dados_in(:,2);

 

% Para cada ponto to expectro.

fun_x = zeros(max(size(x)),1);

delta = zeros(max(size(x)),1);

for i = 1:max(size(x))

% Curva f(x) a ser otimizada calculada em x(i).

fun_x(i) = exp(-beta * abs(x(i)));

 

% Diferença curva real e desejada.

delta(i) = y(i) - fun_x(i);

end

% Módulo da diferença total.

out = sum(delta.^2);

end

% Organizo os dados para a otimização numérica a ser realizada.

dados_in = [tau' funcao_auto_correlacao'];

 

% Função custo

fun = @(x)custo(dados_in, x);

% Otimização de beta

beta0 = 0.01; % Chute inicial

beta = fminsearch(fun, beta0);

disp();

disp(['beta estimado ............: ', num2str(beta), ' s^-1']);

beta estimado ............: 335.4756 s^-1

disp(['const. de tempo (1/beta)..: ', num2str(1/beta), ' s']);

const. de tempo (1/beta)..: 0.0029808 s

disp();

 

% Plota o resultado encontrado.

y_est = zeros(max(size(tau)));

for i = 1:max(size(tau))

y_est(i) = exp(-beta*abs(tau(i)));

end

hold on;

plot(tau, y_est, 'r');

legend('Autocorrelação', 'Curva ótima');

A ideia é realizar esse cálculo para todas as outras medidas, ou seja, fy, fz, wx, wy e wz. De modo a poder se obter esses parâmetros de maneira completa, é interessante fazer uma função que cuspa tudo de uma única vez.