角速度センサーで取得した値をUnityで使う方法を教えてください

角速度センサーで取得した値を使ってUnity上で姿勢推定をしようとしています。
角速度をクォータニオンに変換するための式(dq/dt = 1/2 * w*q)を使ってクォータニオンを計算したのですが、この値をどう操作すればUnity上で姿勢を再現できるのかがいまいち分かりません。

該当のソースコード

C#
1using System.Collections;
2using System.Collections.Generic;
3using System.IO;
4using System.Linq;
5using UnityEngine;
6
7public class list_csv : MonoBehaviour
8{
9
10    TextAsset csvFile;
11    List<string[]> csvDatas = new List<string[]>();
12
13    public static int max_gyr;
14    public static int max_gyr_temp = 60000;
15
16    public static int gyr_N = 4;
17    public static int gyr_M = 4;
18    public static float[,] gyr_matrix = new float[gyr_N, gyr_M];
19    public static float dt = 0.1f;
20
21    public static float[][] q_before = new float[max_gyr_temp][];
22    public static float[][] q_after = new float[max_gyr_temp][];
23
24    public Quaternion from_qua;
25    public Quaternion to_qua;
26    public Quaternion standard;
27
28    private static float t;
29    private static int q_counter = 1;
30
31    void Start()
32    {
33        q_before[0] = new float[] { 1, 0, 0, 0 };
34        // about gyr
35        csvFile = Resources.Load("mpu9250wpi_logs_20221225-124345") as TextAsset;
36        StringReader reader = new StringReader(csvFile.text);
37        while (reader.Peek() != -1)
38        {
39            string line = reader.ReadLine();
40            csvDatas.Add(line.Split(','));
41        }
42        max_gyr = csvDatas.Count;
43
44        float[] x_value = new float[max_gyr];
45        float[] y_value = new float[max_gyr];
46        float[] z_value = new float[max_gyr];
47        float[][] w = new float[max_gyr_temp][];
48
49        for (int i = 0; i < max_gyr - 1; i++)
50        {
51            x_value[i] = float.Parse(csvDatas[i + 1][7]);
52            y_value[i] = float.Parse(csvDatas[i + 1][8]);
53            z_value[i] = float.Parse(csvDatas[i + 1][9]);
54            w[i] = new float[] { x_value[i], y_value[i], z_value[i] };
55        }
56
57        q_after[0] = find_q(w_times_q(w[0], q_before[0]), dt);
58        q_before[0] = q_after[0];
59
60        for (int i = 1; i < max_gyr - 1; i++)
61        {
62            q_before[i] = q_after[i - 1];
63            q_after[i] = find_q(w_times_q(w[i], q_before[i]), dt);
64        }
65
66    }
67
68    // Update is called once per frame
69    void FixedUpdate()
70    {
71        GyroValue();
72    }
73
74    static float[] w_times_q(float[] w, float[] q)
75    {
76        float[] product = new float[4];
77        product[0] = 0 * q[0] + w[2] * q[1] + -w[1] * q[2] + w[0] * q[3];
78        product[1] = -w[2] * q[0] + 0 * q[1] + w[0] * q[2] + w[1] * q[3];
79        product[2] = w[1] * q[0] + -w[0] * q[1] + 0 * q[2] + w[2] * q[3];
80        product[3] = -w[0] * q[0] + -w[1] * q[1] + -w[2] * q[2] + 0 * q[3];
81
82        return product;
83    }
84
85    static float[] find_q(float[] wq, float dt)
86    {
87        float[] result = new float[4];
88        result[0] = 0.5f * wq[0] * dt;
89        result[1] = 0.5f * wq[1] * dt;
90        result[2] = 0.5f * wq[2] * dt;
91        result[3] = 0.5f * wq[3] * dt;
92
93        return result;
94    }
95
96    private void GyroValue()
97    {
98        t += Time.deltaTime;
99        if(t > 0.1f && q_counter < max_gyr - 1)
100        {
101            t = 0;
102            from_qua = to_qua;
103            to_qua.w = q_after[q_counter][0];
104            to_qua.x = q_after[q_counter][1];
105            to_qua.y = q_after[q_counter][2];
106            to_qua.z = q_after[q_counter][3];
107            q_counter++;
108            standard = this.transform.rotation;
109            Debug.Log(q_counter);
110        }
111        if(q_counter == max_gyr - 1)
112        {
113            Debug.Log("EOF");
114        }
115
116        this.transform.rotation = Quaternion.Slerp(from_qua, to_qua, t/0.1f);
117        //this.transform.rotation = to_qua * from_qua;
118        //q_before = q_after;
119    }
120}
121

試したこと

Quaternion.Slerp(from_qua, to_qua, t/0.1f);の第一引数と第二引数を(standard, to_qua * standard)にしたり(standard, to_qua)にしたりしました。

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

使用したセンサーはMPU9250です。

行動規範の内容に同意します

回答1件

自己解決

Quaternion.Eulerを使ったら自己解決しました

C#
1using System.Collections;
2using System.Collections.Generic;
3using System.IO;
4using System.Linq;
5using UnityEngine;
6
7public class list_csv_v3 : MonoBehaviour
8{
9
10    TextAsset csvFile;
11    List<string[]> csvDatas = new List<string[]>();
12
13    public static int max_gyr;
14    public static float dt = 0.1f;
15
16    public Quaternion to_qua;
17    public Quaternion standard;
18
19    private static float t;
20    private static int q_counter = 1;
21    private float x_value;
22    private float y_value;
23    private float z_value;
24    Vector3 angle;
25
26    void Start()
27    {
28        // about gyr
29        csvFile = Resources.Load("mpu9250wpi_logs_20221225-124345") as TextAsset;
30        StringReader reader = new StringReader(csvFile.text);
31        while (reader.Peek() != -1)
32        {
33            string line = reader.ReadLine();
34            csvDatas.Add(line.Split(','));
35        }
36        max_gyr = csvDatas.Count;
37    }
38
39    // Update is called once per frame
40    void FixedUpdate()
41    {
42        GyroValue();
43    }
44
45    private void GyroValue()
46    {
47        t += Time.deltaTime;
48        if (t > 0.1f && q_counter < max_gyr - 1)
49        {
50            t = 0;
51            x_value = float.Parse(csvDatas[q_counter + 1][7]);
52            y_value = float.Parse(csvDatas[q_counter + 1][8]);
53            z_value = float.Parse(csvDatas[q_counter + 1][9]);
54            angle = new Vector3(y_value, z_value, -1.0f * x_value);
55            to_qua = this.transform.rotation * Quaternion.Euler(angle * dt);
56            standard = this.transform.rotation;
57            
58            q_counter++;
59            Debug.Log(q_counter);
60        }
61        if (q_counter == max_gyr - 1)
62        {
63            Debug.Log("EOF");
64        }
65
66        this.transform.rotation = Quaternion.Slerp(standard, to_qua, t * 10);
67    }
68}
69