最小二乗法による球面フィッティングを用いた磁気センサのオフセットの計算[C言語,Arduino]

なんやかんやで初めてのArduinoネタです．もっとも，メインは最小二乗法ですからC言語の記事ですが．

需要の低そうな数式の導出過程や磁気センサーのお話は後回しにしています．

1.最小二乗法による球面フィッティング

多少のノイズが存在する中で，点(a,b,c)を中心とした半径rの球面の座標データが沢山あった時にパラメータa,b,c,rを計算したいという事があるか時々あるかと思います．

今回私も磁気センサーのオフセット（バイアス）を計算しなければならず，問題を単純化していくとこの問題に行き着きました．

今回，球面の座標データ$(m_{xi},m_{yi},m_{zi})$がN個存在するとき，各パラメータは次の連立方程式を計算することで求めることができます（導出過程は後述）．

ただし変数dは次のように定義された値です．

とりあえずこれをプログラムに落とし込んで数式が正しいか確認してみます．

	#include <stdio.h>
	#include <math.h>
	#include <time.h>
	#include <stdlib.h>
	#define N_DIM 4
	#define EPS 1e-8
	#define N_SAMPLE 256
	void sphere_fitting(float *x,float *y,float *z,float *a,float *b,float *c,float *r);
	void solve(float A[N_DIM][N_DIM],float B[N_DIM]);
	bool is_zero(float val);
	float random(float min,float max);
	int main(void){
	// テスト用のx,y,zのデータを格納する配列
	float x[N_SAMPLE];
	float y[N_SAMPLE];
	float z[N_SAMPLE];
	// 推定するパラメータ
	float a = 1.0,b = 2.0,c = 3.0,r = 4.0;[
	// 推定した結果を保存する変数
	float out_a,out_b,out_c,out_r;
	// テスト用のデータ生成に使うヘンス
	float theta,phi;
	srand(time(NULL));
	for(int i = 0 ; i < N_SAMPLE ; i++ ){
	// (a,b,c)を中心に半径rの球面の座標を乱数を加えて作成する
	theta = random(0,2.0*M_PI);
	phi = random(0,M_PI);
	x[i] = a + r * cos(theta)*cos(phi) + random(-0.2,0.2);
	y[i] = b + r * sin(theta)*cos(phi) + random(-0.2,0.2);
	z[i] = c + r * sin(phi) + random(-0.2,0.2);
	}
	// 最小二乗法で計算する
	sphere_fitting(x,y,z, &out_a, &out_b, &out_c, &out_r);
	// 結果を表示する
	printf("original\t(a,b,c,r) = (%lf,%lf,%lf,%lf)\n",a,b,c,r);
	printf("estimate\t(a,b,c,r) = (%lf,%lf,%lf,%lf)\n",out_a,out_b,out_c,out_r);
	return 0;
	}
	// 最小二乗法で球面フィッティングをする関数
	void sphere_fitting(float *x,float *y,float *z,float *a,float *b,float *c,float *r){
	float A[4][4] = {{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}};
	float B[4] = {0.0 , 0.0 ,0.0 , 0.0};
	// ※後から2を掛けた方が良いとかそういうツッコミは無しで
	for(int i = 0 ; i < N_SAMPLE ; i++ ){
	// 左辺(係数行列)
	A[0][0] += 2.0 * x[i] * x[i];
	A[0][1] += 2.0 * x[i] * y[i];
	A[0][2] += 2.0 * x[i] * z[i];
	A[0][3] += 2.0 * x[i];
	A[1][0] += 2.0 * y[i] * x[i];
	A[1][1] += 2.0 * y[i] * y[i];
	A[1][2] += 2.0 * y[i] * z[i];
	A[1][3] += 2.0 * y[i];
	A[2][0] += 2.0 * z[i] * x[i];
	A[2][1] += 2.0 * z[i] * y[i];
	A[2][2] += 2.0 * z[i] * z[i];
	A[2][3] += 2.0 * z[i];
	A[3][0] += 2.0 * x[i];
	A[3][1] += 2.0 * y[i];
	A[3][2] += 2.0 * z[i];
	A[3][3] += 2.0 * 1.0;
	// 右辺
	B[0] += x[i]*(x[i]*x[i]+y[i]*y[i]+z[i]*z[i]);
	B[1] += y[i]*(x[i]*x[i]+y[i]*y[i]+z[i]*z[i]);
	B[2] += z[i]*(x[i]*x[i]+y[i]*y[i]+z[i]*z[i]);
	B[3] += x[i]*x[i]+y[i]*y[i]+z[i]*z[i];
	}
	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]);
	}
	// 連立方程式を求める関数
	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;
	}
	// [low,high)の乱数を発生させる関数
	float random(float low,float high){
	float r = (float)(rand()) / RAND_MAX;
	return low + high * r;
	}

