Calibração de acelerômetros numa IMU -- Parte 2

Após termos determinados os parâmetros de calibração, faz-se necessário resolver o sistema de equações a seguir:

de modo a poder ter-se em mãos os valores "reais" das acelerações. O sistema como está não possui solução exata a ser obtida por meio de uma única equação algébrica. A solução então parte por solução numérica. Existem diversas formas de se resolver esse problema, só procurar algo como nonlinear equations em livros de cálculo numérico que dá para se encontrar algo parecido. Será utilizado o método Newton-Raphson multidimensional.

Em resumo, em um sistema de equações da forma:

A ideia é conseguir encontrar a sequência , tal que , em que:

Para esse caso, é fundamental se determinar o Jacobiano do sistema. O Jacobiano é obtido por meio das seguintes derivadas parciais:

A maneira iterativa de se resolver esse problema segue os seguintes passos:

1º) Dá-se um chute inicial .

2.a) A atualização iterativa é dada por meio de:

2.b) ou por meio de (mais eficiente numericamente, pois não precisa realizar inversões de matrizes):

e

3º) Verifica a convergência, por exemplo, por meio de

4º) Caso a convergência não seja suficiente, repete-se os passos 2 e 3 até que haja a convergência.

Para o caso de se querer resolver a equação do tópico 2.b), a gente pode utilizar o método de Gauss-Jordan, que nada mais é do que o método do pivoteamento e consequente transformação dos elementos em uma diagonal principal unitária. A título de exemplo, tome o seguinte sistema:

A ideia é criar um sistema aumentado e realizar o pivoteamento dos elementos de modo a se ter uma matrix identidade do lado esquerdo e o resultado do lado direito. Ou seja, com base no sistema, a gente cria o sistema aumentado:

e faz o pivoteamento e eliminações de linhas e colunas de modo a resultar na seguinte solução:

Voltando para o caso específico do nosso acelerômetro, a gente realiza o cálculo da seguinte forma:

1) Para o sistema de equações, elenco e como as variáveis solução do meu problema. Sendo assim, reescreveremos o sistema dado por:

para algo como:

2) Faço o Jacobiano do sistema:

E observando as equações, tem-se que:

Por meio desse Jacobiano, faz-se as iterações para os respectivos valores do sistemas e procura-se a solução para e conforma apresentado logo acima.

Para tentar acelerar o processo de formação da matriz aumentada (Augm) e com isso realizar o pivoteamento, o que a gente irá fazer é criar uma matrix da seguinte forma:

Para o caso do inercial em desenvolvimento, o sequinte código em C++ faz essa atividade de convergência das variáveis:

void MPU_6050::obtain_processed_data()

{

// ==============================================

// Acelerômetros

// ==============================================

static float fmx, fmy, fmz;

static float fx, fy, fz;

static float Augm[3][4];

 

// Para a convergência do sistema.

static float tol = 1e-6;

static int max_iter = 5;

 

for (int iter = 0; iter < max_iter; iter++) {

// Valores medidos pelos acelerômetros (não compensado ainda).

fmx = this->raw_ax;

fmy = this->raw_ay;

fmz = this->raw_az;

 

// Chute inicial -- x{0}

fx = this->raw_ax;

fy = this->raw_ay;

fz = this->raw_az;

// Monta a matriz aumentada (J(x) | M(x)).

Augm[0][0] = 1.0f + (1.0f + S1_acc[0][0]) + (2.0 * S2_acc[0][0] * fx);

Augm[0][1] = N1_acc[0][1];

Augm[0][2] = N1_acc[0][2];

Augm[0][3] = b_acc[0][0] + (2.0f * fx) + (S1_acc[0][0] * fx) + (S2_acc[0][0] * fx * fx) + (N1[0][1] * fy) + (N1[0][2] * fz) - fmx;

Augm[1][0] = N1_acc[1][0];

Augm[1][1] = 1.0f + (1.0f + S1_acc[1][0]) + (2.0 * S2_acc[1][0] * fy);

Augm[1][2] = N1_acc[1][2];

Augm[1][3] = b_acc[1][0] + (2.0f * fy) + (S1_acc[1][0] * fy) + (S2_acc[1][0] * fy * fy) + (N1[1][0] * fx) + (N1[1][2] * fz) - fmy;

Augm[2][0] = N1_acc[2][0];

Augm[2][1] = N1_acc[2][1];

Augm[2][2] = 1.0f + (1.0f + S1_acc[2][0]) + (2.0 * S2_acc[2][0] * fz);

Augm[2][3] = b_acc[2][0] + (2.0f * fz) + (S1_acc[2][0] * fz) + (S2_acc[2][0] * fz * fz) + (N1[2][0] * fx) + (N1[2][1] * fy) - fmz;

 

// Pivoteamento do sistema com solução.

for (size_t k = 0; k < 3; k++) {

// Normaliza a linha pivô

float piv = Augm[k][k];

for (size_t j = 0; j < 4; j++) {

Augm[k][j] /= piv;

}

 

// Elimina os outros elementos na mesma coluna

for (size_t i = 0; i < 3; i++) {

if (i != k) {

float fator = Augm[i][k];

for (size_t j = 0; j < 4; j++) {

Augm[i][j] -= fator * Augm[k][j];

}

}

}

}

// Atualização (delta)

float dfx = Augm[0][3];

float dfy = Augm[1][3];

float dfz = Augm[2][3];

 

// f(k+1) = f(k) - delta_x

fx -= dfx;

fy -= dfy;

fz -= dfz;

 

// Critério de parada

if (fabs(dfx) < tol && fabs(dfy) < tol && fabs(dfz) < tol) {

break; // convergiu

}

}

// Atualiza as variáveis já calibradas dos acelerômetros

this->processed_ax = fx;

this->processed_ay = fy;

this->processed_az = fz;

}

Nos próximos posts, eu irei fazer uma equivalência para os girômetros.