四元数からMadgwickフィルタまで
SO(3)とSU(2)と単位四元数
３次元回転群の扱いに(単位)四元数が使える、というのは概ね次の事実の組み合わせによります。
-
SO(3) SU(2) - ↑の準同型を
SU(2) -
SU(2) - パウリ行列(の
i
これより、
- 回転群自由度の保持と更新を、
SO(3) SU(2) - これを具体的な3次元ベクトルに作用したいときは、随伴作用で処理する。
-
SU(2) - それは(単位)四元数を保持して計算することで実現できる。
ということになります。
具体的に見ましょう。
を満たすので、とくに
であり、これは
と同じ交換関係をもちます。
となります。
となるような、
この準同型は、実際先のリー代数の準同型と整合的です。例えば
ここで
したがって、この準同型の微分写像は、それぞれのリー代数を
で写像していることがわかります。
ここまででわかったことは、まず
で対応し、
で対応し、それぞれのリー代数は、
で対応するということです。
角速度の反映
物体の回転姿勢はあきらかに
において
のように、
さて、IMUには、角速度センサと加速度センサがついています。IMUがついた物体を動かすとき、明らかにこのIMUは、物体に密着した座標系で角速度を計測しています。
ということは、もし初期状態に正確な姿勢
を解いてやれば、系の姿勢がわかることになります。
しかし、
を解けばいいことになります。もし、これを
誤差修正
この方式の問題は、誤差が蓄積するということです。単に陽的に足していっているだけなので、時間とともに誤差が蓄積して
ここで、アンカーになるベクトルを外から与えてやることで、これの修正を同時に行うことを考えます。
つまり、基準座標系になにか固定された既知のベクトルがあり、それをIMUが察知することで、現在の
具体的には、基準座標系において、重力加速度は通常一定で鉛直下向きにかかっています。等価原理からこれはIMUの加速度センサ上は逆方向の加速度として検出されます。基準座標系でのこのベクトルを
としておきます。もし現在の姿勢を表す四元数/
が成り立つはずです。ここで
として
具体的に計算すると、まず
ここで、重力/加速度の大きさを考えず、基準座標系に固定して
これに
となります。一応2乗の微分なので係数
ついで毎フレームの更新時に、単位四元数から逸脱する可能性があるので、これを規格化するようにします。
ここで
ここでは重力加速度を参照しましたが、他に地磁気センサーがあればそれも利用できます。というより、重力加速度の向きだけでは、基準座標系を固定できないので、まだ1軸の不定性があり、例えば
M5Atomで実装
四元数関連の型と状態更新を定義します。
#include "types.h"
Qt conj(const Qt &q) {
return {q.w, -q.x, -q.y, -q.z};
}
Qt add(const Qt &q1, const Qt &q2) {
return {q1.w+q2.w, q1.x+q2.x, q1.y+q2.y, q1.z + q2.z};
}
Qt prod(const Qt &q1, const Qt &q2) {
return {
q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
q1.x * q2.w + q1.w * q2.x + q1.y * q2.z - q1.z * q2.y,
q1.y * q2.w + q1.w * q2.y + q1.z * q2.x - q1.x * q2.z,
q1.z * q2.w + q1.w * q2.z + q1.x * q2.y - q1.y * q2.x
};
}
float norm(const Qt &q) {
float ll = q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z;
return sqrtf(ll);
}
float norm3(const Vec &v) {
float ll = v.x*v.x + v.y*v.y + v.z*v.z;
return sqrtf(ll);
}
Qt mult(const Qt &q, float k) {
return {q.w*k,q.x*k,q.y*k,q.z*k};
}
Vec mult3(const Vec &v, float k) {
return {v.x*k,v.y*k,v.z*k};
}
Qt gradientLoss(const Qt &q, const Vec &a) {
float lx = 2.0f * ( q.w * q.y + q.x * q.z) - a.x;
float ly = 2.0f * (-q.w * q.x + q.y * q.z) - a.y;
float lz = 1.0f - 2.0f * (q.x * q.x + q.y * q.y) - a.z;
float w = 2.0f * (q.y * lx - q.x * ly);
float x = 2.0f * (q.z * lx - q.w * ly - 2.0f * q.x * lz);
float y = 2.0f * (q.w * lx + q.z * ly - 2.0f * q.y * lz);
float z = 2.0f * (q.x * lx + q.y * ly);
return {w,x,y,z};
}
Qt update(const Qt &prev, const Vec &gyro, const Vec &accel, float beta, float dt) {
// 加速度は正規化
Vec accel_normal = accel;
float a_norm = norm3(accel);
if (a_norm > 0.0f) {
accel_normal = mult3(accel, 1.0f / a_norm);
}
Qt omega = {0, gyro.x, gyro.y, gyro.z};
Qt q_dot_gyro = mult(prod(prev, omega), 0.5f);
Qt grad = gradientLoss(prev,accel_normal);
// 勾配は正規化してbetaだけで強さを制御
float g_norm = norm(grad);
if (g_norm > 0.0f) {
grad = mult(grad, 1.0f / g_norm);
}
Qt q_dot = add(q_dot_gyro, mult(grad, -beta));
// 次の推定姿勢
Qt next = add(prev, mult(q_dot, dt));
float n = norm(next);
return mult(next, 1.0f / n);
}
ヘッダ：
#ifndef TYPES_H
#define TYPES_H
#include <Arduino.h>
struct Qt {
float w, x, y, z;
};
struct Vec {
float x, y, z;
};
Qt conj(const Qt &q);
Qt prod(const Qt &q1, const Qt &q2);
Qt mult(const Qt &q, float k);
Vec mult3(const Vec &q, float k);
Qt add(const Qt &q1, const Qt &q2);
Qt gradientLoss(const Qt &q, const Vec &a);
Qt update(const Qt &prev, const Vec &gyro, const Vec &accel, float beta, float dt);
float norm(const Qt &q);
float norm3(const Vec &q);
#endif
せっかくなので、旋回角度をマウス入力にします。
Atomで使えるBLE Mouseのライブラリがあったので使わせてもらいます。
メインルーチン側もGemini氏に教えてもらいつつポチポチ
#include <M5Unified.h>
#include "types.h"
#include <BleMouse.h>
Qt currentPose = {1.0f, 0.0f, 0.0f, 0.0f};
float beta = 0.1f;
uint32_t lastMicros = 0;
Vec gyroOffset = {0, 0, 0};
// マウス関連
BleMouse bleMouse("AtomS3 AirMouse", "M5Stack");
float mouseSensitivity = 1500.0f;
// 開始時に角速度バイアスを計測
void calibrateGyro() {
Serial.println("Calibrating...");
gyroOffset.x = 0.0f;
gyroOffset.y = 0.0f;
gyroOffset.z = 0.0f;
float gx, gy, gz;
int samples = 1000;
for (int i = 0; i < samples; i++) {
M5.Imu.getGyroData(&gx, &gy, &gz);
gyroOffset.x += gx;
gyroOffset.y += gy;
gyroOffset.z += gz;
delay(2);
}
gyroOffset.x /= samples;
gyroOffset.y /= samples;
gyroOffset.z /= samples;
Serial.println("Done!");
}
// マウス送信
void mouseDiff(const Qt prevPose, const Qt newPose) {
Qt diff = prod(conj(prevPose),newPose);
// float deltax = (int)(diff.y * mouseSensitivity);
float deltax = -(int)(diff.z * mouseSensitivity);
float deltay = -(int)(diff.x * mouseSensitivity);
bleMouse.move(deltax,deltay);
}
void setup() {
auto cfg = M5.config();
M5.begin(cfg);
Serial.begin(1500000);
if (!M5.Imu.begin()) {
while (true) { delay(1); }
}
bleMouse.begin();
lastMicros = micros();
Serial.println("Wait for calibration...");
delay(2000);
calibrateGyro();
}
void loop() {
M5.update();
// 経過時間チェック
uint32_t currentMicros = micros();
uint32_t diff = currentMicros - lastMicros;
// 10msで1フレーム
if (diff < 10000) return;
float dt = diff / 1000000.0f;
lastMicros = currentMicros;
// IMU読み出し
float ax, ay, az, gx, gy, gz;
M5.Imu.getAccelData(&ax, &ay, &az);
M5.Imu.getGyroData(&gx, &gy, &gz);
// オフセットを差し引き
gx -= gyroOffset.x;
gy -= gyroOffset.y;
gz -= gyroOffset.z;
// ラジアンへ変換
Vec gyro_rad = { gx * ((float)PI / 180.0f), gy * ((float)PI / 180.0f), gz * ((float)PI / 180.0f) };
Vec accel = { ax, ay, az };
Qt newPose = update(currentPose, gyro_rad, accel, beta, dt);
Serial.printf("W:%.3f X:%.3f Y:%.3f Z:%.3f | Hz:%.1f\r\n",
newPose.w, newPose.x, newPose.y, newPose.z, 1.0f / dt);
if (bleMouse.isConnected() && M5.BtnA.isPressed()) {
mouseDiff(currentPose,newPose);
}
currentPose = newPose;
}
W:-0.555 X:0.006 Y:-0.004 Z:-0.832 | Hz:100.0
W:-0.555 X:0.006 Y:-0.004 Z:-0.832 | Hz:100.0
W:-0.555 X:0.006 Y:-0.005 Z:-0.832 | Hz:100.0
W:-0.555 X:0.005 Y:-0.005 Z:-0.832 | Hz:100.0
W:-0.555 X:0.005 Y:-0.004 Z:-0.832 | Hz:100.0
W:-0.555 X:0.006 Y:-0.003 Z:-0.832 | Hz:100.0
W:-0.555 X:0.007 Y:-0.003 Z:-0.832 | Hz:100.0
W:-0.555 X:0.006 Y:-0.003 Z:-0.832 | Hz:100.0
W:-0.555 X:0.006 Y:-0.004 Z:-0.832 | Hz:100.0
四元数の値をそのまま持ってるので、水平から90度傾けるとXかYが
(追記)これだとまだ様子がわかりづらいのでここでもGemini氏にシリアルポート->ブラウザ経由で可視化するページを作ってもらいます
(以下ほぼGeminiポン出し(!!))
<!DOCTYPE html>
<html lang="ja">
<head>
<meta charset="UTF-8">
<title>AtomS3 Orientation Visualizer</title>
<style>
body { margin: 0; overflow: hidden; background: #1a1a1a; font-family: sans-serif; }
#ui { position: absolute; top: 20px; left: 20px; color: white; z-index: 10; background: rgba(0,0,0,0.6); padding: 15px; border-radius: 8px; border: 1px solid #444; }
button { background: #007bff; color: white; border: none; padding: 10px 20px; border-radius: 4px; cursor: pointer; font-size: 16px; transition: 0.2s; }
button:hover { background: #0056b3; }
#stats { margin-top: 10px; font-family: monospace; font-size: 12px; color: #aaa; }
</style>
</head>
<body>
<div id="ui">
<h2>AtomS3 IMU Visualizer</h2>
<button id="connect">シリアルポートに接続</button>
<div id="stats">左ドラッグ: 回転 / 右ドラッグ: 移動 / スクロール: ズーム</div>
</div>
<script type="importmap">
{
"imports": {
"three": "https://unpkg.com/three@0.160.0/build/three.module.js",
"three/addons/": "https://unpkg.com/three@0.160.0/examples/jsm/"
}
}
</script>
<script type="module">
import * as THREE from 'three';
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
// --- Three.js Setup ---
const scene = new THREE.Scene();
const camera = new THREE.PerspectiveCamera(75, window.innerWidth / window.innerHeight, 0.1, 1000);
const renderer = new THREE.WebGLRenderer({ antialias: true });
renderer.setSize(window.innerWidth, window.innerHeight);
renderer.setPixelRatio(window.devicePixelRatio);
document.body.appendChild(renderer.domElement);
// カメラ操作 (OrbitControls)
const controls = new OrbitControls(camera, renderer.domElement);
controls.enableDamping = true; // 滑らかな動き
// 照明
const light = new THREE.DirectionalLight(0xffffff, 1);
light.position.set(5, 10, 7.5);
scene.add(light);
scene.add(new THREE.AmbientLight(0x404040));
// 地面 (これがあると「下」が分かりやすい)
const gridHelper = new THREE.GridHelper(20, 20, 0x888888, 0x444444);
scene.add(gridHelper);
// ワールド座標軸 (Red:X, Green:Y, Blue:Z)
const worldAxes = new THREE.AxesHelper(5);
scene.add(worldAxes);
// AtomS3 キューブ
// Madgwickの座標系に合わせるため、少し縦長な箱にする
const geometry = new THREE.BoxGeometry(2, 0.4, 1.5);
const materials = [
new THREE.MeshPhongMaterial({color: 0xff0000}), // +X (右面)
new THREE.MeshPhongMaterial({color: 0xaa0000}), // -X
new THREE.MeshPhongMaterial({color: 0x00ff00}), // +Y (上面)
new THREE.MeshPhongMaterial({color: 0x00aa00}), // -Y
new THREE.MeshPhongMaterial({color: 0x0000ff}), // +Z (正面)
new THREE.MeshPhongMaterial({color: 0x0000aa}) // -Z
];
const cube = new THREE.Mesh(geometry, materials);
scene.add(cube);
// キューブに固定されたローカル軸
const localAxes = new THREE.AxesHelper(2);
cube.add(localAxes);
camera.position.set(5, 5, 8);
controls.update();
function animate() {
requestAnimationFrame(animate);
controls.update(); // Damping を有効にするために必要
renderer.render(scene, camera);
}
animate();
// --- Serial Port Logic ---
const connectBtn = document.getElementById('connect');
const statsDiv = document.getElementById('stats');
connectBtn.addEventListener('click', async () => {
try {
const port = await navigator.serial.requestPort();
await port.open({ baudRate: 1500000 });
connectBtn.textContent = "Connected";
const decoder = new TextDecoderStream();
port.readable.pipeTo(decoder.writable);
const reader = decoder.readable.getReader();
let buffer = "";
while (true) {
const { value, done } = await reader.read();
if (done) break;
buffer += value;
const lines = buffer.split("\n");
buffer = lines.pop();
for (const line of lines) {
const match = line.match(/W:([-.\d]+)\s+X:([-.\d]+)\s+Y:([-.\d]+)\s+Z:([-.\d]+)/);
if (match) {
const w = parseFloat(match[1]);
const x = parseFloat(match[2]);
const y = parseFloat(match[3]);
const z = parseFloat(match[4]);
// 座標軸配置に要注意
cube.quaternion.set(y, z, x, w);
statsDiv.innerHTML = `W:${w.toFixed(3)} X:${x.toFixed(3)} Y:${y.toFixed(3)} Z:${z.toFixed(3)}`;
}
}
}
} catch (err) { console.error(err); }
});
window.addEventListener('resize', () => {
camera.aspect = window.innerWidth / window.innerHeight;
camera.updateProjectionMatrix();
renderer.setSize(window.innerWidth, window.innerHeight);
});
</script>
</body>
</html>
例によってfile://だとまずそうなので適当にホスティングします。とりあえずLive Server
を使いました。
いい感じに追従しているように見えますが、時々ドリフトして勝手にゆっくり回ったりします。やはりもう一本拘束が欲しいところです。
Discussion