Calibração dinâmica do magnetômetro

Estou trabalhando em um magnetômetro AK8975 que faz parte de uma IMU. O que parece ser muito complicado para mim. Esse chip fornece um vetor 3D como saída, descrevendo o campo magnético da Terra em qualquer lugar ou perto dele.

Eu tentei dois tipos de algoritmos de cálculo de título: um é simples arctan(-y/x)e outro é matemática cancelada por inclinação (inclinação) e cancelada por banco (rolagem), conforme mencionado abaixo. Tanto na inclinação quanto nos bancos produzem resultados errados.

Eu sou capaz de obter o rumo correto na Terra (usando simples recursos de estudo abertos disponíveis) quando ele é girado, mantendo horizontal o plano do solo usando qualquer um dos dois algos.

Tentei a calibração para erros de ferro macio e duro. Eu poderia plotá-lo em 3D e mostra uma esfera 3D perfeita. Ainda não funciona com inclinação ou declinação.

Qualquer ponteiro será útil.

O código e suas implementações são os seguintes:

void Compass_Heading()
{
  double MAG_X;
  double MAG_Y;
  double cos_roll;
  double sin_roll;
  double cos_pitch;
  double sin_pitch;

  cos_roll = cos(roll);
  sin_roll = sin(roll);
  cos_pitch = cos(pitch);
  sin_pitch = sin(pitch); 

  //// Tilt compensated Magnetic filed X:
  MAG_X = magnetom_x*cos_pitch + magnetom_y*sin_roll*sin_pitch + magnetom_z*cos_roll*sin_pitch;
  //// Tilt compensated Magnetic filed Y:
  MAG_Y = magnetom_y*cos_roll-magnetom_z*sin_roll;
  //// Magnetic Heading


  MAG_Heading = atan2(-MAG_Y, MAG_X) ;

}

Onde magnetom_x, #_ye #_zsão componentes de um vector de 3D que, na verdade, são valores RAW do magnetómetro. roll e pitch são de uma saída misteriosa do filtro Kalman do acelerômetro e giroscópio a bordo. Esses três sensores estão no ATAVRSBIN1 . O rolo e o arremesso estão ok até esta fase.

Agora, um cálculo simples de cabeçalho de acordo com journal_of_sensors_renaudin et al_2010c.pdf deveria ter sido MAG_Heading = atan2(-magnetom_y, magnetom_x) ;e com compensação como acima.

O código geral é simplesmente do OPEN AHRS .

Dados no formato Roll, Pitch e Yaw. Girei o dispositivo apenas pela minha mão. Os três primeiros foram concentrados apenas em Roll, Pitch e Yaw, respectivamente. O resto dois é primeiro girado o dispositivo em torno de 45 graus ao longo de X (enrolado) e depois girado ao longo do Z local do magnetômetro. Em seguida, o mesmo foi repetido com cerca de 45 graus de rotação ao longo de Y (inclinado) e depois girado ao longo do Z local do magnetômetro.

Os gráficos plotados dentro do intervalo de -180 a 180 graus.

Ângulos em graus em um arquivo As características YAW no Roll.

Ângulos em graus em um arquivo As características YAW no Pitch.

Ângulos em graus em um arquivo As características YAW no próprio Yaw.

Ângulos em graus em um arquivo As características YAW no Yaw com 45 graus rolados.

Ângulos em graus em um arquivo As características YAW em Yaw com 45 graus inclinados.

Nota: Para as duas últimas fotos: Primeiro mantido na posição inicial, é o mesmo para todos (consulte arquivos txt). Em seguida, rolou 45 graus e, em seguida, o dispositivo de avião (com magnetômetro) foi girado ao longo do eixo Z do magnetômetro.

Da mesma forma, para a última imagem, o dispositivo foi inclinado 45 graus e depois ao longo do eixo Z do magnetômetro.

Espero que isso ajude a resolver meu problema.

Os novos desenvolvimentos são os seguintes:

Eu trabalhei alguns no título. Eu tenho a seguinte saída. csv

csv

csv

Eu gosto dos seus gráficos. Eles mostram claramente que rolar, arremessar e guinar parecem estar funcionando. Parabéns! Isso já é mais progresso do que a maioria das pessoas faz.

Eu estou supondo que o código que você apresentou está calculando o valor MAG_Heading "errado", diferente do valor MAG_Heading que você esperava.

Seria muito mais fácil para nós ajudá-lo se você nos desse: (Esta é a seção "descreva os sintomas" de "Como fazer perguntas de maneira inteligente" )

• a saída do magnetômetro AK8975 valoriza m_x, m_y e m_z em algum momento no tempo.
• Os valores de pitch and roll no mesmo instante
• o valor de saída MAG_Heading alegadamente errado calculado a partir desses valores
• o que você esperava que o MAG_Heading correto fosse

Então, fico especulando que talvez você esteja enfrentando os mesmos tipos de problemas que criei para mim :-).

• Que formato de ângulo suas funções sin () e cos () e atan2 () esperam? Você precisa fazer algum tipo de conversão entre o pitch e o roll do formato que são armazenados nesse formato? Você precisa converter desse formato para o que você precisa no MAG_heading? (brads, graus ou radianos? ponto flutuante ou ponto fixo?)
• Existe um deslocamento nos valores brutos de m_x, m_y, m_z que precisam ser subtraídos?
• Todas as partes estão alinhadas da maneira assumida pelo código? Em particular, o eixo de inclinação e rotação está alinhado com o eixo do magnetômetro? (M_x deve apontar para a frente, ao longo do eixo do rolo? M_y deve apontar para a direita, ao longo do eixo do passo?)
• Talvez algum valor do sensor ou outro - talvez m_z - precise ser negado antes de alimentar esse código?
• Talvez esse código esteja sendo interrompido por uma interrupção ou outra que corrompe seus valores internos? Parece que me lembro de um projeto diferente que, depois que alguém colocava uma "divisão" em uma rotina de interrupção, todo cálculo de função trigonométrica em qualquer outro lugar do programa geralmente produzia o resultado errado.
• Talvez as interrupções sejam disparadas com tanta frequência que esse código nunca termine de executar?

Parece haver outras pessoas discutindo códigos muito semelhantes em outros lugares: http://diydrones.com/forum/topics/heading-from-3d-magnetometer ; http://diydrones.ning.com/profiles/blogs/dcm-imu-theory-first-draft ; http://aeroquad.com/showthread.php?1138-REVOLUTION!!!-New-IMU !!! ; http://www.rcgroups.com/forums/showthread.php?t=1436742&page=6 ; http://aeroquad.com/showthread.php?691-Hold-your-heading-with-HMC5843-Magnetometer ; etc.

A nota de aplicação do LSM303 possui um guia útil para calibrar uma bússola com compensação de inclinação aplicável ao seu problema. É bem detalhado, caso contrário, eu teria reescrito os cálculos aqui. Observe que os valores do acelerômetro são necessários para cálculos de inclinação total, rotação e guinada, pois uma rotação em torno do eixo das linhas de campo magnético resulta em nenhuma alteração nos valores do magnetômetro. O mesmo acontece com a gravidade com o acelerômetro.