view raw sphere_fitting_test.c hosted with ❤ by GitHub

計算結果はこのようになりました．

データにノイズ（乱数）を加えているのでぴったり一致することはありませんが，十分な結果が得られていると思います．

2.磁気センサーのオフセットの計算に適用する

さて，（僕にとっては）本題です．磁気センサーはその場所の磁束密度を計測するセンサーなのですが，生データにはオフセット（バイアス）が存在しています．

実際に磁気センサーを動かしながら取得したデータをプロットしてみたものがこちらです．

正直センサーが悪かったとしか言いようが無い……でも一部全くプロットされていないところがあるのは何でだ pic.twitter.com/OvUckezTVp

— もりとにー (@TonyMooori) 2016年5月27日

概ねある点（オフセット）を中心とした球面上のデータになっていると信じます．というか，そういうモデルとします．

というのも，磁気センサーは磁束密度，つまり磁場のベクトルを計測しているわけで，周りに変なものがなければ定常な磁場のベクトルである地磁気が測定されるはずだからです．

しかし，僕はドローンの姿勢検出に磁気センサーを使うのですが，回転角度を求めるときにオフセット（バイアス）があると非常に困るのです．

平たく言えば球の中心を原点に持ってきたい，というわけです．

実際に先ほどのプログラムをArduino用に改良してバイアスを計測してみます．Arduinoでは大きな配列を扱うことはできません．

しかしこの連立方程式を作るのに大きな配列を用意する必要はないのでご安心ください．

使用する磁気センサーはなんでも良いのですが，手元にあるのがmpu9150なので，センサー開発元のsparkfunが公開しているライブラリを使います．

・MPU-9150_Breakout: Example code and PCB design files for the MPU-9105, 9DOF.

	#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;
	}

view raw mag_offset.ino hosted with ❤ by GitHub

実行結果としては僕の環境ではうまくいきました（a,b,c,rの値が常にほぼ一定）．この記事を読んでくださっている方々はわかると思いますが，このプログラムが直接役に立つということはそんなに無いですが，参考程度にはなったかと思います．

・数式の導出

