Arduinoでquaternionを使って３次元角度を計算する

arduinoでquaternionを計算する。

quaternionのライブラリのgithubを作ったので、arduino IDEで使ってください。
quaternionの勉強不足でエラーハンドルが足りないと思います。githubのissueに投げてもらえると、頑張ります。　

コード

#include <Quaternion.h>

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

  //初期化したらx,y,z,wは全て0後からx,y,z,wを代入しても良い
  Quaternion a;
  a.x=1.0f;
  a.y=0.0f;
  a.z=1.0f;
  a.w=1.0f;


  //quaternionの初期化ではx,y,z,wの順番で入れても良い
  float x = 1.0f;
  float y = 0.0f;
  float z = 1.0f;
  float w = 0.0f;
  Quaternion b(x, y, z, w);

  //inverseを求める
  Quaternion inv = a.inverse();

  //掛け算を求める。
  Quaternion product = a.multiple(b);

  //大きさを求める
  float norm = a.norm();

  //角度差を求める
  Quaternion diff = a.Angular_difference(b);

  Serial.print("a[x,y,z,w] = [");
  Serial.print(a.toString());
  Serial.println("]");

  Serial.print("b[x,y,z,w] = [");
  Serial.print(b.toString());
  Serial.println("]");

  Serial.print("inv[x,y,z,w] = [");
  Serial.print(inv.toString());
  Serial.println("]");

  Serial.print("product[x,y,z,w] = [");
  Serial.print(product.toString());
  Serial.println("]");

  Serial.print("norm = ");
  Serial.print(norm);
  Serial.println("");

  Serial.print("diff[x,y,z,w] = [");
  Serial.print(diff.toString());
  Serial.println("]");

  delay(1000);

}

void loop() {
}

参考資料

1. quaternionの角度差について
2. quaternion計算ノート

を参考にしました