Unityで相補フィルターを使ってジョイコンの姿勢推定をした話

using UnityEngine;
using UnityJoycon;

public class JoyConMotion
{
    // 相補フィルターの時定数.この値を調整して補正の強さを決める
    private const float tau = 0.9f;
    
    // 現在の姿勢
    private Quaternion _orientation = Quaternion.identity;
    public Quaternion Orientation => _orientation;

    private JoyCon _joycon;

    public JoyConMotion(JoyCon joycon)
    {
        _joycon = joycon;
    }

    /// <summary>
    /// 毎フレームIMUデータを入力して姿勢を更新
    /// </summary>
    public void Update(float deltaTime)
    {
        if (_joycon == null || !_joycon.TryGetState(out var state)) return;
        
        // 最新のIMUデータを取得
        var lastSample = state.ImuSamples[^1];
        var acc = lastSample.Acceleration;
        var gyro = lastSample.Gyroscope;

        // Joy-Conの軸をUnityの座標系に合わせて入れ替えつつ更新処理へ
        // (ライブラリや持ち方によって軸のXYZマッピングは変わるのでそこら辺はいい感じにいじってください)
        UpdateOrientation(
            new Vector3(acc.Y, acc.X, acc.Z), 
            new Vector3(-gyro.Y, -gyro.X, -gyro.Z), 
            deltaTime
        );
    }

    private void UpdateOrientation(Vector3 acc, Vector3 gyro, float deltaTime)
    {
        // ジャイロスコープによる回転（積分）
        // 前フレームの姿勢に対して、角速度 * 時間 の分だけ回転させる
        Quaternion gyroDelta = Quaternion.Euler(gyro * deltaTime);
        Quaternion gyroOrientation = _orientation * gyroDelta;

        // 加速度センサーによる重力方向の計算
        Vector3 accNorm = acc.normalized;

        // 加速度の大きさが1G（重力）から大きく外れている場合は、
        // 激しく動かしている最中なので加速度センサの値をつかわないようにする
        if (accNorm.sqrMagnitude < 0.9f || accNorm.sqrMagnitude > 1.1f)
        {
            _orientation = gyroOrientation;
            return;
        }

        // 座標系の調整
        accNorm.x = -accNorm.x;
        accNorm.z = -accNorm.z;
        
        // 加速度ベクトル（重力方向）から求まる，ジョイコンが向くべき傾き
        Quaternion accQuat = Quaternion.FromToRotation(Vector3.up, accNorm);

        // 相補フィルターによる合成
        // 時定数tauとdeltaTimeを使って係数を計算
        float alpha = tau / (tau + deltaTime);
        float t = 1f - alpha;

        // ジャイロによる回転をベースに,
        // 加速度による傾きへ少しだけ(tの割合で)近づける
        _orientation = Quaternion.Slerp(gyroOrientation, accQuat, t);
        
        // 正規化
        _orientation = Quaternion.Normalize(_orientation);
    }
    // ずれちゃったとき用のリセット
    public void ResetOrientation()
    {
        _orientation = Quaternion.identity;
    }
}

using System;
using UnityEngine;
using UnityJoycon;
using UnityJoycon.Hidapi;

public class JoyConRight : MonoBehaviour
{
    private Hidapi _hidapi;
    private JoyCon _joycon;
    private JoyConMotion _motion; // 前述の姿勢推定クラス

    private async void Awake()
    {
        // Joy-Conの接続処理
        _hidapi = new Hidapi();
        var deviceInfos = _hidapi.GetDevices(0x057e); 
        if (deviceInfos.Count == 0)
        {
            Debug.LogError("Joy-Conが見つかりません");
            return;
        }

        var deviceInfo = deviceInfos[0];
        var device = _hidapi.OpenDevice(deviceInfo);
        
        // JoyConインスタンスの作成
        _joycon = await JoyCon.CreateAsync(device);
        
        // 姿勢推定クラスの初期化
        _motion = new JoyConMotion(_joycon);
    }

    private void FixedUpdate()
    {
        if (_joycon == null || !_joycon.TryGetState(out var state)) return;

        // 姿勢のリセット（Rボタン）
        if (state.IsButtonPressed(Button.R))
        {
            _motion.ResetOrientation();
        }

        // 姿勢推定の更新計算
        // ここでジャイロと加速度を用いた計算が行われます
        _motion.Update(Time.deltaTime);

        // 計算結果をこのオブジェクトの回転に適用します
        transform.rotation = _motion.Orientation;
    }
    
    private async void OnDestroy()
    {
        if (_joycon != null)
        {
            await _joycon.DisposeAsync();
            _joycon = null;
        }

        if (_hidapi != null)
        {
            _hidapi.Dispose();
            _hidapi = null;
        }
    }
}