Modelo matemático para acelerômetro
Esse modelo eu peguei no livro do Noureldin (Fundamentals of Inertial Navigation, Satellite-based, Positioning ant their Integration, Springer, 2013.), presente na seção 4.11.
em que:
: é o valor medido diretamente pelo acelerômetro, ou seja, o que a gente ler do hardware;
: é o valor verdadeiro a ser empregado no desenvolvimento das equações de mecanização;
: é a matriz que condensa os fatores de escala linear;
: é a matriz que condensa os fatores de escala não-linear;
: é a matriz que representa a não hortogonalidade da tríade de acelerômetros;
: é o vetor de gravidade anômalo, ou seja, os desvios em relação ao valor teórico; e
: é o vetor que representa os erros dos ruídos dos sensores.A seguir, as matrizes e seus termos:
e 
OBS: o subscrito "m" nos termos de dizem respeito à medida realizada (measurement).
dizem respeito à medida realizada (measurement). Desenvolvendo a equação acima, em seus termos (modificação aqui minha), tem-se que:
Após juntar os termos semelhantes, tem-se o seguinte resultado:
Que desenvolvidas, termo a termo, fornecem os seguintes resultados para cada um dos eixos.
1. X+
Posicionando o acelerômetro com o eixo X com a direção para cima, o vetor gravidade encontra-se na direção contrária à direção positiva do eixo X. Nessa condição, a medida de referência para a calibração deve ser +g para o eixo X. Desse modo, as equações ficam representadas da seguinte forma:
Nessa posição, com o sistema fixo e sem nenhuma parturbação, coleta-se vários dados dos sensores e tira-se a média de cada um deles (basicamente para tentar eliminar os erros de medida):
A ideia agora é guardar essas equações e tentar encontrar similares para os demais eixos e orientações possíveis.
2. X-
Após a coleta de bastante dados, tira-se a média das medidas (para basicamente eliminar o parâmetro de erro):
3. Y+
De modo similar ao que fora realizado para o eixo X, tem-se que:
Após bastante dados, tira-se a média (para basicamente eliminar o parâmetro de erro):
4. Y-
Após bastante dados, tira-se a média (para basicamente eliminar o parâmetro de erro):
5. Z+
De modo similar ao que fora realizado para os eixos X e Y, tem-se que:
Após bastante dados, tira-se a média (para basicamente eliminar o parâmetro de erro):
6. Z-
Após bastante dados, tira-se a média (para basicamente eliminar o parâmetro de erro):
Com base nesses parâmetros e médias do sistema, a ideia é tentar organizar os parâmetros em matrizes e encontrar a solução que melhor se acomoda ao sistema. Para tal a gente realizará o seguinte algoritmo.
- Coloca o IMU fixo com o eixo X apontado para cima (X up) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "x_up.txt".
- Coloca o IMU fixo com o eixo X apontado para baixo (X down) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "x_down.txt".
- Coloca o IMU fixo com o eixo Y apontado para cima (Y up) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "y_up.txt".
- Coloca o IMU fixo com o eixo Y apontado para baixo (Y down) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "y_down.txt".
- Coloca o IMU fixo com o eixo Z apontado para cima (Z up) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "z_up.txt".
- Coloca o IMU fixo com o eixo Z apontado para baixo (Z down) durante um tempo considerável (>10min, por exemplo). Salva o resultado das medidas de aceleração em um arquivo do tipo "z_down.txt".
- Carrega todos os dados num software de mais alto nível (Matlab, Python) ou mesmo em C/C++ (fica a seu critério) para separação das médias dos sistemas. A organização dos parâmetros e a separação de todos os termos encontram-de disponíveis no código abaixo:
clear all; close all; clc;
% ===========================================================
% Adiciona o caminho de modo a possibilitar pegar os dados
% coletados em determinado dia. Todos os 6 arquivos '*.txt'
% encontram-se dentro da pasta "dados_acelerometros"
% ===========================================================
addpath("dados_acelerometros");
% ===========================================================
% A primeira coisa que precisamos é coletar os parâmetros
% da calibração. No caso, a gente faz a parte de tratativa
% dos dados do sistema.
% ===========================================================
%%%%%%%%%%%%%%
% X UP %
%%%%%%%%%%%%%%
data = load('x_up.txt');
fx_x_up = mean(data(:,1));
fy_x_up = mean(data(:,2));
fz_x_up = mean(data(:,3));
clear data;
%%%%%%%%%%%%%%
% X DOWN %
%%%%%%%%%%%%%%
data = load('x_down.txt');
fx_x_down = mean(data(:,1));
fy_x_down = mean(data(:,2));
fz_x_down = mean(data(:,3));
clear data;
%%%%%%%%%%%%%%
% Y UP %
%%%%%%%%%%%%%%
data = load('y_up.txt');
fx_y_up = mean(data(:,1));
fy_y_up = mean(data(:,2));
fz_y_up = mean(data(:,3));
clear data;
%%%%%%%%%%%%%%
% Y DOWN %
%%%%%%%%%%%%%%
data = load('y_down.txt');
fx_y_down = mean(data(:,1));
fy_y_down = mean(data(:,2));
fz_y_down = mean(data(:,3));
clear data;
%%%%%%%%%%%%%%
% Z UP %
%%%%%%%%%%%%%%
data = load('z_up.txt');
fx_z_up = mean(data(:,1));
fy_z_up = mean(data(:,2));
fz_z_up = mean(data(:,3));
clear data;
%%%%%%%%%%%%%%
% Z DOWN %
%%%%%%%%%%%%%%
data = load('z_down.txt');
fx_z_down = mean(data(:,1));
fy_z_down = mean(data(:,2));
fz_z_down = mean(data(:,3));
clear data;
Após a coleta de dados do sistema, a ideia é tentar organizar os parâmetros de forma que a gente consiga organizar os parâmetros para buscar uma solução ótima para todos os parâmetros do sistema (menor custo). A forma encontrada de fazer isso foi organizar os dados no formato matricial:
em que 
é a matriz que condensa as medidas de médias das acelerações encontradas para cada eixo, X representa a matriz que desejamos encontrar. Em específico, para o meu caso, fiz a seguinte organização e sequência:
é a matriz que condensa as medidas de médias das acelerações encontradas para cada eixo, X representa a matriz que desejamos encontrar. Em específico, para o meu caso, fiz a seguinte organização e sequência:1º) Reescrever todas as equações acima apresentadas. Todas as variáveis a serem determinadas estão em negrito.
São ao todo 15 parâmetros a serem determinados, todos eles organizandos na variável X a seguir (você pode escolher a sua própria):
2º) Rearranjar as variáveis acima, de modo a separar as variáveis a serem determinadas, tem-se que:
O que equivale a escrevermos em produtos de matrizes a:
%============================================================
% Para resolução do sistema Ya = A.X, precisamos identificar
% as variáveis Ya e A.
%============================================================
% Matriz Ya
Ya = zeros(18, 1);
Ya(1, 1) = fx_x_up - 2*g;
Ya(2, 1) = fy_x_up;
Ya(3, 1) = fz_x_up;
Ya(4, 1) = fx_x_down + 2*g;
Ya(5, 1) = fy_x_down;
Ya(6, 1) = fz_x_down;
Ya(7, 1) = fx_y_up;
Ya(8, 1) = fy_y_up - 2*g;
Ya(9, 1) = fz_y_up;
Ya(10, 1) = fx_y_down;
Ya(11, 1) = fy_y_down + 2*g;
Ya(12, 1) = fz_y_down;
Ya(13, 1) = fx_z_up;
Ya(14, 1) = fy_z_up;
Ya(15, 1) = fz_z_up - 2*g;
Ya(16, 1) = fx_z_down;
Ya(17, 1) = fy_z_down;
Ya(18, 1) = fz_z_down + 2*g;
% Matriz A
A = zeros(18, 15);
A(1, 1) = 1;
A(1, 4) = g;
A(1, 13) = g*g;
A(2, 2) = 1;
A(2, 9) = g;
A(3, 3) = 1;
A(3, 11) = g;
A(4, 1) = 1;
A(4, 4) = -g;
A(4, 13) = g*g;
A(5, 2) = 1;
A(5, 9) = -g;
A(6, 3) = 1;
A(6, 11) = -g;
A(7, 1) = 1;
A(7, 7) = g;
A(8, 2) = 1;
A(8, 5) = g;
A(8, 14) = g*g;
A(9, 3) = 1;
A(9, 12) = g;
A(10, 1) = 1;
A(10, 7) = -g;
A(11, 2) = 1;
A(11, 5) = -g;
A(11, 14) = g*g;
A(12, 3) = 1;
A(12, 12) = -g;
A(13, 1) = 1;
A(13, 8) = g;
A(14, 2) = 1;
A(14, 10) = g;
A(15, 3) = 1;
A(15, 6) = g;
A(15, 15) = g*g;
A(16, 1) = 1;
A(16, 8) = -g;
A(17, 2) = 1;
A(17, 10) = -g;
A(18, 3) = 1;
A(18, 6) = -g;
A(18, 15) = g*g;
%%% Solução da equação
x = inv(A'*A) * A' * Ya;
É basicamente isso que a gente faz na tarefa de calibração dos acelerômetros, com a determinação dos 15 parâmetros possíveis nessa caracterização do sistema.
