最小二乗法による球面フィッティングによる磁気センサのオフセットの計算

mag_offset.ino

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU9150.h"

#define N_SAMPLE 256
#define WAIT_TIME 50
#define N_DIM 4
#define EPS 1e-8

// ライブラリ内で定義されたセンサーの値を取得するクラスのインスタンス
MPU9150 sensor;

// 磁気センサの値を保存する変数
float mx, my, mz;

// 連立方程式
float A[4][4];
float B[4];

void setup() {
  Serial.begin(9600);

  // センサーの初期化
  Wire.begin();
  Serial.println("Initializing I2C devices...");
  sensor.initialize();
  Serial.println("Testing device connections...");
  Serial.println(sensor.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
}

void loop() {
  // 動かさないと値が取れないので動かして！！！って表示する
  Serial.println("calculating mag offset...");
  Serial.println("move sensor...");
  // ちょっと待ってあげる
  delay(500);
  
  init_data();    // 初期化して
  calc_offset();  // オフセットを計算・表示する

  delay(2000);
}

void calc_offset() {
  float a, b, c, r;
  for (int i = 0 ; i < N_SAMPLE ; i++ ) {
    // センサーの値を読み取る
    get_mag();

    // 左辺(係数行列)
    A[0][0] += 2.0 * mx * mx;
    A[0][1] += 2.0 * mx * my;
    A[0][2] += 2.0 * mx * mz;
    A[0][3] += 2.0 * mx;

    A[1][0] += 2.0 * my * mx;
    A[1][1] += 2.0 * my * my;
    A[1][2] += 2.0 * my * mz;
    A[1][3] += 2.0 * my;

    A[2][0] += 2.0 * mz * mx;
    A[2][1] += 2.0 * mz * my;
    A[2][2] += 2.0 * mz * mz;
    A[2][3] += 2.0 * mz;

    A[3][0] += 2.0 * mx;
    A[3][1] += 2.0 * my;
    A[3][2] += 2.0 * mz;
    A[3][3] += 2.0 * 1.0;

    // 右辺
    B[0] += mx * (mx * mx + my * my + mz * mz);
    B[1] += my * (mx * mx + my * my + mz * mz);
    B[2] += mz * (mx * mx + my * my + mz * mz);
    B[3] += mx * mx + my * my + mz * mz;

    Serial.print(mx);Serial.print("\t");
    Serial.print(my);Serial.print("\t");
    Serial.print(mz);Serial.print("\n");

    delay(WAIT_TIME);
  }
  solve(A, B);  // 連立方程式を計算

  // a,b,c,dのパラメータがB[0],B[1],B[2],B[3]に代入されているので
  // a,b,c,rを代入して関数を終わる
  a = B[0];
  b = B[1];
  c = B[2];
  r = sqrt(B[0] * B[0] + B[1] * B[1] + B[2] * B[2] + 2 * B[3]);

  Serial.print("(x,y,z,r) = ");
  Serial.print("(");
  Serial.print(a); Serial.print("\t");
  Serial.print(b); Serial.print("\t");
  Serial.print(c); Serial.print("\t");
  Serial.print(r); Serial.print("\t");
  Serial.println(")");
}

void init_data() {
  // A,Bを初期化する関数
  A[0][0] = 0.0; A[0][1] = 0.0; A[0][2] = 0.0; A[0][3] = 0.0;
  A[1][0] = 0.0; A[1][1] = 0.0; A[1][2] = 0.0; A[1][3] = 0.0;
  A[2][0] = 0.0; A[2][1] = 0.0; A[2][2] = 0.0; A[2][3] = 0.0;
  A[3][0] = 0.0; A[3][1] = 0.0; A[3][2] = 0.0; A[3][3] = 0.0;
  B[0] = B[1] = B[2] = B[3] = 0.0;
}

void get_mag() {
  // センサーの値を一時的に格納する変数
  int16_t raw_ax, raw_ay, raw_az;
  int16_t raw_gx, raw_gy, raw_gz;
  int16_t raw_mx, raw_my, raw_mz;

  // センサーの値を読み取る
  sensor.getMotion9(
    &raw_ax, &raw_ay, &raw_az,
    &raw_gx, &raw_gy, &raw_gz,
    &raw_mx, &raw_my, &raw_mz);

  // スケールを調整
  mx = ((float)raw_mx) / 4096.0;
  my = ((float)raw_my) / 4096.0;
  mz = ((float)raw_mz) / 4096.0;
}

// 連立方程式を求める関数
void solve(float A[N_DIM][N_DIM], float B[N_DIM]) {
  float temp;

  for (int i = 0 ; i < N_DIM ; i++ ) {
    // ピボット選択的みたいな処理
    for (int j = i + 1 ; j < N_DIM ; j++ ) {
      if ( is_zero(A[i][j]) == false )
        break;

      for (int k = 0 ; k < N_DIM ; k++ )
        A[i][k] += A[j][k];
      B[i] += B[j];
    }

    // 対角成分を1に
    temp = A[i][i];
    for (int j = i ; j < N_DIM ; j++ )
      A[i][j] /= temp;
    B[i] /= temp;

    // 前進消去
    for (int j = i + 1 ; j < N_DIM ; j++ ) {
      temp = A[j][i];

      for (int k = i ; k < N_DIM ; k++ )
        A[j][k] -= temp * A[i][k];
      B[j] -= temp * B[i];
    }
  }

  // 後進消去
  for (int i = N_DIM - 1 ; i >= 0 ; i-- )
    for (int j = i - 1 ; j >= 0 ; j-- )
      B[j] -= A[j][i] * B[i];
}

// ほぼ0ならtrueを返す関数
bool is_zero(float val){
  return -EPS < val && val < EPS;
}