個人的にはあまり興味はないのですが（とは言うものの面白いのですが），球の最小二乗法の式の導出を示しておきます．まず，球面の座標データ$(m_{xi},m_{yi},m_{zi})$がN個存在するとすると，全ての点で以下の数式が成り立ちます．
$$(m_{xi}-a)^2+(m_{yi}-b)^2+(m_{zi}-c)^2=r^2$$ ただし点(a,b,c)は球の中心座標，rは半径です．しかし実際にはノイズが入っている上，データ数Nは非常に多いのでこのようにはなりません．なのでこの問題を次のように置き換えます．とりあえず関数を変形します．
$$(m_{xi}-a)^2+(m_{yi}-b)^2+(m_{zi}-c)^2-r^2=0$$ ここで次の関数を最小化する最小化問題に置き換えます．
$$J = \frac{1}{4}\sum_{i=1}^{N}{\{(m_{xi}-a)^2+(m_{yi}-b)^2+(m_{zi}-c)^2-r^2\}}^2$$ 1/4は気にしないでください．もう少し変形します．
$$J= \frac{1}{4}\sum_{i=1}^{N}{( m_{xi}^2 - 2am_{xi}+m_{yi}^2 - 2bm_{yi}+m_{zi}^2 - 2cm_{zi}-2d )}^2$$ ただし$a^2+b^2+c^2-r^2=-2d$です．係数は後々綺麗になるように調整しているものなので関係ないです．ここでこの関数を最小化するa,b,c,d周りでは偏微分が0になっているはずです．まぁ厳密にどうとかは私は知りませんが．なのでとりあえず偏微分します．
$$\frac{\partial J}{\partial a} = \sum_{i=1}^{N}{( m_{xi}^2 - 2am_{xi}+m_{yi}^2 - 2bm_{yi}+m_{zi}^2 - 2cm_{zi}-2d )(-m_{xi})} = 0 \\ \frac{\partial J}{\partial b} = \sum_{i=1}^{N}{( m_{xi}^2 - 2am_{xi}+m_{yi}^2 - 2bm_{yi}+m_{zi}^2 - 2cm_{zi}-2d )(-m_{yi})} = 0 \\ \frac{\partial J}{\partial c} = \sum_{i=1}^{N}{( m_{xi}^2 - 2am_{xi}+m_{yi}^2 - 2bm_{yi}+m_{zi}^2 - 2cm_{zi}-2d )(-m_{zi})} = 0 \\ \frac{\partial J}{\partial d} = \sum_{i=1}^{N}{( m_{xi}^2 - 2am_{xi}+m_{yi}^2 - 2bm_{yi}+m_{zi}^2 - 2cm_{zi}-2d )(-1)} = 0$$ 更に変形して行きます． $$\sum_{i=1}^{N}{( -m_{xi}^3 + 2am_{xi}^2-m_{xi}m_{yi}^2 + 2bm_{xi}m_{yi}-m_{xi}m_{zi}^2 + 2cm_{xi}m_{zi}+2m_{xi}d)} = 0 \\ \sum_{i=1}^{N}{( -m_{xi}^2m_{yi} + 2am_{xi}m_{yi}-m_{yi}^3 + 2bm_{yi}^2-m_{yi}m_{zi}^2 + 2cm_{yi}m_{zi}+2m_{yi}d)} = 0 \\ \sum_{i=1}^{N}{( -m_{xi}^2m_{zi} + 2am_{xi}m_{zi}-m_{yi}^2m_{zi} + 2bm_{yi}m_{zi}-m_{zi}^3 + 2cm_{zi}^2+2m_{zi}d)} = 0 \\ \sum_{i=1}^{N}{( -m_{xi}^2 + 2am_{xi}-m_{yi}^2 + 2bm_{yi}-m_{zi}^2 + 2cm_{zi}+2d )} = 0$$ 最後に，普通の連立1次方程式に直します． $$\left( \begin{array}{ccc} \sum{2m_{xi}^2} & \sum{2m_{xi}m_{yi}} & \sum{2m_{xi}m_{zi}} & \sum{2m_{xi}} \\ \sum{2m_{xi}m_{yi}} & \sum{2m_{yi}^2} & \sum{2m_{yi}m_{zi}} & \sum{2m_{yi}} \\ \sum{2m_{xi}m_{zi}} & \sum{2m_{yi}m_{zi}} & \sum{2m_{zi}^2} & \sum{2m_{zi}} \\ \sum{2m_{xi}} & \sum{2m_{yi}} & \sum{2m_{zi}} & {2N} \\ \end{array} \right) \left( \begin{array}{ccc} a \\ b \\ c \\ d \\ \end{array} \right) = \left( \begin{array}{ccc} \sum{m_{xi}(m_{xi}^2 + m_{yi}^2 + m_{zi}^2 )} \\ \sum{m_{yi}(m_{xi}^2 + m_{yi}^2 + m_{zi}^2 )} \\ \sum{m_{zi}(m_{xi}^2 + m_{yi}^2 + m_{zi}^2 )} \\ \sum{(m_{xi}^2 + m_{yi}^2 + m_{zi}^2 )}\\ \end{array} \right)$$ こんなところでしょうか．結局係数が消せてないのですが……

このような話を理解するには『これなら分かる最適化数学 ―基礎原理から計算手法まで― 』という本がおすすめです．

全部ではないですが，最近読みました(後半部分等，少々難しい内容がありましたので)．

ただ内容としては非常に面白かったです．

今日は以上です．