madgwick filter の実装したい

Question

前提・実現したいこと

スマートフォンから取得した加速度センサーとジャイロセンサーの値(別々のcsvファイル)を使って，madgwick filter を実装して出てきた値をcsvファイルとして保存し，スマートフォンの姿勢推定をしたいと思っています．
またmainでそれぞれのcsvファイルから読みだして用意した配列に格納しファイルを閉じ，filterUpdate関数を通して出てきた値をcsvファイルに書き込んで，推定された姿勢をpythonのほうでアニメ―ションにしたいと思っています．

質問

・用意した配列(例えばa[2][1000]，ｗ[2][1000])をどのようにしてfilterUpdate関数に通せばうまくいくのか
・filterUpdate関数から出てくる値はどのような値なのか(例えば関数のどこの値が出てくるのか)．
　出てくる値の数はいくつなのか．
・関数から出てきた値をどのようにcsvファイルに書き込めばよいのか

該当のソースコード

#include <stdio.h>
#pragma warning(disable : 4996)
// Math library required for ‘sqrt’
#include <math.h>
// System constants
#define deltat 0.001f // sampling period in seconds (shown as 1 ms)
#define gyroMeasError 3.14159265358979f * (5.0f / 180.0f) // gyroscope measurement error in rad/s (shown as 5 deg/s)
#define beta sqrt(3.0f / 4.0f) * gyroMeasError // compute beta
// Global system variables
float a_x, a_y, a_z; // accelerometer measurements
float w_x, w_y, w_z; // gyroscope measurements in rad/s
float SEq_1 = 1.0f, SEq_2 = 0.0f, SEq_3 = 0.0f, SEq_4 = 0.0f; // estimated orientation quaternion elements with initial conditions
void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y, float a_z)
{
// Local system variables
float norm; // vector norm
float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion derrivative from gyroscopes elements
float f_1, f_2, f_3; // objective function elements
float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements
float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error
// Axulirary variables to avoid reapeated calcualtions
float halfSEq_1 = 0.5f * SEq_1;
float halfSEq_2 = 0.5f * SEq_2;
float halfSEq_3 = 0.5f * SEq_3;
float halfSEq_4 = 0.5f * SEq_4;
float twoSEq_1 = 2.0f * SEq_1;
float twoSEq_2 = 2.0f * SEq_2;
float twoSEq_3 = 2.0f * SEq_3;
// Normalise the accelerometer measurement
norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
a_x /= norm;
a_y /= norm;
a_z /= norm;
// Compute the objective function and Jacobian
f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication
J_12or23 = 2.0f * SEq_4;
J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication
J_14or21 = twoSEq_2;
J_32 = 2.0f * J_14or21; // negated in matrix multiplication
J_33 = 2.0f * J_11or24; // negated in matrix multiplication
// Compute the gradient (matrix multiplication)
SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1;
SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2;
// Normalise the gradient
norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
SEqHatDot_1 /= norm;
SEqHatDot_2 /= norm;
SEqHatDot_3 /= norm;
SEqHatDot_4 /= norm;
// Compute the quaternion derrivative measured by gyroscopes
SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
// Compute then integrate the estimated quaternion derrivative
SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat;
SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat;
SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat;
SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;
// Normalise quaternion
norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
SEq_1 /= norm;
SEq_2 /= norm;
SEq_3 /= norm;
SEq_4 /= norm;
}

int main() {

	float a[2][1000],w[2][1000];


	FILE *fp1,*fp2;

	fp1 = fopen("加速度.csv", "r");
	if (fp1 == NULL) {
		printf("ファイルを開くことが出来ませんでした．¥n");
		return;
	}
	printf("\n");
	fscanf(fp1, "%lf,%lf,%lf", a[0], a[1], a[2]);
	fclose(fp1);


	fp2 = fopen("角速度.csv", "r");
	if (fp2 == NULL) {
		printf("ファイルを開くことが出来ませんでした．¥n");
		return;
	}
	printf("\n");
	fscanf(fp2, "%lf,%lf,%lf", w[0], w[1], w[2]);
	fclose(fp2);
	

	////////////////////////////////
	/*ここからの実装がわかりません*/
	////////////////////////////////



	return 0;
	
	}

補足情報（FW/ツールのバージョンなど）

visual studio2017を使っています
またmain()のファイル操作が間違っていたらご指摘お願いします．
用意した加速度のcsvファイルは下の画像の様になっています．
角速度のほうも同じように用意してます

Comments

[jbpb0]

Madgwickフィルター
とかでググると解説してるWebページがたくさん見つかるので、そういうのを読みながらコードを分析したら、関数の使い方は分かるのではないですかね？？

[rck79851871]

申し訳ないですけどいろいろ検索した上でわからなかったので質問しています・・・

[episteme]

> filterUpdate関数から出てくる値はどのような値なのか(例えば関数のどこの値が出てくるのか)．
> 出てくる値の数はいくつなのか．

こんな質問するってことは、"どっかから拾ってきたコードをやみくもに貼り付けた"ってことか?

[jbpb0]

> filterUpdate関数から出てくる値はどのような値なのか(例えば関数のどこの値が出てくるのか)．
> 出てくる値の数はいくつなのか．

これに回答するには、関数中のどの変数に質問者さんが欲しい値が入っているのかが分からないといけないので、理論が書かれてる
https://forums.parallax.com/uploads/attachments/41167/106661.pdf
や、それの解説Webページとかを読みながら、関数中の処理を全部解読しなければいけません
それを他人に求めるのですか?

質問者さんがコードを分析して、
「変数〇〇に欲しい値が入っていて、それをcsvファイルに保存したいのだけど、そのやり方が分からない」
みたいに質問すれば、それはプログラミングの質問だから、回答される可能性は高いと思いますけど

たまたまこのアルゴリズムを知ってる人がたまたま見たら回答してくれるかもしれないけど、その可能性は低いと思います

[jbpb0]

> 関数から出てきた値をどのようにcsvファイルに書き込めばよいのか

csvファイルの読み書きは、filterUpdate関数の中身とは無関係なので、それだけの別の質問にした方が、回答が得られやすいと思います
その場合は、この質問のような題名ではなく、csvファイルの読み書きのやり方について知りたい、と分かるような題名にして